mirror of https://github.com/ArduPilot/ardupilot
387 lines
14 KiB
C++
387 lines
14 KiB
C++
/*
|
|
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_config.h"
|
|
|
|
#if HAL_MOUNT_SIYI_ENABLED
|
|
|
|
#include "AP_Mount_Backend_Serial.h"
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#define AP_MOUNT_SIYI_PACKETLEN_MAX 42 // 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;
|
|
|
|
#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
|
|
// send camera thermal range message to GCS
|
|
void send_camera_thermal_range(mavlink_channel_t chan) const override;
|
|
#endif
|
|
|
|
// change camera settings not normally used by autopilot
|
|
// THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot
|
|
// THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C)
|
|
// THERMAL_RAW_DATA: 0:Disable Raw Data (30fps), 1:Enable Raw Data (25fps)
|
|
bool change_setting(CameraSetting setting, float value) 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,
|
|
GET_TEMP_FULL_IMAGE = 0x14,
|
|
READ_RANGEFINDER = 0x15,
|
|
SET_THERMAL_PALETTE = 0x1B,
|
|
EXTERNAL_ATTITUDE = 0x22,
|
|
SET_TIME = 0x30,
|
|
SET_THERMAL_RAW_DATA = 0x34,
|
|
SET_THERMAL_GAIN = 0x38,
|
|
POSITION_DATA = 0x3e,
|
|
};
|
|
|
|
// 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,
|
|
ZT6,
|
|
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;
|
|
|
|
#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
|
|
// get thermal min/max if available at 5hz
|
|
void request_thermal_minmax();
|
|
#endif
|
|
|
|
// 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 and position to gimbal
|
|
uint32_t _last_attitude_send_ms;
|
|
void send_attitude_position(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[];
|
|
|
|
#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
|
|
// thermal variables
|
|
struct {
|
|
uint32_t last_req_ms; // system time of last request for thermal min/max temperatures
|
|
uint32_t last_update_ms; // system time of last update of thermal min/max temperatures
|
|
float max_C; // thermal max temp in C
|
|
float min_C; // thermal min temp in C
|
|
Vector2ui max_pos; // thermal max temp position on image in pixels. x=0 is left, y=0 is top
|
|
Vector2ui min_pos; // thermal min temp position on image in pixels. x=0 is left, y=0 is top
|
|
} _thermal;
|
|
#endif
|
|
|
|
// count of SET_TIME packets, we send 5 times to cope with packet loss
|
|
uint8_t sent_time_count;
|
|
};
|
|
|
|
#endif // HAL_MOUNT_SIYI_ENABLED
|