ArduPilot开源飞控之AP_Mount_Topotek
- 1. 源由
- 2. 框架设计
- 3. 重要函数
- 3.1 动态过程
- 3.1.1 AP_Mount_Topotek::update
- 3.1.2 AP_Mount_Backend::calculate_poi
- 3.2 基础能力
- 3.2.1 AP_Mount_Topotek::healthy
- 3.2.2 AP_Mount_Topotek::has_pan_control
- 3.3 设备功能
- 3.3.1 AP_Mount_Topotek::take_picture
- 3.3.2 AP_Mount_Topotek::record_video
- 3.3.3 AP_Mount_Topotek::set_zoom
- 3.3.4 AP_Mount_Topotek::set_focus
- 3.3.5 AP_Mount_Topotek::set_tracking
- 3.3.6 AP_Mount_Topotek::cancel_tracking
- 3.3.7 AP_Mount_Topotek::set_lens
- 3.4 测距功能
- 3.4.1 AP_Mount_Topotek::get_rangefinder_distance
- 3.4.2 AP_Mount_Topotek::set_rangefinder_enable
- 3.5 辅助函数
- 3.5.1 AP_Mount_Topotek::set_camera_source
- 3.5.2 AP_Mount_Topotek::send_camera_information
- 3.5.3 AP_Mount_Topotek::send_camera_settings
- 4. 总结
- 5. 参考资料
1. 源由
AP_Mount_Topotek
是最近上传的代码,也是看下来最为独立且完善的云台设备(含摄像头、测距、ROI跟随等)后端代码。
- AP_Mount: integrate topotek gimbal driver
- AP_Mount: add topotek backend
2. 框架设计
-
构造函数:继承自
AP_Mount_Backend_Serial
的构造函数,使用using
关键字。 -
禁止复制:使用
CLASS_NO_COPY
宏显式禁止AP_Mount_Topotek
实例的复制。 -
重写方法:
update()
:更新安装位置。healthy() const
:检查安装是否正常。has_pan_control() const
:如果安装可以控制平移,则返回 true。- 多个与摄像头控制相关的方法(如
take_picture()
、record_video()
、set_zoom()
、set_focus()
等)。 - 发送摄像头信息和设置到地面控制站(GCS)的方法。
- 与测距仪交互的方法。
-
枚举:
HeaderType
、AddressByte
、ControlByte
、ParseState
:用于数据包解析和通信协议的枚举类型。
-
私有成员:
- 各种布尔标志和计数器(如
_recording
、_is_tracking
、_sdcard_status
等),用于管理内部状态。 - 缓冲区(
_msg_buff
)和结构体(_parser
),用于消息处理和解析。
- 各种布尔标志和计数器(如
-
私有方法:
- 用于读取传入数据包、请求云台信息、向云台发送命令(如
send_angle_target()
、send_rate_target()
)以及分析云台响应(如gimbal_angle_analyse()
、gimbal_record_analyse()
等)的方法。 - 用于计算校验和、十六进制转换和处理数据包传输的实用方法。
- 用于读取传入数据包、请求云台信息、向云台发送命令(如
-
数据结构:
Identifier
:用于表示标识符的固定大小数组的typedef
。UartCmdFunctionHandler
:用于将 UART 命令键映射到成员函数以进行消息处理的结构体。
-
实例变量:
- 各种实例变量(如
_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