代码熟悉

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 sensorstd::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时，大致是悬停拉力。