ArduPilot开源飞控之AP_Mount_Siyi

1. 源由

AP_Mount_Siyi主要是挂载SIYI云台，配合SIYI的产品，比如相机。

SIYI Gimbal Camera

目前，SIYI云台摄像头还是挺不错的，而且在开源社区也是蛮热门的。另外，有专业的团队进行代码维护，相对来说应用代码涉及的内容会更加全面。

2. 框架设计

继承自 AP_Mount_Backend
用于控制摄像头和万向节的类

这个类是一个复杂的云台和摄像头控制系统，提供了初始化、更新、健康检查以及各种摄像头操作（如拍照、录像、变焦、对焦等）的方法。它还定义了一些用于处理和发送数据包的私有方法和变量。

2.1 类和继承

class AP_Mount_Siyi : public AP_Mount_Backend
{
    // 使用基类构造函数
    using AP_Mount_Backend::AP_Mount_Backend;

    // 禁止拷贝构造和赋值
    CLASS_NO_COPY(AP_Mount_Siyi);

AP_Mount_Siyi 继承自 AP_Mount_Backend。
使用 using 关键字继承了基类的构造函数。
使用宏 CLASS_NO_COPY 禁止类的拷贝构造和赋值操作。

2.2 公共方法

public:
    void init() override;
    void update() override;
    bool healthy() const override;
    bool has_roll_control() const override { return false; }
    bool has_pan_control() const override { return yaw_range_valid(); }
    
    bool take_picture() override;
    bool record_video(bool start_recording) override;
    bool set_zoom(ZoomType zoom_type, float zoom_value) override;
    SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
    bool set_lens(uint8_t lens) override;
    bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
    void send_camera_information(mavlink_channel_t chan) const override;
    void send_camera_settings(mavlink_channel_t chan) const override;
    bool get_rangefinder_distance(float& distance_m) const override;

init、update、healthy 等方法是重载基类的虚函数。
has_roll_control 返回 false，表示该设备不支持滚动控制。
has_pan_control 返回 yaw_range_valid() 的结果，表示是否支持平移控制。

2.3 保护方法

protected:
    bool get_attitude_quaternion(Quaternion& att_quat) override;
    bool get_angular_velocity(Vector3f& rates) override {
        rates = _current_rates_rads;
        return true;
    }

get_attitude_quaternion 获取姿态四元数。
get_angular_velocity 获取角速度，这里直接返回 _current_rates_rads。

2.4 私有成员和方法

private:
    enum class SiyiCommandId { ... };
    enum class FunctionFeedbackInfo : uint8_t { ... };
    enum class PhotoFunction : uint8_t { ... };
    enum class ParseState : uint8_t { ... };
    enum class HardwareModel : uint8_t { ... } _hardware_model;
    enum class HdrStatus : uint8_t { OFF = 0, ON = 1 };
    enum class RecordingStatus : uint8_t { OFF = 0, ON = 1, NO_CARD = 2, DATA_LOSS = 3 };
    enum class GimbalMotionMode : uint8_t { LOCK = 0, FOLLOW = 1, FPV = 2 };
    enum class GimbalMountingDirection : uint8_t { UNDEFINED = 0, NORMAL = 1, UPSIDE_DOWN = 2 };
    enum class VideoOutputStatus : uint8_t { HDMI = 0, CVBS = 1 };
    
    typedef struct { ... } GimbalConfigInfo;
    static_assert(sizeof(GimbalConfigInfo) == 7, "GimbalConfigInfo must be 7 bytes");
    
    enum class CameraImageType : uint8_t { ... };

    typedef struct { ... } Version;
    typedef struct { ... } FirmwareVersion;

    void read_incoming_packets();
    void process_packet();
    bool send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len);
    bool send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte);
    void request_firmware_version();
    void request_hardware_id();
    void request_configuration();
    void request_function_feedback_info();
    void request_gimbal_attitude();
    void request_rangefinder_distance();
    void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef);
    bool set_motion_mode(const GimbalMotionMode mode, const bool force=false);
    void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef);
    void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef);
    bool send_zoom_rate(float zoom_value);
    bool send_zoom_mult(float zoom_mult);
    float get_zoom_mult_max() const;
    void update_zoom_control();
    const char* get_model_name() const;
    void check_firmware_version() const;
    
    AP_HAL::UARTDriver *_uart;
    bool _initialised;
    bool _got_hardware_id;
    FirmwareVersion _fw_version;
    uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];
    uint8_t _msg_buff_len;
    const uint8_t _msg_buff_data_start = 8;
    
    struct PACKED {
        uint16_t data_len;
        uint8_t command_id;
        uint16_t data_bytes_received;
        uint16_t crc16;
        ParseState state;
    } _parsed_msg;

    uint32_t _last_send_ms;
    uint16_t _last_seq;
    Vector3f _current_angle_rad;
    Vector3f _current_rates_rads;
    uint32_t _last_current_angle_rad_ms;
    uint32_t _last_req_current_angle_rad_ms;
    ZoomType _zoom_type;
    float _zoom_rate_target;
    float _zoom_mult;
    uint32_t _last_zoom_control_ms;
    GimbalConfigInfo _config_info;
    uint32_t _last_rangefinder_req_ms;
    uint32_t _last_rangefinder_dist_ms;
    float _rangefinder_dist_m;
    uint32_t _last_attitude_send_ms;

    void send_attitude(void);

    struct HWInfo {
        uint8_t hwid[2];
        const char* model_name;
    };
    static const HWInfo hardware_lookup_table[];
    
    uint8_t sent_time_count;
};

枚举 SiyiCommandId、FunctionFeedbackInfo 等定义了各种命令和状态。
GimbalConfigInfo 结构体存储云台配置信息，并用 static_assert 确保其大小为 7 字节。
各种私有方法用于发送、处理和读取数据包，管理云台的运动模式和摄像头的控制。

2.5 解析状态

enum class ParseState : uint8_t {
    WAITING_FOR_HEADER_LOW,
    WAITING_FOR_HEADER_HIGH,
    WAITING_FOR_CTRL,
    WAITING_FOR_DATALEN_LOW,
    WAITING_FOR_DATALEN_HIGH,
    WAITING_FOR_SEQ_LOW,
    WAITING_FOR_SEQ_HIGH,
    WAITING_FOR_CMDID,
    WAITING_FOR_DATA,
    WAITING_FOR_CRC_LOW,
    WAITING_FOR_CRC_HIGH,
};

ParseState 枚举表示数据包解析的不同状态。

2.6 重要成员变量

_uart：指向用于通信的 UART 驱动程序。
_initialised：标志驱动程序是否已初始化。
_got_hardware_id：标志是否已收到硬件 ID。
_fw_version：固件版本信息。
_msg_buff 和 _msg_buff_len：用于存储消息缓冲区和其长度。
_parsed_msg：结构体，用于存储解析消息的状态。
_current_angle_rad 和 _current_rates_rads：存储从云台接收到的当前角度和角速度。

3. 重要方法

3.1 AP_Mount_Siyi::init

该初始化例程在AP_Mount::init中调用。

AP_Mount_Siyi::init()
├── 定义 `serial_manager` 变量，并获取 `AP_SerialManager` 的实例
│   ├── `const AP_SerialManager& serial_manager = AP::serialmanager();`
├── 通过 `serial_manager` 查找串口并赋值给 `_uart`
│   ├── `_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Gimbal, 0);`
│   ├── 检查 `_uart` 是否为 `nullptr`
│       ├── 如果是 `nullptr`
│       │   ├── `return;`
├── 设置 `_initialised` 为 `true`
│   ├── `_initialised = true;`
├── 调用基类的 `init()` 方法
    ├── `AP_Mount_Backend::init();`

3.2 AP_Mount_Siyi::update

该定时更新例程在AP_Mount::update中调用。

AP_Mount_Siyi::update
├── 检查是否初始化 _initialised
│   ├── 如果未初始化，立即退出
├── 读取云台的传入数据包 read_incoming_packets()
├── 获取当前时间 now_ms
├── 定期请求固件版本或配置
│   ├── 如果自上次发送以来已过1000毫秒
│   │   ├── 更新最后发送时间 _last_send_ms
│   │   ├── 如果未获取硬件ID，则请求硬件ID request_hardware_id()
│   │   ├── 否则如果固件版本未收到，则请求固件版本 request_firmware_version()
│   │   ├── 否则请求配置 request_configuration()
│   │   └── 发送UTC时间给摄像头 (AP_RTC_ENABLED)
│       └── 如果已发送时间计数 sent_time_count 小于 5
│           ├── 获取UTC时间
│           └── 发送时间数据包 send_packet()
├── 定期请求姿态 request_gimbal_attitude()
│   ├── 如果自上次请求姿态以来已过50毫秒
│       ├── 请求云台姿态
│       └── 更新最后请求姿态时间 _last_req_current_angle_rad_ms
├── 定期请求测距仪距离 request_rangefinder_distance()
│   ├── 如果硬件型号为 ZT30 且自上次请求以来已过100毫秒
│       └── 请求测距仪距离
│       └── 更新最后请求时间 _last_rangefinder_req_ms
├── 定期发送姿态 send_attitude()
│   ├── 如果自上次发送姿态以来已过100毫秒
│       ├── 更新最后发送姿态时间 _last_attitude_send_ms
│       └── 发送姿态
├── 运行变焦控制 update_zoom_control()
├── RC输入改变时切换到 RC_TARGETING 模式 set_rctargeting_on_rcinput_change()
├── 获取当前云台模式 get_mode()
│   ├── 根据模式设定目标角度或速率
│       ├── MAV_MOUNT_MODE_RETRACT 模式
│           ├── 获取收回角度 _params.retract_angles
│           ├── 设置目标类型为角度
│           └── 将角度从度转换为弧度并设置
│       ├── MAV_MOUNT_MODE_NEUTRAL 模式
│           ├── 获取中立角度 _params.neutral_angles
│           ├── 设置目标类型为角度
│           └── 将角度从度转换为弧度并设置
│       ├── MAV_MOUNT_MODE_MAVLINK_TARGETING 模式
│           └── 处理传入消息时存储MAVLINK目标
│       ├── MAV_MOUNT_MODE_RC_TARGETING 模式
│           ├── 使用飞行员的RC输入更新目标
│           ├── 获取RC目标 get_rc_target()
│           └── 根据目标类型更新角度或速率
│       ├── MAV_MOUNT_MODE_GPS_POINT 模式
│           ├── 获取指向ROI的角度目标 get_angle_target_to_roi()
│           └── 设置目标类型为角度
│       ├── MAV_MOUNT_MODE_HOME_LOCATION 模式
│           ├── 获取指向Home位置的角度目标 get_angle_target_to_home()
│           └── 设置目标类型为角度
│       ├── MAV_MOUNT_MODE_SYSID_TARGET 模式
│           ├── 获取指向其他车辆的角度目标 get_angle_target_to_sysid()
│           └── 设置目标类型为角度
│       └── 其他模式
│           └── 引发内部错误 INTERNAL_ERROR()
├── 根据目标类型发送目标角度或速率
│   ├── 目标类型为角度
│       └── 发送目标角度 send_target_angles()
│   └── 目标类型为速率
│       └── 发送目标速率 send_target_rates()

3.3 AP_Mount_Siyi::read_incoming_packets

AP_Mount_Siyi::update时，调用该函数，对SIYI云台串口数据报文进行解析。

AP_Mount_Siyi::read_incoming_packets
|
|-- 检查串口接口上是否有数据可读
|   |-- 获取可读的字节数 `nbytes`
|   |-- 如果没有字节可读，则返回
|
|-- 初始化重置解析器状态的标志 `reset_parser`
|
|-- 遍历接收到的每个字节
|   |-- 读取一个字节 `b`
|   |-- 将字节添加到消息缓冲区 `_msg_buff`
|   |-- 检查消息是否过长，如果过长则重置解析器状态
|
|   |-- 根据当前解析状态处理字节
|       |-- `WAITING_FOR_HEADER_LOW`
|           |-- 如果字节为头部低字节 `AP_MOUNT_SIYI_HEADER1`，则转到 `WAITING_FOR_HEADER_HIGH` 状态
|           |-- 否则，重置解析器状态
|
|       |-- `WAITING_FOR_HEADER_HIGH`
|           |-- 如果字节为头部高字节 `AP_MOUNT_SIYI_HEADER2`，则转到 `WAITING_FOR_CTRL` 状态
|           |-- 否则，重置解析器状态
|
|       |-- `WAITING_FOR_CTRL`
|           |-- 转到 `WAITING_FOR_DATALEN_LOW` 状态
|
|       |-- `WAITING_FOR_DATALEN_LOW`
|           |-- 保存数据长度低字节，转到 `WAITING_FOR_DATALEN_HIGH` 状态
|
|       |-- `WAITING_FOR_DATALEN_HIGH`
|           |-- 保存数据长度高字节，并进行数据长度合理性检查
|           |-- 如果数据长度合理，转到 `WAITING_FOR_SEQ_LOW` 状态
|           |-- 否则，重置解析器状态并输出调试信息
|
|       |-- `WAITING_FOR_SEQ_LOW`
|           |-- 转到 `WAITING_FOR_SEQ_HIGH` 状态
|
|       |-- `WAITING_FOR_SEQ_HIGH`
|           |-- 转到 `WAITING_FOR_CMDID` 状态
|
|       |-- `WAITING_FOR_CMDID`
|           |-- 保存命令ID，如果有数据则转到 `WAITING_FOR_DATA` 状态，否则转到 `WAITING_FOR_CRC_LOW` 状态
|
|       |-- `WAITING_FOR_DATA`
|           |-- 接收数据字节，如果所有数据接收完毕，则转到 `WAITING_FOR_CRC_LOW` 状态
|
|       |-- `WAITING_FOR_CRC_LOW`
|           |-- 保存CRC低字节，转到 `WAITING_FOR_CRC_HIGH` 状态
|
|       |-- `WAITING_FOR_CRC_HIGH`
|           |-- 保存CRC高字节，进行CRC校验
|           |-- 如果CRC校验成功，则处理接收到的封包 `process_packet()`
|           |-- 否则，输出调试信息
|           |-- 重置解析器状态
|
|-- 重置解析器状态
|   |-- 重置解析状态为 `WAITING_FOR_HEADER_LOW`
|   |-- 清空消息缓冲区长度 `_msg_buff_len`
|   |-- 重置 `reset_parser` 标志

3.4 AP_Mount_Siyi::process_packet

云台摄像机报文处理，支持以下命令：

SiyiCommandId::ACQUIRE_FIRMWARE_VERSION(0x01)
SiyiCommandId::HARDWARE_ID(0x02)
SiyiCommandId::AUTO_FOCUS(0x04)
SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS(0x05)
SiyiCommandId::MANUAL_FOCUS(0x06)
SiyiCommandId::GIMBAL_ROTATION(0x07)
SiyiCommandId::CENTER(0x08)
SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO(0x0A)
SiyiCommandId::FUNCTION_FEEDBACK_INFO(0x0B)
SiyiCommandId::PHOTO(0x0C)
SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE(0x0D)
SiyiCommandId::READ_RANGEFINDER(0x15)

AP_Mount_Siyi::process_packet
└── switch ((SiyiCommandId)_parsed_msg.command_id)
    ├── case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION
    │   ├── 检查 _parsed_msg.data_bytes_received 是否为 12 或 8
    │   ├── 解析并显示固件版本
    │   ├── 检查摄像机固件版本是否全为零
    │   ├── 设置 _fw_version.received 为 true
    │   ├── 显示摄像机和云台固件版本
    │   ├── 显示变焦固件版本（如果存在）
    │   └── 检查固件版本是否为最新
    ├── case SiyiCommandId::HARDWARE_ID
    │   ├── 查找硬件 ID 的前两位
    │   ├── 设置 _hardware_model
    │   └── 设置 _got_hardware_id 为 true
    ├── case SiyiCommandId::AUTO_FOCUS
    │   └── #if AP_MOUNT_SIYI_DEBUG
    │       ├── 检查 _parsed_msg.data_bytes_received 是否为 1
    │       └── 调试输出自动对焦信息
    ├── case SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS
    │   ├── 检查 _parsed_msg.data_bytes_received 是否为 2
    │   └── 解析变焦倍数并调试输出
    ├── case SiyiCommandId::MANUAL_FOCUS
    │   └── #if AP_MOUNT_SIYI_DEBUG
    │       ├── 检查 _parsed_msg.data_bytes_received 是否为 1
    │       └── 调试输出手动对焦信息
    ├── case SiyiCommandId::GIMBAL_ROTATION
    │   └── #if AP_MOUNT_SIYI_DEBUG
    │       ├── 检查 _parsed_msg.data_bytes_received 是否为 1
    │       └── 调试输出云台旋转信息
    ├── case SiyiCommandId::CENTER
    │   └── #if AP_MOUNT_SIYI_DEBUG
    │       ├── 检查 _parsed_msg.data_bytes_received 是否为 1
    │       └── 调试输出居中信息
    ├── case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO
    │   ├── 保存之前的录制状态
    │   ├── 更新云台配置信息
    │   ├── 如果录制状态变化，向用户警报
    │   └── 调试输出云台配置信息
    ├── case SiyiCommandId::FUNCTION_FEEDBACK_INFO
    │   ├── 检查 _parsed_msg.data_bytes_received 是否为 1
    │   ├── 解析功能反馈信息
    │   └── 根据反馈信息进行相应操作
    ├── case SiyiCommandId::PHOTO
    │   └── 不执行任何操作
    ├── case SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE
    │   ├── 检查 _parsed_msg.data_bytes_received 是否为 12
    │   ├── 更新当前角度和速率
    │   └── 解析并转换角度和速率信息
    ├── case SiyiCommandId::READ_RANGEFINDER
    │   ├── 解析测距仪距离
    │   └── 更新最后一次测距时间
    └── default
        └── 调试输出未处理的命令 ID

#if AP_MOUNT_SIYI_DEBUG
└── 如果存在意外的数据长度
    └── 调试输出意外长度信息

3.5 AP_Mount_Siyi::update_zoom_control

对摄像头使用SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS进行光学调焦。

AP_Mount_Siyi::update_zoom_control
└── if (_zoom_type == ZoomType::RATE)
    ├── const uint32_t now_ms = AP_HAL::millis();
    ├── if (now_ms - _last_zoom_control_ms < 1000)
    │   └── return;
    ├── _last_zoom_control_ms = now_ms;
    ├── if (!is_zero(_zoom_rate_target))
    │   └── send_zoom_rate(_zoom_rate_target);
    └── // 如果 _zoom_rate_target 是零，则不会发送缩放率，因为这会触发自动对焦

3.6 AP_Mount_Backend::set_rctargeting_on_rcinput_change

当RC控制配置辅助功能RC_Channel::AUX_FUNC::MOUNT1_ROLL、RC_Channel::AUX_FUNC::MOUNT1_PITCH、RC_Channel::AUX_FUNC::MOUNT1_YAW时，可以直接通过RC功能键进行手动控制。

AP_Mount_Backend::set_rctargeting_on_rcinput_change
├── 如果 (!rc().has_valid_input())
│   └── 返回;
├── roll_ch = rc().find_channel_for_option(...)
├── pitch_ch = rc().find_channel_for_option(...)
├── yaw_ch = rc().find_channel_for_option(...)
├── 如果 (!last_rc_input.initialised)
│   └── 初始化 last_rc_input
└── 如果 (get_mode() != MAV_MOUNT_MODE_RC_TARGETING && get_mode() != MAV_MOUNT_MODE_RETRACT)
    ├── roll_dz = MAX(roll_ch->get_dead_zone(), 10)
    ├── pitch_dz = MAX(pitch_ch->get_dead_zone(), 10)
    ├── yaw_dz = MAX(yaw_ch->get_dead_zone(), 10)
    └── 如果 (遥控器输入发生变化条件)
        └── 设置模式为 MAV_MOUNT_MODE_RC_TARGETING
└── 如果 (get_mode() == MAV_MOUNT_MODE_RC_TARGETING || get_mode() == MAV_MOUNT_MODE_RETRACT)
    └── 更新 last_rc_input

4. 辅助函数

4.1 基本能力

检查SIYI云台摄像头：

健康状态(无串口通信，认为设备不健康)
是否具备roll方向旋转能力
是否具备yaw方向旋转能力

注：ZT30具备3维光学吊舱；A2 mini仅具有Pitch。

    // return true if healthy
    bool healthy() const override;

    // return true if this mount accepts roll targets
    bool has_roll_control() const override { return false; }

    // has_pan_control - returns true if this mount can control its pan (required for multicopters)
    bool has_pan_control() const override { return yaw_range_valid(); };

    // returns true if user has configured a valid yaw angle range
    // allows user to disable yaw even on 3-axis gimbal
    bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }

4.2 摄像头&红外测距功能

摄像头

    //
    // camera controls
    //

    // take a picture.  returns true on success
    bool take_picture() override;

    // start or stop video recording
    // set start_recording = true to start record, false to stop recording
    bool record_video(bool start_recording) override;

    // set zoom specified as a rate or percentage
    bool set_zoom(ZoomType zoom_type, float zoom_value) override;

    // set focus specified as rate, percentage or auto
    // focus in = -1, focus hold = 0, focus out = 1
    SetFocusResult set_focus(FocusType focus_type, float focus_value) override;

    // set camera lens as a value from 0 to 8.  ZT30 only
    bool set_lens(uint8_t lens) override;

    // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
    // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
    bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;

    // send camera information message to GCS
    void send_camera_information(mavlink_channel_t chan) const override;

    // send camera settings message to GCS
    void send_camera_settings(mavlink_channel_t chan) const override;

红外测距

    //
    // rangefinder
    //

    // get rangefinder distance.  Returns true on success
    bool get_rangefinder_distance(float& distance_m) const override;

4.3 云台操作

云台控制相关操作&功能

    // send packet to gimbal
    // returns true on success, false if outgoing serial buffer is full
    bool send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len);
    bool send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte);

    // request info from gimbal
    void request_firmware_version() { send_packet(SiyiCommandId::ACQUIRE_FIRMWARE_VERSION, nullptr, 0); }
    void request_hardware_id() { send_packet(SiyiCommandId::HARDWARE_ID, nullptr, 0); }
    void request_configuration() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO, nullptr, 0); }
    void request_function_feedback_info() { send_packet(SiyiCommandId::FUNCTION_FEEDBACK_INFO, nullptr, 0); }
    void request_gimbal_attitude() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE, nullptr, 0); }
    void request_rangefinder_distance() { send_packet(SiyiCommandId::READ_RANGEFINDER, nullptr, 0); }

    // rotate gimbal.  pitch_rate and yaw_rate are scalars in the range -100 ~ +100
    // yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
    void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef);

    // Set gimbal's motion mode if it has changed. Use force=true to always send.
    //   FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
    //   LOCK: roll, pitch and yaw are all in earth-frame
    //   FPV: roll, pitch and yaw are all in body-frame
    // Returns true if message successfully sent to Gimbal
    bool set_motion_mode(const GimbalMotionMode mode, const bool force=false);

    // send target pitch and yaw rates to gimbal
    // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame
    void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef);

    // send target pitch and yaw angles to gimbal
    // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame
    void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef);

    // send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1
    bool send_zoom_rate(float zoom_value);

    // send zoom multiple command to camera. e.g. 1x, 10x, 30x
    bool send_zoom_mult(float zoom_mult);

    // get zoom multiple max
    float get_zoom_mult_max() const;

    // update zoom controller
    void update_zoom_control();

    // get model name string, returns nullptr if hardware id is unknown
    const char* get_model_name() const;

    // Checks that the firmware version on the Gimbal meets the minimum supported version.
    void check_firmware_version() const;

4. 总结

AP_Mount_Siyi是在云台基础上集成了摄像头、红外测距等诸多功能的复合设备。

云台摄像头串口接飞控
AI模块串口接二次开发设备

其中云台摄像头串口接飞控，走的就是本文所描述的协议。

在这里插入图片描述

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

6. 补充资料 - SIYI AI Object Tracking

注：关于SIYI AI模块，详见：SIYI AI模块规格书。其两个UART串口其中一个可以用于二次开发，另一个不知道是干什么用的？？？目前，得到的反馈是保留。

从AI Object Tracking的逻辑：

GroundControlStation选择识别区域
MAVLink发送给FlightController
FlightController透传该区域给CompanionComputer
CompanionComputer区域内识别目标（识别失败，到第4步结束）
4.1 CompanionComputer反馈执行结果给FlightController
4.2 FlightController透传给GroundControlStation
【识别成功】CompanionComputer绘制OSD框选（GCS地面站显示）
CompanionComputer执行算法进行pitch/roll/yaw操作，进行跟踪
遇到异常（目标丢失），停止跟踪

SIYI AI Object Tracking由以下组件组成：

SIYI AirUnit
SIYI AI Module
SIYI Gimbal Camera

MAVLink链路是GCS/AirUnit/FC组成的
AP_Camera/AP_Mount是集成的
Camera/AirUnit/AI Module都是以太网网络通信

鉴于SIYI Gimbal Camera/SIYI AI Module/SIYI AirUnit闭源代码设计，其整个过程内部处理逻辑可能存在一定的差异，不建议深入。

后续我们将寻找一个更为Open的代码进行相关设计研读。