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;
    }

    // 示例命令: #TPUD2wCAP01
    return 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;
    }

    // 示例命令: #TPUD2wREC01
    return 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;
        }
        // 示例命令: #TPUM2wZMC00
        return 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;
        }
        // 发送对焦命令并切换到手动对焦
        // 示例命令: #TPUM2wFCC00
        if (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: 下一个
    // 示例命令: #TPUD2wPIP0A
    if (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
    // 示例命令：#TPUM2wLRF00
    return 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:下一个
    // 示例命令：#TPUD2wPIP0A
    uint8_t pip_setting = 0;
    switch (primary_source) {
    case 0: // 默认（RGB）
        FALLTHROUGH;  // 继续执行下一个case
    case 1: // RGB
        switch (secondary_source) {
        case 0: // RGB + 默认（无）
            pip_setting = 0;    // 仅主
            break;
        case 2: // PIP RGB+IR
            pip_setting = 1;    // 主+次
            break;
        default:
            return false;
        }
        break;
    case 2: // IR
        switch (secondary_source) {
        case 0: // IR + 默认（无）
            pip_setting = 3;    // 仅次
            break;
        case 1: // IR+RGB
            pip_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_t
        0,                      // 焦距 float (mm)
        0,                      // 水平传感器尺寸 float (mm)
        0,                      // 垂直传感器尺寸 float (mm)
        0,                      // 水平分辨率 uint16_t (像素)
        0,                      // 垂直分辨率 uint16_t (像素)
        0,                      // 镜头 ID uint8_t
        flags,                  // 标志 uint32_t (相机能力标志)
        0,                      // 相机定义版本 uint16_t
        cam_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，如果未知则为 NaN
        NaN);               // 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