1. 源由

AP_Mount_Backend是云台设备模版，抽象云台设备操作方法，并在该类中得以实现。

提供了一个全面的接口，用于控制和监控安装设备（云台），集成了各种功能，如目标跟踪、模式设置、命令处理和通过 MAVLink 消息与地面控制系统通信。它设计灵活，可能用于适配不同类型的安装设备及其特定需求。

2. 框架设计

2.1 类构造函数

• 构造函数：初始化后端实例，传入前端引用 (AP_Mount)、参数 (AP_Mount_Params) 和实例编号 (instance)。

2.2 公共方法

2.2.1 重要方法

• 初始化方法：virtual void init(); - 执行实例的初始化。
• 设备ID设置：void set_dev_id(uint32_t id); - 设置设备ID。
• 更新方法：
  • virtual void update() = 0; - 纯虚函数，用于更新安装设备的位置。
  • virtual void update_fast() {} - 可选方法，用于更快速的更新，这里没有具体实现。

2.2.2 运动能力（需要在子类中重写）

• 健康检查：virtual bool healthy() const; - 表示安装设备是否正常工作。
• 控制能力：
  • virtual bool has_roll_control() const; - 是否具有横滚控制能力。
  • virtual bool has_pitch_control() const; - 是否具有俯仰控制能力。
  • virtual bool has_pan_control() const = 0; - 是否可以控制偏航，纯虚函数。

2.2.3 模式处理

• bool valid_mode(MAV_MOUNT_MODE mode) const; - 检查模式是否有效。
• enum MAV_MOUNT_MODE get_mode() const; - 获取当前模式。
• bool set_mode(enum MAV_MOUNT_MODE mode); - 设置模式。

2.2.4 目标处理

• 方法用于设置角度和速率目标，处理兴趣区域（ROI）目标和系统目标，比如calculate_poi 等。

2.2.5 命令处理

• 处理来自 MAVLink 消息的命令的方法，例如 do_mount_control 和 do_gimbal_manager_configure。

2.2.6 消息传递和状态

• 发送状态消息到 MAVLink 通道的方法（send_gimbal_device_attitude_status、send_gimbal_manager_information 等）。

2.2.7 回调函数

• 处理各种 MAVLink 消息的回调方法（handle_gimbal_report、handle_param_value 等）。

2.2.8 相机控制

• 控制相机的方法，例如拍照 (take_picture)、录像 (record_video)、设置变焦 (set_zoom) 等。

2.2.9 测距仪

• 与测距仪交互的方法（get_rangefinder_distance、set_rangefinder_enable）。

2.3 保护成员

• 内部使用的枚举和结构体（MountTargetType、Options、MountTarget 等）。
• 辅助方法（option_set、yaw_range_valid 等）和内部状态变量（_mode、_yaw_lock、_roi_target 等）。

3. 重要函数

3.1 重要方法

3.1.1 AP_Mount_Backend::AP_Mount_Backend

构造函数，初始化以下几个成员变量：

• _frontend
• _params
• _instance

    // ConstructorAP_Mount_Backend(class AP_Mount &frontend, class AP_Mount_Params &params, uint8_t instance) :_frontend(frontend),_params(params),_instance(instance){}

3.1.2 AP_Mount_Backend::init

启动线程calculate_poi进行计算

// Default init function for every mount
void AP_Mount_Backend::init()
{// setting default target sysid from parameters_target_sysid = _params.sysid_default.get();#if AP_MOUNT_POI_TO_LATLONALT_ENABLED// create a calculation thread for poi.if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Mount_Backend::calculate_poi, void),"mount_calc_poi",8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mount: failed to start POI thread");}
#endif
}

3.1.3 AP_Mount_Backend::set_dev_id

设置设备DEV_ID。

// set device id of this instance, for MNTx_DEVID parameter
void AP_Mount_Backend::set_dev_id(uint32_t id)
{_params.dev_id.set_and_save(int32_t(id));
}

3.1.4 AP_Mount_Backend::update/update_fast

• update函数必须在继承设备类中实现
• update_fast一般不需要实现

    // update mount position - should be called periodicallyvirtual void update() = 0;// used for gimbals that need to read INS data at full ratevirtual void update_fast() {}

3.2 运动能力

3.2.1 AP_Mount_Backend::healthy

stub函数，默认设备健康。

    // return true if healthyvirtual bool healthy() const { return true; }

3.2.2 AP_Mount_Backend::has_roll_control

通过配置roll_angle_min和roll_angle_max判断。

// return true if this mount accepts roll targets
bool AP_Mount_Backend::has_roll_control() const
{return (_params.roll_angle_min < _params.roll_angle_max);
}

3.2.3 AP_Mount_Backend::has_pitch_control

通过配置pitch_angle_min和pitch_angle_max判断。

// return true if this mount accepts pitch targets
bool AP_Mount_Backend::has_pitch_control() const
{return (_params.pitch_angle_min < _params.pitch_angle_max);
}

3.2.4 AP_Mount_Backend::has_pan_control

必须定义yaw方向的能力。

    // returns true if this mount can control its pan (required for multicopters)virtual bool has_pan_control() const = 0;

3.3 模式处理

3.3.1 AP_Mount_Backend::valid_mode

有效模式

• MAV_MOUNT_MODE_RETRACT
• MAV_MOUNT_MODE_NEUTRAL
• MAV_MOUNT_MODE_MAVLINK_TARGETING
• MAV_MOUNT_MODE_RC_TARGETING
• MAV_MOUNT_MODE_GPS_POINT
• MAV_MOUNT_MODE_SYSID_TARGET
• MAV_MOUNT_MODE_HOME_LOCATION

bool AP_Mount_Backend::valid_mode(MAV_MOUNT_MODE mode) const
{switch (mode) {case MAV_MOUNT_MODE_RETRACT...MAV_MOUNT_MODE_HOME_LOCATION:return true;case MAV_MOUNT_MODE_ENUM_END:return false;}return false;
}

3.3.2 AP_Mount_Backend::set_mode

设置模式

bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
{if (!valid_mode(mode)) {return false;}_mode = mode;return true;
}

3.3.3 AP_Mount_Backend::get_mode

获取模式

    // get mount's modeenum MAV_MOUNT_MODE get_mode() const { return _mode; }

3.4 目标处理

3.4.1 AP_Mount_Backend::set_roi_target

设定ROI区域

// set_roi_target - sets target location that mount should attempt to point towards
void AP_Mount_Backend::set_roi_target(const Location &target_loc)
{// set the target gps location_roi_target = target_loc;_roi_target_set = true;// set the mode to GPS tracking modeset_mode(MAV_MOUNT_MODE_GPS_POINT);// optionally set RC_TARGETING yaw lock stateif (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {set_yaw_lock(true);}
}

3.4.2 AP_Mount_Backend::clear_roi_target

清除ROI区域

// clear_roi_target - clears target location that mount should attempt to point towards
void AP_Mount_Backend::clear_roi_target()
{// clear the target GPS location_roi_target_set = false;// reset the mode if in GPS tracking modeif (get_mode() == MAV_MOUNT_MODE_GPS_POINT) {MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get();set_mode(default_mode);}
}

3.4.3 AP_Mount_Backend::get_angle_target_to_roi

计算地球坐标系下ROI角度

// get angle targets (in radians) to ROI location
// returns true on success, false on failure
bool AP_Mount_Backend::get_angle_target_to_roi(MountTarget& angle_rad) const
{if (!_roi_target_set) {return false;}return get_angle_target_to_location(_roi_target, angle_rad);
}

3.4.4 AP_Mount_Backend::calculate_poi

定时更新相机ROI角度

// calculate the Location that the gimbal is pointing at
void AP_Mount_Backend::calculate_poi()
{while (true) {// run this loop at 10hzhal.scheduler->delay(100);// calculate poi if requested within last 30 seconds{WITH_SEMAPHORE(poi_calculation.sem);if ((poi_calculation.poi_request_ms == 0) ||(AP_HAL::millis() - poi_calculation.poi_request_ms > AP_MOUNT_POI_REQUEST_TIMEOUT_MS)) {continue;}}// get the current location of vehicleconst AP_AHRS &ahrs = AP::ahrs();Location curr_loc;if (!ahrs.get_location(curr_loc)) {continue;}// change vehicle alt to AMSLcurr_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);// project forward from vehicle looking for terrain// start testing at vehicle's locationLocation test_loc = curr_loc;Location prev_test_loc = curr_loc;// get terrain altitude (AMSL) at test_locauto terrain = AP_Terrain::get_singleton();float terrain_amsl_m;if ((terrain == nullptr) || !terrain->height_amsl(test_loc, terrain_amsl_m, true)) {continue;}// retrieve gimbal attitudeQuaternion quat;if (!get_attitude_quaternion(quat)) {// gimbal attitude unavailablecontinue;}// iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-levelconst float dist_increment_m = MAX(terrain->get_grid_spacing(), 10);const float mount_pitch_deg = degrees(quat.get_euler_pitch());const float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + degrees(ahrs.get_yaw()));float total_dist_m = 0;bool get_terrain_alt_success = true;float prev_terrain_amsl_m = terrain_amsl_m;while (total_dist_m < AP_MOUNT_POI_DIST_M_MAX && (test_loc.alt * 0.01) > terrain_amsl_m) {total_dist_m += dist_increment_m;// backup previous test location and terrain amslprev_test_loc = test_loc;prev_terrain_amsl_m = terrain_amsl_m;// move test location forwardtest_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m);// get terrain's alt-above-sea-level (at test_loc)// fail if terrain alt cannot be retrievedif (!terrain->height_amsl(test_loc, terrain_amsl_m, true) || std::isnan(terrain_amsl_m)) {get_terrain_alt_success = false;continue;}}// if a fail occurred above when getting terrain alt then restart calculations from the beginningif (!get_terrain_alt_success) {continue;}if (total_dist_m >= AP_MOUNT_POI_DIST_M_MAX) {// unable to find terrain within dist_maxcontinue;}// test location has dropped below terrain// interpolate along line between prev_test_loc and test_locfloat dist_interp_m = linear_interpolate(0, dist_increment_m, 0, prev_test_loc.alt * 0.01 - prev_terrain_amsl_m, test_loc.alt * 0.01 - terrain_amsl_m);{WITH_SEMAPHORE(poi_calculation.sem);poi_calculation.poi_loc = prev_test_loc;poi_calculation.poi_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m);poi_calculation.att_quat = {quat[0], quat[1], quat[2], quat[3]};poi_calculation.loc = curr_loc;poi_calculation.poi_update_ms = AP_HAL::millis();}}
}

3.5 命令处理

3.5.1 AP_Mount_Backend::handle_command_do_mount_control

AP_Mount::handle_command //MAV_CMD_DO_MOUNT_CONTROL└──> AP_Mount::handle_command_do_mount_control└──> AP_Mount_Backend::handle_command_do_mount_control

// handle do_mount_control command.  Returns MAV_RESULT_ACCEPTED on success
MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_command_int_t &packet)
{const MAV_MOUNT_MODE new_mode = (MAV_MOUNT_MODE)packet.z;// interpret message fields based on modeswitch (new_mode) {case MAV_MOUNT_MODE_RETRACT:case MAV_MOUNT_MODE_NEUTRAL:case MAV_MOUNT_MODE_RC_TARGETING:case MAV_MOUNT_MODE_HOME_LOCATION:// simply set modeset_mode(new_mode);return MAV_RESULT_ACCEPTED;case MAV_MOUNT_MODE_MAVLINK_TARGETING: {// set target angles (in degrees) from mavlink messageconst float pitch_deg = packet.param1;  // param1: pitch (earth-frame, degrees)const float roll_deg = packet.param2;   // param2: roll (earth-frame, degrees)const float yaw_deg = packet.param3;    // param3: yaw (body-frame, degrees)// warn if angles are invalid to catch angles sent in centi-degreesif ((fabsf(pitch_deg) > 90) || (fabsf(roll_deg) > 180) || (fabsf(yaw_deg) > 360)) {send_warning_to_GCS("invalid angle targets");return MAV_RESULT_FAILED;}set_angle_target(packet.param2, packet.param1, packet.param3, false);return MAV_RESULT_ACCEPTED;}case MAV_MOUNT_MODE_GPS_POINT: {// set lat, lon, alt position targets from mavlink message// warn if lat, lon appear to be in param1,2 instead of param x,y as this indicates// sender is relying on a bug in AP-4.2's (and earlier) handling of MAV_CMD_DO_MOUNT_CONTROLif (!is_zero(packet.param1) && !is_zero(packet.param2) && packet.x == 0 && packet.y == 0) {send_warning_to_GCS("GPS_POINT target invalid");return MAV_RESULT_FAILED;}// param4: altitude in meters// x: latitude in degrees * 1E7// y: longitude in degrees * 1E7const Location target_location {packet.x,                       // latitude in degrees * 1E7packet.y,                       // longitude in degrees * 1E7(int32_t)packet.param4 * 100,   // alt converted from meters to cmLocation::AltFrame::ABOVE_HOME};set_roi_target(target_location);return MAV_RESULT_ACCEPTED;}default:// invalid modereturn MAV_RESULT_FAILED;}
}

3.5.2 AP_Mount_Backend::handle_command_do_gimbal_manager_configure

AP_Mount::handle_command //MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE└──> AP_Mount::handle_command_do_gimbal_manager_configure└──> AP_Mount_Backend::handle_command_do_gimbal_manager_configure

// handle do_gimbal_manager_configure.  Returns MAV_RESULT_ACCEPTED on success
// requires original message in order to extract caller's sysid and compid
MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{// sanity check param1 and param2 valuesif ((packet.param1 < -3) || (packet.param1 > UINT8_MAX) || (packet.param2 < -3) || (packet.param2 > UINT8_MAX)) {return MAV_RESULT_FAILED;}// backup the current values so we can detect a changemavlink_control_id_t prev_control_id = mavlink_control_id;// convert negative packet1 and packet2 valuesint16_t new_sysid = packet.param1;switch (new_sysid) {case -1:// leave unchangedbreak;case -2:// set itself in controlmavlink_control_id.sysid = msg.sysid;mavlink_control_id.compid = msg.compid;break;case -3:// remove control if currently in controlif ((mavlink_control_id.sysid == msg.sysid) && (mavlink_control_id.compid == msg.compid)) {mavlink_control_id.sysid = 0;mavlink_control_id.compid = 0;}break;default:mavlink_control_id.sysid = packet.param1;mavlink_control_id.compid = packet.param2;break;}// send gimbal_manager_status if control has changedif (prev_control_id != mavlink_control_id) {GCS_SEND_MESSAGE(MSG_GIMBAL_MANAGER_STATUS);}return MAV_RESULT_ACCEPTED;
}

3.6 消息传递和状态

3.6.1 AP_Mount_Backend::send_gimbal_device_attitude_status

GCS_MAVLINK::try_send_message  //MSG_GIMBAL_DEVICE_ATTITUDE_STATUS└──> GCS_MAVLINK::send_gimbal_device_attitude_status└──> AP_Mount::send_gimbal_device_attitude_status└──> AP_Mount_Backend::send_gimbal_device_attitude_status

// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan)
{if (suppress_heartbeat()) {// block heartbeat from transmitting to the GCSGCS_MAVLINK::disable_channel_routing(chan);}Quaternion att_quat;if (!get_attitude_quaternion(att_quat)) {return;}Vector3f ang_velocity { nanf(""), nanf(""), nanf("") };IGNORE_RETURN(get_angular_velocity(ang_velocity));// construct quaternion arrayconst float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4};mavlink_msg_gimbal_device_attitude_status_send(chan,0,   // target system0,   // target componentAP_HAL::millis(),    // autopilot system timeget_gimbal_device_flags(),quat_array,    // attitude expressed as quaternionang_velocity.x,    // roll axis angular velocity (NaN for unknown)ang_velocity.y,    // pitch axis angular velocity (NaN for unknown)ang_velocity.z,    // yaw axis angular velocity (NaN for unknown)0,                                           // failure flags (not supported)std::numeric_limits<double>::quiet_NaN(),    // delta_yaw (NaN for unknonw)std::numeric_limits<double>::quiet_NaN(),    // delta_yaw_velocity (NaN for unknonw)_instance + 1);  // gimbal_device_id
}

3.6.2 AP_Mount_Backend::send_gimbal_manager_information

GCS_MAVLINK::try_send_message  //MSG_GIMBAL_MANAGER_INFORMATION└──> GCS_MAVLINK::send_gimbal_manager_information└──> AP_Mount::send_gimbal_manager_information└──> AP_Mount_Backend::send_gimbal_manager_information

// send a GIMBAL_MANAGER_INFORMATION message to GCS
void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
{mavlink_msg_gimbal_manager_information_send(chan,AP_HAL::millis(),                       // autopilot system timeget_gimbal_manager_capability_flags(),  // bitmap of gimbal manager capability flags_instance + 1,                          // gimbal device idradians(_params.roll_angle_min),        // roll_min in radiansradians(_params.roll_angle_max),        // roll_max in radiansradians(_params.pitch_angle_min),       // pitch_min in radiansradians(_params.pitch_angle_max),       // pitch_max in radiansradians(_params.yaw_angle_min),         // yaw_min in radiansradians(_params.yaw_angle_max));        // yaw_max in radians
}

3.7 回调函数

3.7.1 AP_Mount_Backend::handle_gimbal_report

    // handle a GIMBAL_REPORT messagevirtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}

3.7.2 AP_Mount_Backend::handle_param_value

    // handle a PARAM_VALUE messagevirtual void handle_param_value(const mavlink_message_t &msg) {}

3.8 相机控制

虚函数，根据云台+相机的复合设备都需要在具体的设备驱动后台实现。

3.8.1 AP_Mount_Backend::take_picture

    // take a picture.  returns true on successvirtual bool take_picture() { return false; }

3.8.2 AP_Mount_Backend::record_video

    // start or stop video recording.  returns true on success// set start_recording = true to start record, false to stop recordingvirtual bool record_video(bool start_recording) { return false; }

3.8.3 AP_Mount_Backend::set_zoom

    // set zoom specified as a rate or percentagevirtual bool set_zoom(ZoomType zoom_type, float zoom_value) { return false; }

3.9 测距仪

虚函数，根带测距仪的复合设备都需要在具体的设备驱动后台实现。

3.9.1 AP_Mount_Backend::get_rangefinder_distance

    // get rangefinder distance.  Returns true on successvirtual bool get_rangefinder_distance(float& distance_m) const { return false; }

3.9.2 AP_Mount_Backend::set_rangefinder_enable

    // enable/disable rangefinder.  Returns true on successvirtual bool set_rangefinder_enable(bool enable) { return false; }

3.10 辅助函数

3.10.1 AP_Mount_Backend::get_angle_target_to_location

// get angle targets (in radians) to a Location
// returns true on success, false on failure
bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const
{// exit immediately if vehicle's location is unavailableLocation current_loc;if (!AP::ahrs().get_location(current_loc)) {return false;}// exit immediate if location is invalidif (!loc.initialised()) {return false;}const float GPS_vector_x = Location::diff_longitude(loc.lng, current_loc.lng)*cosf(ToRad((current_loc.lat + loc.lat) * 0.00000005f)) * 0.01113195f;const float GPS_vector_y = (loc.lat - current_loc.lat) * 0.01113195f;int32_t target_alt_cm = 0;if (!loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) {return false;}int32_t current_alt_cm = 0;if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, current_alt_cm)) {return false;}float GPS_vector_z = target_alt_cm - current_alt_cm;float target_distance = 100.0f*norm(GPS_vector_x, GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.// calculate roll, pitch, yaw anglesangle_rad.roll = 0;angle_rad.pitch = atan2f(GPS_vector_z, target_distance);angle_rad.yaw = atan2f(GPS_vector_x, GPS_vector_y);angle_rad.yaw_is_ef = true;return true;
}

3.10.2 AP_Mount_Backend::set_angle_target

// set angle target in degrees
// roll and pitch are in earth-frame
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
{// enforce angle limitsroll_deg = constrain_float(roll_deg, _params.roll_angle_min, _params.roll_angle_max);pitch_deg = constrain_float(pitch_deg, _params.pitch_angle_min, _params.pitch_angle_max);if (!yaw_is_earth_frame) {// only limit yaw if in body-frame.  earth-frame yaw limiting is backend specific// custom wrap code (instead of wrap_180) to better handle yaw of <= -180if (yaw_deg > 180) {yaw_deg -= 360;}yaw_deg = constrain_float(yaw_deg, _params.yaw_angle_min, _params.yaw_angle_max);}// set angle targetsmnt_target.target_type = MountTargetType::ANGLE;mnt_target.angle_rad.roll = radians(roll_deg);mnt_target.angle_rad.pitch = radians(pitch_deg);mnt_target.angle_rad.yaw = radians(yaw_deg);mnt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame;// set the mode to mavlink targetingset_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);// optionally set RC_TARGETING yaw lock stateif (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {set_yaw_lock(yaw_is_earth_frame);}
}

4. 总结

AP_Mount_Backend是在云台+相机+测距+相机跟随的复合设备抽象类。

• AP_Mount_Backend
• AP_Mount_Servo
• AP_Mount_SoloGimbal
• AP_Mount_Alexmos
• AP_Mount_SToRM32
• AP_Mount_SToRM32_serial
• AP_Mount_Gremsy
• AP_Mount_Siyi
• AP_Mount_Scripting
• AP_Mount_Xacti
• AP_Mount_Viewpro
• AP_Mount_Topotek //最近更新的设备类

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源飞控之AP_Mount