ArduPilot开源飞控之AP_Mount_Topotek

1. 源由

AP_Mount_Topotek是最近上传的代码，也是看下来最为独立且完善的云台设备(含摄像头、测距、ROI跟随等)后端代码。

• AP_Mount: integrate topotek gimbal driver
• AP_Mount: add topotek backend

2. 框架设计

1. 构造函数：继承自 AP_Mount_Backend_Serial 的构造函数，使用 using 关键字。

2. 禁止复制：使用 CLASS_NO_COPY 宏显式禁止 AP_Mount_Topotek 实例的复制。

3. 重写方法：

   • update()：更新安装位置。
   • healthy() const：检查安装是否正常。
   • has_pan_control() const：如果安装可以控制平移，则返回 true。
   • 多个与摄像头控制相关的方法（如 take_picture()、record_video()、set_zoom()、set_focus() 等）。
   • 发送摄像头信息和设置到地面控制站（GCS）的方法。
   • 与测距仪交互的方法。

4. 枚举：

   • HeaderType、AddressByte、ControlByte、ParseState：用于数据包解析和通信协议的枚举类型。

5. 私有成员：

   • 各种布尔标志和计数器（如 _recording、_is_tracking、_sdcard_status 等），用于管理内部状态。
   • 缓冲区（_msg_buff）和结构体（_parser），用于消息处理和解析。

6. 私有方法：

   • 用于读取传入数据包、请求云台信息、向云台发送命令（如 send_angle_target()、send_rate_target()）以及分析云台响应（如 gimbal_angle_analyse()、gimbal_record_analyse() 等）的方法。
   • 用于计算校验和、十六进制转换和处理数据包传输的实用方法。

7. 数据结构：

   • Identifier：用于表示标识符的固定大小数组的 typedef。
   • UartCmdFunctionHandler：用于将 UART 命令键映射到成员函数以进行消息处理的结构体。

8. 实例变量：

   • 各种实例变量（如 _last_tracking_state、_last_mode、_firmware_ver 等），用于存储云台的状态和接收到的信息。

3. 重要函数

3.1 动态过程

3.1.1 AP_Mount_Topotek::update

AP_Mount_Topotek::update() // 更新云台位置 - 应定期调用
|
|-- if (!_initialised)
|   |-- return;  // 未初始化则退出
|
|-- read_incoming_packets()  // 读取来自云台的传入数据包
|
|-- uint32_t now_ms = AP_HAL::millis();  // 10Hz更新频率
|   |-- if ((now_ms - _last_req_current_info_ms) < 100)
|       |-- return;  // 控制更新频率，避免过于频繁
|   |-- _last_req_current_info_ms = now_ms;
|
|-- if (_last_zoom_stop)
|   |-- // 重发停止变焦命令第二次，以防止数据传输错误
|   |-- _last_zoom_stop = false;
|   |-- send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM, true, 0);
|
|-- if (_last_focus_stop)
|   |-- // 重发停止对焦命令第二次，以防止数据传输错误
|   |-- _last_focus_stop = false;
|   |-- send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0);
|
|-- send_location_info()  // 发送与GPS相关的信息到云台
|
|-- _last_req_step++;  // 1Hz频率调用
|   |-- if (_last_req_step >= 10)
|       |-- _last_req_step = 0;
|
|-- switch (_last_req_step)
|   |-- case 0:
|       |-- // 获取云台版本
|       |-- if (!_got_gimbal_version)
|           |-- request_gimbal_version();
|       |-- break;
|   |-- case 2:
|       |-- // 请求云台姿态，1Hz
|       |-- request_gimbal_attitude();
|       |-- break;
|   |-- case 4:
|       |-- // 请求存储卡信息
|       |-- request_gimbal_sdcard_info();
|       |-- break;
|   |-- case 6:
|       |-- // 请求跟踪信息
|       |-- if (_is_tracking)
|           |-- request_track_status();
|       |-- break;
|
|-- set_rctargeting_on_rcinput_change()  // 若RC输入发生变化，则切换到RC_TARGETING模式
|
|-- if (_is_tracking)  // 处理跟踪状态
|   |-- if (_last_mode != _mode)
|       |-- // 若模式发生变化，则取消跟踪
|       |-- cancel_tracking();
|   |-- else
|       |-- return;  // 图像跟踪激活，不发送姿态目标
|
|-- _last_mode = _mode;
|
|-- switch (get_mode())  // 根据云台模式更新
|   |-- case MAV_MOUNT_MODE_RETRACT:
|       |-- // 将云台移动到“收起”位置
|       |-- const Vector3f &angle_bf_target = _params.retract_angles.get();
|       |-- mnt_target.target_type = MountTargetType::ANGLE;
|       |-- mnt_target.angle_rad.set(angle_bf_target * DEG_TO_RAD, false);
|       |-- break;
|   |-- case MAV_MOUNT_MODE_NEUTRAL:
|       |-- // 将云台移动到中性位置
|       |-- const Vector3f &angle_bf_target = _params.neutral_angles.get();
|       |-- mnt_target.target_type = MountTargetType::ANGLE;
|       |-- mnt_target.angle_rad.set(angle_bf_target * DEG_TO_RAD, false);
|       |-- break;
|   |-- case MAV_MOUNT_MODE_MAVLINK_TARGETING:
|       |-- // mavlink目标处理
|       |-- break;
|   |-- case MAV_MOUNT_MODE_RC_TARGETING:
|       |-- // RC_TARGETING模式，使用RC输入更新目标
|       |-- MountTarget rc_target;
|       |-- get_rc_target(mnt_target.target_type, rc_target);
|       |-- switch (mnt_target.target_type)
|           |-- case MountTargetType::ANGLE:
|               |-- mnt_target.angle_rad = rc_target;
|               |-- break;
|           |-- case MountTargetType::RATE:
|               |-- mnt_target.rate_rads = rc_target;
|               |-- break;
|       |-- break;
|   |-- case MAV_MOUNT_MODE_GPS_POINT:
|       |-- // 将云台指向GPS点
|       |-- if (get_angle_target_to_roi(mnt_target.angle_rad))
|           |-- mnt_target.target_type = MountTargetType::ANGLE;
|       |-- break;
|   |-- case MAV_MOUNT_MODE_HOME_LOCATION:
|       |-- // 将云台指向Home位置
|       |-- if (get_angle_target_to_home(mnt_target.angle_rad))
|           |-- mnt_target.target_type = MountTargetType::ANGLE;
|       |-- break;
|   |-- case MAV_MOUNT_MODE_SYSID_TARGET:
|       |-- // 将云台指向另一个车辆
|       |-- if (get_angle_target_to_sysid(mnt_target.angle_rad))
|           |-- mnt_target.target_type = MountTargetType::ANGLE;
|       |-- break;
|   |-- default:
|       |-- // 未知模式，引发内部错误
|       |-- INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|       |-- break;
|
|-- switch (mnt_target.target_type) // 根据目标类型发送目标角度或速率
|   |-- case MountTargetType::ANGLE:
|       |-- send_angle_target(mnt_target.angle_rad);
|       |-- break;
|   |-- case MountTargetType::RATE:
|       |-- send_rate_target(mnt_target.rate_rads);
|       |-- break;

3.1.2 AP_Mount_Backend::calculate_poi

略，详见：ArduPilot开源飞控之AP_Mount_Backend

3.2 基础能力

3.2.1 AP_Mount_Topotek::healthy

// 如果健康则返回true
bool AP_Mount_Topotek::healthy() const
{// 如果未初始化，则立即退出if (!_initialised) {return false;}// 如果最近没有接收到姿态信息，则认为不健康const uint32_t last_current_angle_ms = _last_current_angle_ms;return (AP_HAL::millis() - last_current_angle_ms < AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS);
}

3.2.2 AP_Mount_Topotek::has_pan_control

// has_pan_control - 如果该云台可以控制其水平旋转（多旋翼飞行器所需），则返回 true
bool has_pan_control() const override { return yaw_range_valid(); };

3.3 设备功能

3.3.1 AP_Mount_Topotek::take_picture

// 拍摄照片。成功返回 true
bool AP_Mount_Topotek::take_picture()
{// 如果未初始化，立即退出if (!_initialised) {return false;}// 如果内存卡异常，立即退出if (!_sdcard_status) {GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD 卡错误", send_message_prefix);return false;}// 示例命令: #TPUD2wCAP01return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE, true, 1);
}

3.3.2 AP_Mount_Topotek::record_video

// 启动或停止视频录制。成功时返回 true
// 设置 start_recording = true 开始录制，设置为 false 停止录制
bool AP_Mount_Topotek::record_video(bool start_recording)
{// 如果未初始化，立即退出if (!_initialised) {return false;}// 如果存储卡异常，立即退出if (!_sdcard_status) {GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD 卡错误", send_message_prefix);return false;}// 示例命令: #TPUD2wREC01return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_RECORD_VIDEO, true, start_recording ? 1 : 0);
}

3.3.3 AP_Mount_Topotek::set_zoom

// 设置缩放指定为比例
bool AP_Mount_Topotek::set_zoom(ZoomType zoom_type, float zoom_value)
{// 如果没有初始化则立即退出if (!_initialised) {return false;}// 缩放比例if (zoom_type == ZoomType::RATE) {uint8_t zoom_cmd;if (is_zero(zoom_value)) {// 停止缩放zoom_cmd = 0;_last_zoom_stop = true;} else if (zoom_value < 0) {// 缩小zoom_cmd = 1;} else {// 放大zoom_cmd = 2;}// 示例命令: #TPUM2wZMC00return send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM, true, zoom_cmd);}// 不支持的缩放类型return false;
}

3.3.4 AP_Mount_Topotek::set_focus

// 设置对焦类型，可以是速度、百分比或自动
// focus in = -1, focus hold = 0, focus out = 1
SetFocusResult AP_Mount_Topotek::set_focus(FocusType focus_type, float focus_value)
{// 如果没有初始化，立即退出if (!_initialised) {return SetFocusResult::FAILED;}switch (focus_type) {case FocusType::RATE: {// 停止对焦uint8_t focus_cmd;if (is_zero(focus_value)) {focus_cmd = 0;_last_focus_stop = true;} else if (focus_value < 0) {// 对焦-focus_cmd = 2;} else {// 对焦+focus_cmd = 1;}// 发送对焦命令并切换到手动对焦// 示例命令: #TPUM2wFCC00if (send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, focus_cmd) &&send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0x11)) {return SetFocusResult::ACCEPTED;}return SetFocusResult::FAILED;}case FocusType::PCT:// 不支持return SetFocusResult::INVALID_PARAMETERS;case FocusType::AUTO:// 自动对焦if (send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0x10)) {return SetFocusResult::ACCEPTED;}return SetFocusResult::FAILED;}// 不支持的对焦类型return SetFocusResult::INVALID_PARAMETERS;
}

3.3.5 AP_Mount_Topotek::set_tracking

// 设置跟踪模式为无、点或矩形（参见 TrackingType 枚举）
// 如果是 POINT 仅使用 p1，如果是 RECTANGLE 则 p1 是左上角，p2 是右下角
// p1、p2 的范围是 0 到 1。0 表示左或上，1 表示右或下
bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
{// 如果未初始化则立即退出if (!_initialised) {return false;}// 局部变量保存跟踪中心和宽度int16_t track_center_x, track_center_y, track_width, track_height;bool send_tracking_cmd = false;switch (tracking_type) {case TrackingType::TRK_NONE:return cancel_tracking();case TrackingType::TRK_POINT: {// 计算跟踪中心、宽度和高度track_center_x = (int16_t)((p1.x*TRACK_TOTAL_WIDTH - 960) /  0.96);track_center_y = (int16_t)((p1.y*TRACK_TOTAL_HEIGHT - 540) /  0.54);track_width = (int16_t)(TRACK_RANGE / 0.96);track_height = (int16_t)(TRACK_RANGE / 0.54);send_tracking_cmd = true;break;}case TrackingType::TRK_RECTANGLE:// 计算左上角和右下角点// 处理 p1 和 p2 顺序意外的情况int16_t upper_leftx = (int16_t)(MIN(p1.x, p2.x)*TRACK_TOTAL_WIDTH);int16_t upper_lefty = (int16_t)(MIN(p1.y, p2.y)*TRACK_TOTAL_HEIGHT);int16_t bottom_rightx = (int16_t)(MAX(p1.x, p2.x)*TRACK_TOTAL_WIDTH);int16_t bottom_righty = (int16_t)(MAX(p1.y, p2.y)*TRACK_TOTAL_HEIGHT);// 计算宽度和高度并进行合理性检查 const int16_t frame_selection_width = bottom_rightx - upper_leftx;const int16_t frame_selection_height = bottom_righty - upper_lefty;if (frame_selection_width <= 0 或 frame_selection_height <= 0) {return false;}// 计算跟踪中心track_center_x = (int16_t)((((upper_leftx + bottom_rightx) * 0.5) - 960) / 0.96);track_center_y = (int16_t)((((upper_lefty + bottom_righty) * 0.5) - 540) / 0.54);// 转换后的跟踪范围track_width = (int16_t)(frame_selection_width / 0.96);track_height = (int16_t)(frame_selection_height / 0.54);send_tracking_cmd = true;break;}if (send_tracking_cmd) {// 准备数据字节uint8_t databuff[10];databuff[0] = HIGHBYTE(track_center_x);databuff[1] = LOWBYTE(track_center_x);databuff[2] = HIGHBYTE(track_center_y);databuff[3] = LOWBYTE(track_center_y);databuff[4] = HIGHBYTE(track_width);databuff[5] = LOWBYTE(track_width);databuff[6] = HIGHBYTE(track_height);databuff[7] = LOWBYTE(track_height);databuff[8] = 0;databuff[9] = 0;// 发送跟踪命令bool res = send_variablelen_packet(HeaderType::VARIABLE_LEN,AddressByte::SYSTEM_AND_IMAGE,AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING,true,(uint8_t*)databuff, ARRAY_SIZE(databuff));_is_tracking |= res;return res;}// 不应该到达这里return false;
}

3.3.6 AP_Mount_Topotek::cancel_tracking

// 向云台发送取消跟踪命令（如果有必要）
// 成功返回 true，发送消息失败返回 false
bool AP_Mount_Topotek::cancel_tracking()
{// 如果未初始化则立即退出if (!_initialised) {return false;}// 发送跟踪命令if (send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 1)) {return true;}return false;
}

3.3.7 AP_Mount_Topotek::set_lens

// 设置摄像头画中画模式
bool AP_Mount_Topotek::set_lens(uint8_t lens)
{// 如果未初始化，立即退出if (!_initialised) {return false;}// 检查镜头编号的有效性// 00: 仅主镜头, 01: 主镜头+副镜头, 02: 副镜头+主镜头, 03: 仅副镜头, 0A: 下一个// 示例命令: #TPUD2wPIP0Aif (lens > 3) {return false;}// 发送画中画命令return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_PIP, true, lens);
}

3.4 测距功能

3.4.1 AP_Mount_Topotek::get_rangefinder_distance

// 获取测距仪距离。成功时返回 true
bool AP_Mount_Topotek::get_rangefinder_distance(float& distance_m) const
{// 如果不健康或距离为负则返回 false// healthy() 检查姿态超时，该超时与测距仪距离在同一消息中if (!healthy() || (_measure_dist_m < 0)) {return false;}distance_m = _measure_dist_m;return true;
}

3.4.2 AP_Mount_Topotek::set_rangefinder_enable

// enable/disable rangefinder.  Returns true on success
// 启用/禁用测距仪。成功时返回true
bool AP_Mount_Topotek::set_rangefinder_enable(bool enable)
{// exit immediately if not initialised// 如果未初始化，则立即退出if (!_initialised) {return false;}// 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement// 00：停止测距，01：开始测距，02：单次测量，03：连续测量// sample command: #TPUM2wLRF00// 示例命令：#TPUM2wLRF00return send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_LRF, true, enable ? 3 : 0);
}

3.5 辅助函数

3.5.1 AP_Mount_Topotek::set_camera_source

// set_camera_source功能上与set_lens相同，只是通过类型指定主要和次要镜头
// 主要和次要源使用AP_Camera::CameraSource枚举转换为uint8_t
bool AP_Mount_Topotek::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
{// 如果未初始化，则立即退出if (!_initialised) {return false;}// 将主要和次要源映射到画中画（PiP）设置// PiP设置 00:仅主，01:主+次，02:次+主，03:仅次，0A:下一个// 示例命令：#TPUD2wPIP0Auint8_t pip_setting = 0;switch (primary_source) {case 0: // 默认（RGB）FALLTHROUGH;  // 继续执行下一个casecase 1: // RGBswitch (secondary_source) {case 0: // RGB + 默认（无）pip_setting = 0;    // 仅主break;case 2: // PIP RGB+IRpip_setting = 1;    // 主+次break;default:return false;}break;case 2: // IRswitch (secondary_source) {case 0: // IR + 默认（无）pip_setting = 3;    // 仅次break;case 1: // IR+RGBpip_setting = 2;    // 次+主break;default:return false;}break;default:return false;}// 发送PiP命令return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_PIP, true, pip_setting);
}

3.5.2 AP_Mount_Topotek::send_camera_information

// 发送相机信息消息到地面控制站
void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const
{// 如果未初始化，则立即退出if (!_initialised) {return;}static const uint8_t vendor_name[32] = "Topotek";  // 厂商名称static uint8_t model_name[32] {};  // 模型名称const char cam_definition_uri[140] {};  // 相机定义URI// 能力标志const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO |CAMERA_CAP_FLAGS_CAPTURE_IMAGE |CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM |CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS |CAMERA_CAP_FLAGS_HAS_TRACKING_POINT |CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE;// 发送相机信息消息mavlink_msg_camera_information_send(chan,AP_HAL::millis(),       // time_boot_ms，引导系统时间（毫秒）vendor_name,            // 厂商名称 uint8_t[32]model_name,             // 模型名称 uint8_t[32]_firmware_ver,          // 固件版本 uint32_t0,                      // 焦距 float (mm)0,                      // 水平传感器尺寸 float (mm)0,                      // 垂直传感器尺寸 float (mm)0,                      // 水平分辨率 uint16_t (像素)0,                      // 垂直分辨率 uint16_t (像素)0,                      // 镜头 ID uint8_tflags,                  // 标志 uint32_t (相机能力标志)0,                      // 相机定义版本 uint16_tcam_definition_uri,     // 相机定义URI char[140]_instance + 1);         // 云台设备 ID uint8_t
}

3.5.3 AP_Mount_Topotek::send_camera_settings

// 向 GCS 发送相机设置消息
void AP_Mount_Topotek::send_camera_settings(mavlink_channel_t chan) const
{// 如果未初始化，则立即退出if (!_initialised) {return;}const float NaN = nanf("0x4152");// 发送 CAMERA_SETTINGS 消息mavlink_msg_camera_settings_send(chan,AP_HAL::millis(),   // time_boot_ms，系统启动时间（毫秒）_recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // 相机模式（0: 图像, 1: 视频, 2: 图像勘测）NaN,                // zoomLevel 浮点数，百分比从 0 到 100，如果未知则为 NaNNaN);               // focusLevel 浮点数，百分比从 0 到 100，如果未知则为 NaN
}

4. 总结

该协议支持来源于拓扑联创，其英文网站Topotek。

由于开源社区体系的完善，设备提供方为了更好的服务客服，融入社区，就必须提供已有后端驱动接口或者提供上述对接源代码。

从设计的角度，这就是一个类似灰盒的接口暴露在外，供三方应用更好的集成和测试。这是一种非常好的设计模式，很高兴看到这么多Ardupilot Partner的设备源源不断的进入社区。

5. 参考资料

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