初读代码

代码熟悉

main函数

一篇关于chrono时间的文章解释，https://www.cnblogs.com/jwk000/p/3560086.html

pidcontroller 的setpoint函数，接口为void setPoint(float target, float kProportional, float kIntegral, float kDerivative)

一个很重要的二维数组：localposition
std::cout << " LocalPositionx: " << LocalPosition.at(nextnumber - 1)[0] << " LocalPositiony: " << LocalPosition.at(nextnumber - 1)[1] << std::endl;
std::cout << " LocalPosition3: " << LocalPosition.at(nextnumber - 1)[2] << std::endl;
可以看出第一维应该是任务数相关，第二维是一个xyz（猜测）。

原来的main函数中，controlmode为1时，也就是376行为下一目标为停机坪时候的代码。
所用到的得到图片为image = img2mat.get_below_mat();
这是一个ImageToMat的.h文件，为自己所写。

cv::Mat get_below_mat() {
		std::vector<ImageRequest> request = { ImageRequest(3, ImageType::Scene) };
		const std::vector<ImageResponse>& response = client.simGetImages(request);

		img = cv::Mat(response.at(0).height, response.at(0).width, CV_8SC3);
		img = cv::imdecode(response.at(0).image_data_uint8, 1);

		return img;
	}
//其中，最底层的调用官方API接口为一下：
vector<ImageCaptureBase::ImageResponse> RpcLibClientBase::simGetImages(vector<ImageCaptureBase::ImageRequest> request)

然后链接一下官放hellodrone给的代码示例，里面有用到获取图片。

int main() 
{
    using namespace msr::airlib;

    msr::airlib::MultirotorRpcLibClient client;
    typedef ImageCaptureBase::ImageRequest ImageRequest;
    typedef ImageCaptureBase::ImageResponse ImageResponse;
    typedef ImageCaptureBase::ImageType ImageType;
    typedef common_utils::FileSystem FileSystem;
    
    try {
        client.confirmConnection();

        std::cout << "Press Enter to get FPV image" << std::endl; std::cin.get();
        vector<ImageRequest> request = { ImageRequest("0", ImageType::Scene), ImageRequest("1", ImageType::DepthPlanner, true) };
        const vector<ImageResponse>& response = client.simGetImages(request);
        std::cout << "# of images received: " << response.size() << std::endl;

        if (response.size() > 0) {
            std::cout << "Enter path with ending separator to save images (leave empty for no save)" << std::endl; 
            std::string path;
            std::getline(std::cin, path);

            for (const ImageResponse& image_info : response) {
                std::cout << "Image uint8 size: " << image_info.image_data_uint8.size() << std::endl;
                std::cout << "Image float size: " << image_info.image_data_float.size() << std::endl;

                if (path != "") {
                    std::string file_path = FileSystem::combine(path, std::to_string(image_info.time_stamp));
                    if (image_info.pixels_as_float) {
                        Utils::writePfmFile(image_info.image_data_float.data(), image_info.width, image_info.height,
                            file_path + ".pfm");
                    }
                    else {
                        std::ofstream file(file_path + ".png", std::ios::binary);
                        file.write(reinterpret_cast<const char*>(image_info.image_data_uint8.data()), image_info.image_data_uint8.size());
                        file.close();
                    }
                }
            }
        }

        std::cout << "Press Enter to arm the drone" << std::endl; std::cin.get();
        client.enableApiControl(true);
        client.armDisarm(true);

        auto barometer_data = client.getBarometerData();
        std::cout << "Barometer data \n" 
            << "barometer_data.time_stamp \t" << barometer_data.time_stamp << std::endl
            << "barometer_data.altitude \t" << barometer_data.altitude << std::endl
            << "barometer_data.pressure \t" << barometer_data.pressure << std::endl
            << "barometer_data.qnh \t" << barometer_data.qnh << std::endl;

        auto imu_data = client.getImuData();
        std::cout << "IMU data \n" 
            << "imu_data.time_stamp \t" << imu_data.time_stamp << std::endl 
            << "imu_data.orientation \t" << imu_data.orientation << std::endl 
            << "imu_data.angular_velocity \t" << imu_data.angular_velocity << std::endl 
            << "imu_data.linear_acceleration \t" << imu_data.linear_acceleration << std::endl;

        auto gps_data = client.getGpsData();
        std::cout << "GPS data \n" 
            << "gps_data.time_stamp \t" << gps_data.time_stamp << std::endl 
            << "gps_data.gnss.time_utc \t" << gps_data.gnss.time_utc << std::endl
            << "gps_data.gnss.geo_point \t" << gps_data.gnss.geo_point << std::endl
            << "gps_data.gnss.eph \t" << gps_data.gnss.eph << std::endl
            << "gps_data.gnss.epv \t" << gps_data.gnss.epv << std::endl
            << "gps_data.gnss.velocity \t" << gps_data.gnss.velocity << std::endl
            << "gps_data.gnss.fix_type \t" << gps_data.gnss.fix_type << std::endl;

        auto magnetometer_data = client.getMagnetometerData();
        std::cout << "Magnetometer data \n" 
            << "magnetometer_data.time_stamp \t" << magnetometer_data.time_stamp << std::endl 
            << "magnetometer_data.magnetic_field_body \t" << magnetometer_data.magnetic_field_body << std::endl; 
            // << "magnetometer_data.magnetic_field_covariance" << magnetometer_data.magnetic_field_covariance // not implemented in sensor

        std::cout << "Press Enter to takeoff" << std::endl; std::cin.get();
        float takeoffTimeout = 5; 
        client.takeoffAsync(takeoffTimeout)->waitOnLastTask();

        // switch to explicit hover mode so that this is the fall back when 
        // move* commands are finished.
        std::this_thread::sleep_for(std::chrono::duration<double>(5));
        client.hoverAsync()->waitOnLastTask();

        std::cout << "Press Enter to fly in a 10m box pattern at 3 m/s velocity" << std::endl; std::cin.get();
        // moveByVelocityZ is an offboard operation, so we need to set offboard mode.
        client.enableApiControl(true); 
        auto position = client.getMultirotorState().getPosition();
        float z = position.z(); // current position (NED coordinate system).  
        const float speed = 3.0f;
        const float size = 10.0f; 
        const float duration = size / speed;
        DrivetrainType driveTrain = DrivetrainType::ForwardOnly;
        YawMode yaw_mode(true, 0);
        std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));
        std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(0, speed, z, duration, driveTrain, yaw_mode);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));
        std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(-speed, 0, z, duration, driveTrain, yaw_mode);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));
        std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(0, -speed, z, duration, driveTrain, yaw_mode);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));

        client.hoverAsync()->waitOnLastTask();

        std::cout << "Press Enter to land" << std::endl; std::cin.get();
        client.landAsync()->waitOnLastTask();

        std::cout << "Press Enter to disarm" << std::endl; std::cin.get();
        client.armDisarm(false);

    }
    catch (rpc::rpc_error&  e) {
        std::string msg = e.get_error().as<std::string>();
        std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl;
    }

    return 0;
}

kalman filter

位于kalman filter中的结构体。
typedef struct
{
float filtered;
float est_error;
void start() { filtered = 0; est_error = 0; }
}prev_states;

一个client的接口：

bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration)
{
    std::shared_ptr<OffboardCommand> cmd = std::make_shared<MoveByAngleThrottle>(controller_, pitch, roll, throttle, yaw_rate, duration);
    return enqueueCommand(cmd);
}

class DroneControllerBase : public VehicleControllerBase
这个类是控制飞行的类别。
有如下public方法：
public: //interface for outside world

virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) = 0;

virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action);

virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action);
virtual bool goHome(CancelableBase& cancelable_action);

virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration
    , CancelableBase& cancelable_action);

virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration
    , CancelableBase& cancelable_action);
virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode,
    CancelableBase& cancelable_action);

virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode,
    CancelableBase& cancelable_action);
virtual bool moveOnPath(const vector<Vector3r>& path, float velocity, DrivetrainType drivetrain, const YawMode& yaw_mode,
    float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action);

virtual bool moveToPosition(float x, float y, float z, float velocity, DrivetrainType drivetrain,
    const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action);


virtual bool moveToZ(float z, float velocity, const YawMode& yaw_mode,
    float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action);

virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action);

virtual bool rotateToYaw(float yaw, float margin, CancelableBase& cancelable_action);

virtual bool rotateByYawRate(float yaw_rate, float duration, CancelableBase& cancelable_action);

virtual bool hover(CancelableBase& cancelable_action);

virtual Kinematics::State getKinematicsEstimated() = 0;

virtual Vector3r getPosition() = 0;

virtual Vector3r getVelocity() = 0;

virtual Quaternionr getOrientation() = 0;

virtual Pose getDebugPose();

Vector2r getPositionXY();

float getZ();

virtual LandedState getLandedState() = 0;
virtual int getRemoteControlID() { return -1; }
virtual RCData getRCData() = 0;
virtual RCData estimateRCTrims(CancelableBase& cancelable_action, float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100);
virtual void setRCData(const RCData& rcData) = 0;
virtual GeoPoint getHomeGeoPoint() = 0;
virtual GeoPoint getGpsLocation() = 0;
virtual CollisionInfo getCollisionInfo();
virtual void setCollisionInfo(const CollisionInfo& collision_info);
virtual void setSafetyEval(const shared_ptr<SafetyEval> safety_eval_ptr);
virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);
virtual const VehicleParams& getVehicleParams() = 0;
virtual bool loopCommandPre();
virtual void loopCommandPost();
DroneControllerBase() = default;
virtual ~DroneControllerBase() = default;

C++语言方面

用到的share ptr：shared_ptr 是一个标准的共享所有权的智能指针, 允许多个指针指向同一个对象. 定义在 memory 文件中(非memory.h), 命名空间为 std

std::this_thread::sleep_for这个是thread

在写测试func遇到的问题：

1. 首先写一个主函数，测试moveByAngleThrottle，在主函数中，删掉while（1）函数中的绝大多数，包括controlmode的删除，然后
   clock_t begin = clock();

    		//std::cout << " LocalPositionx: " << LocalPosition.at(nextnumber - 1)[0] << " LocalPositiony: " << LocalPosition.at(nextnumber - 1)[1] << std::endl;  // ！！注释掉有效
    		//std::cout << " LocalPosition3: " << LocalPosition.at(nextnumber - 1)[2] << std::endl;
    		curr_bardata = client.getBarometerdata();
    		ned_curr(2) = curr_bardata.altitude - Barometer_origin.altitude;
    		ned_curr(2) = kalman(ned_curr(2), prev, i_kalman, Q, R);
    		std::cout << "ned_curr: " << ned_curr(2) << std::endl;
    		// control函数括号中的值为当前值，期望值由setpoint函数指定
    		float delta_throttle = pidZ.control(ned_curr(2)) + 0.6;
    		std::cout << "delta_throttle: " << delta_throttle << std::endl;
    		// 中间 = 左右中间的取值，类似saturation函数
    		CLIP3(0.4, delta_throttle, 0.8);
    		client.moveByAngleThrottle(0.0f, 0.0f, (float)delta_throttle, 0.0f, 0.01f);
    		std::this_thread::sleep_for(std::chrono::duration<double>(0.01f));
    		client.moveByAngleThrottle(2.0f, 0.0f, (float)delta_throttle, 0.0f, 1.2f);
    		std::this_thread::sleep_for(std::chrono::duration<double>(1.2f));
    		client.moveByAngleThrottle(0.0f, 0.0f, (float)0.7, 0.0f, 1.2f);
    		std::this_thread::sleep_for(std::chrono::duration<double>(1.2f));
   

2.可以看到如上代码，但是运行出错，运行模式为Release x64模式。

尚不知道这是什么错，但是通过注释掉要往外打印的msg和localPosition两句话之后，可以成功运行，猜测可能由于编译器优化，使得变量消失，while（1）循环的后面次数读取不到值。

2. 测试（1）

下图对应代码为：
client.moveByAngleThrottle(0.0f, 0.0f, 0.6f, 0.0f, 5.0f); std::this_thread::sleep_for(std::chrono::duration(5.0f));

下图对应代码为
client.moveByAngleThrottle(0.5f, 0.0f, 0.8f, 0.0f, 5.0f);
std::this_thread::sleep_for(std::chrono::duration(5.0f));

可以看出throttle的值为0.6时，大致是悬停拉力。