ardupilot/libraries/AP_Mount/AP_Mount_Siyi.h

/*
  Siyi gimbal driver using custom serial protocol

  Packet format (courtesy of Siyi's SDK document)

  -------------------------------------------------------------------------------------------
  Field     Index   Bytes       Description
  -------------------------------------------------------------------------------------------
  STX       0       2           0x5566: starting mark
  CTRL      2       1           bit 0: need_ack.  set if the current data packet needs ack
                                bit 1: ack_pack.  set if the current data packate IS an ack
                                bit 2-7: reserved
  Data_len  3       2           Data field byte length.  Low byte in the front
  SEQ       5       2           Frame sequence (0 ~ 65535).  Low byte in the front.  May be used to detect packet loss
  CMD_ID    7       1           Command ID
  DATA      8       Data_len    Data
  CRC16             2           CRC16 check the complete data package.  Low byte in the front
 */

#pragma once

#include "AP_Mount_Backend_Serial.h"

#if HAL_MOUNT_SIYI_ENABLED

#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>

#define AP_MOUNT_SIYI_PACKETLEN_MAX     38  // maximum number of bytes in a packet sent to or received from the gimbal

class AP_Mount_Siyi : public AP_Mount_Backend_Serial
{

public:
    // Constructor
    using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;

    /* Do not allow copies */
    CLASS_NO_COPY(AP_Mount_Siyi);

    // update mount position - should be called periodically
    void update() override;

    // 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(); };

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

protected:

    // get attitude as a quaternion.  returns true on success
    bool get_attitude_quaternion(Quaternion& att_quat) override;

    // get angular velocity of mount. Only available on some backends
    bool get_angular_velocity(Vector3f& rates) override {
        rates = _current_rates_rads;
        return true;
    }
    
private:

    // serial protocol command ids
    enum class SiyiCommandId {
        ACQUIRE_FIRMWARE_VERSION = 0x01,
        HARDWARE_ID = 0x02,
        AUTO_FOCUS = 0x04,
        MANUAL_ZOOM_AND_AUTO_FOCUS = 0x05,
        MANUAL_FOCUS = 0x06,
        GIMBAL_ROTATION = 0x07,
        CENTER = 0x08,
        ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A,
        FUNCTION_FEEDBACK_INFO = 0x0B,
        PHOTO = 0x0C,
        ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
        ABSOLUTE_ZOOM = 0x0F,
        SET_CAMERA_IMAGE_TYPE = 0x11,
        READ_RANGEFINDER = 0x15,
        EXTERNAL_ATTITUDE = 0x22,
        SET_TIME = 0x30,
    };

    // Function Feedback Info packet info_type values
    enum class FunctionFeedbackInfo : uint8_t {
        SUCCESS = 0,
        FAILED_TO_TAKE_PHOTO = 1,
        HDR_ON = 2,
        HDR_OFF = 3,
        FAILED_TO_RECORD_VIDEO = 4
    };

    // Photo Function packet func_type values
    enum class PhotoFunction : uint8_t {
        TAKE_PICTURE = 0,
        HDR_TOGGLE = 1,
        RECORD_VIDEO_TOGGLE = 2,
        LOCK_MODE = 3,
        FOLLOW_MODE = 4,
        FPV_MODE = 5
    };

    // parsing state
    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,
    };

    // hardware model enum
    enum class HardwareModel : uint8_t {
        UNKNOWN = 0,
        A2,
        A8,
        ZR10,
        ZR30,
        ZT30
    } _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,
    };

    // Response message for "Acquire Gimbal Confuguration Information" (0x0A)
    typedef struct {
        uint8_t _reserved1;
        HdrStatus hdr_status;
        uint8_t _reserved3;
        RecordingStatus record_status;
        GimbalMotionMode motion_mode;
        GimbalMountingDirection mounting_dir;
        VideoOutputStatus video_mode;
    } GimbalConfigInfo;
    static_assert(sizeof(GimbalConfigInfo) == 7, "GimbalConfigInfo must be 7 bytes");

    // camera image types (aka lens)
    enum class CameraImageType : uint8_t {
        MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE = 0,
        MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM = 1,
        MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL = 2,
        MAIN_ZOOM_SUB_THERMAL = 3,
        MAIN_ZOOM_SUB_WIDEANGLE = 4,
        MAIN_WIDEANGLE_SUB_THERMAL = 5,
        MAIN_WIDEANGLE_SUB_ZOOM = 6,
        MAIN_THERMAL_SUB_ZOOM = 7,
        MAIN_THERMAL_SUB_WIDEANGLE = 8
    };

    typedef struct {
        uint8_t major;
        uint8_t minor;
        uint8_t patch;
    } Version;
    typedef struct {
        Version camera;
        Version gimbal;
        Version zoom;
        bool received; // true once version information has been received
    } FirmwareVersion;

    // reading incoming packets from gimbal and confirm they are of the correct format
    // results are held in the _parsed_msg structure
    void read_incoming_packets();

    // process successfully decoded packets held in the _parsed_msg structure
    void process_packet();

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

    // internal variables
    bool _got_hardware_id;                          // true once hardware id ha been received

    FirmwareVersion _fw_version;                    // firmware version (for reporting for GCS)

    // buffer holding bytes from latest packet.  This is only used to calculate the crc
    uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];
    uint8_t _msg_buff_len;
    const uint8_t _msg_buff_data_start = 8;         // data starts at this byte of _msg_buff

    // parser state and unpacked fields
    struct PACKED {
        uint16_t data_len;                          // expected number of data bytes
        uint8_t command_id;                         // command id
        uint16_t data_bytes_received;               // number of data bytes received so far
        uint16_t crc16;                             // latest message's crc
        ParseState state;                           // state of incoming message processing
    } _parsed_msg;

    // variables for sending packets to gimbal
    uint32_t _last_send_ms;                         // system time (in milliseconds) of last packet sent to gimbal
    uint16_t _last_seq;                             // last sequence number used (should be increment for each send)

    // actual attitude received from gimbal
    Vector3f _current_angle_rad;                    // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
    Vector3f _current_rates_rads;                   // current angular rates in rad/s (x=roll, y=pitch, z=yaw)
    uint32_t _last_current_angle_rad_ms;            // system time _current_angle_rad was updated
    uint32_t _last_req_current_angle_rad_ms;        // system time that this driver last requested current angle

    // absolute zoom control.  only used for A8 that does not support abs zoom control
    ZoomType _zoom_type;                            // current zoom type
    float _zoom_rate_target;                        // current zoom rate target
    float _zoom_mult;                               // most recent actual zoom multiple received from camera
    uint32_t _last_zoom_control_ms;                 // system time that zoom control was last run

    // Configuration info received from gimbal
    GimbalConfigInfo _config_info;
    
    // rangefinder variables
    uint32_t _last_rangefinder_req_ms;              // system time of last request for rangefinder distance
    uint32_t _last_rangefinder_dist_ms;             // system time of last successful read of rangefinder distance
    float _rangefinder_dist_m;                      // distance received from rangefinder

    // sending of attitude to gimbal
    uint32_t _last_attitude_send_ms;
    void send_attitude(void);

    // hardware lookup table indexed by HardwareModel enum values (see above)
    struct HWInfo {
        uint8_t hwid[2];
        const char* model_name;
    };
    static const HWInfo hardware_lookup_table[];

    // count of SET_TIME packets, we send 5 times to cope with packet loss
    uint8_t sent_time_count;
};

#endif // HAL_MOUNT_SIYISERIAL_ENABLED