Line data Source code
1 : /*
2 : Siyi gimbal driver using custom serial protocol
3 :
4 : Packet format (courtesy of Siyi's SDK document)
5 :
6 : -------------------------------------------------------------------------------------------
7 : Field Index Bytes Description
8 : -------------------------------------------------------------------------------------------
9 : STX 0 2 0x5566: starting mark
10 : CTRL 2 1 bit 0: need_ack. set if the current data packet needs ack
11 : bit 1: ack_pack. set if the current data packate IS an ack
12 : bit 2-7: reserved
13 : Data_len 3 2 Data field byte length. Low byte in the front
14 : SEQ 5 2 Frame sequence (0 ~ 65535). Low byte in the front. May be used to detect packet loss
15 : CMD_ID 7 1 Command ID
16 : DATA 8 Data_len Data
17 : CRC16 2 CRC16 check the complete data package. Low byte in the front
18 : */
19 :
20 : #pragma once
21 :
22 : #include "AP_Mount_config.h"
23 :
24 : #if HAL_MOUNT_SIYI_ENABLED
25 :
26 : #include "AP_Mount_Backend_Serial.h"
27 :
28 : #include <AP_HAL/AP_HAL.h>
29 : #include <AP_Math/AP_Math.h>
30 : #include <AP_Common/AP_Common.h>
31 :
32 : #define AP_MOUNT_SIYI_PACKETLEN_MAX 42 // maximum number of bytes in a packet sent to or received from the gimbal
33 :
34 : class AP_Mount_Siyi : public AP_Mount_Backend_Serial
35 : {
36 :
37 : public:
38 : // Constructor
39 : using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
40 :
41 : /* Do not allow copies */
42 : CLASS_NO_COPY(AP_Mount_Siyi);
43 :
44 : // update mount position - should be called periodically
45 : void update() override;
46 :
47 : // return true if healthy
48 : bool healthy() const override;
49 :
50 : // return true if this mount accepts roll targets
51 0 : bool has_roll_control() const override { return false; }
52 :
53 : // has_pan_control - returns true if this mount can control its pan (required for multicopters)
54 0 : bool has_pan_control() const override { return yaw_range_valid(); };
55 :
56 : //
57 : // camera controls
58 : //
59 :
60 : // take a picture. returns true on success
61 : bool take_picture() override;
62 :
63 : // start or stop video recording
64 : // set start_recording = true to start record, false to stop recording
65 : bool record_video(bool start_recording) override;
66 :
67 : // set zoom specified as a rate or percentage
68 : bool set_zoom(ZoomType zoom_type, float zoom_value) override;
69 :
70 : // set focus specified as rate, percentage or auto
71 : // focus in = -1, focus hold = 0, focus out = 1
72 : SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
73 :
74 : // set camera lens as a value from 0 to 8. ZT30 only
75 : bool set_lens(uint8_t lens) override;
76 :
77 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
78 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
79 : bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
80 :
81 2 : bool has_camera_information() const override { return true; }
82 : // return camera vendor name
83 2 : void get_camera_vendor_name(char *buf, uint8_t buflen) const override { strncpy(buf, "Siyi", buflen); }
84 : // return camera model name
85 2 : void get_camera_model_name(char *buf, uint8_t buflen) const override { strncpy(buf, get_model_name(), buflen); }
86 : // return camera firmware version
87 2 : uint32_t get_camera_firmware_version() const override {
88 2 : return _fw_version.camera.major | (_fw_version.camera.minor << 8) | (_fw_version.camera.patch << 16);
89 : }
90 : // return focal length in mm for the current hardware model
91 : float get_camera_focal_length_mm() const override;
92 : // return camera capability flags
93 2 : uint32_t get_camera_cap_flags() const override {
94 : return (CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
95 : CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
96 : CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM |
97 2 : CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS);
98 : }
99 :
100 : // send camera settings message to GCS
101 : void send_camera_settings(mavlink_channel_t chan) const override;
102 :
103 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
104 : // send camera thermal range message to GCS
105 : void send_camera_thermal_range(mavlink_channel_t chan) const override;
106 : #endif
107 :
108 : // change camera settings not normally used by autopilot
109 : // THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot
110 : // THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C)
111 : // THERMAL_RAW_DATA: 0:Disable Raw Data (30fps), 1:Enable Raw Data (25fps)
112 : bool change_setting(CameraSetting setting, float value) override;
113 :
114 : //
115 : // rangefinder
116 : //
117 :
118 : // get rangefinder distance. Returns true on success
119 : bool get_rangefinder_distance(float& distance_m) const override;
120 :
121 : protected:
122 :
123 : // get attitude as a quaternion. returns true on success
124 : bool get_attitude_quaternion(Quaternion& att_quat) override;
125 :
126 : // get angular velocity of mount. Only available on some backends
127 13 : bool get_angular_velocity(Vector3f& rates) override {
128 13 : rates = _current_rates_rads;
129 13 : return true;
130 : }
131 :
132 : private:
133 :
134 : // serial protocol command ids
135 : enum class SiyiCommandId {
136 : ACQUIRE_FIRMWARE_VERSION = 0x01,
137 : HARDWARE_ID = 0x02,
138 : AUTO_FOCUS = 0x04,
139 : MANUAL_ZOOM_AND_AUTO_FOCUS = 0x05,
140 : MANUAL_FOCUS = 0x06,
141 : GIMBAL_ROTATION = 0x07,
142 : CENTER = 0x08,
143 : ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A,
144 : FUNCTION_FEEDBACK_INFO = 0x0B,
145 : PHOTO = 0x0C,
146 : ACQUIRE_GIMBAL_ATTITUDE = 0x0D,
147 : ABSOLUTE_ZOOM = 0x0F,
148 : SET_CAMERA_IMAGE_TYPE = 0x11,
149 : GET_TEMP_FULL_IMAGE = 0x14,
150 : READ_RANGEFINDER = 0x15,
151 : SET_THERMAL_PALETTE = 0x1B,
152 : EXTERNAL_ATTITUDE = 0x22,
153 : SET_TIME = 0x30,
154 : SET_THERMAL_RAW_DATA = 0x34,
155 : SET_THERMAL_GAIN = 0x38,
156 : POSITION_DATA = 0x3e,
157 : };
158 :
159 : // Function Feedback Info packet info_type values
160 : enum class FunctionFeedbackInfo : uint8_t {
161 : SUCCESS = 0,
162 : FAILED_TO_TAKE_PHOTO = 1,
163 : HDR_ON = 2,
164 : HDR_OFF = 3,
165 : FAILED_TO_RECORD_VIDEO = 4
166 : };
167 :
168 : // Photo Function packet func_type values
169 : enum class PhotoFunction : uint8_t {
170 : TAKE_PICTURE = 0,
171 : HDR_TOGGLE = 1,
172 : RECORD_VIDEO_TOGGLE = 2,
173 : LOCK_MODE = 3,
174 : FOLLOW_MODE = 4,
175 : FPV_MODE = 5
176 : };
177 :
178 : // parsing state
179 : enum class ParseState : uint8_t {
180 : WAITING_FOR_HEADER_LOW,
181 : WAITING_FOR_HEADER_HIGH,
182 : WAITING_FOR_CTRL,
183 : WAITING_FOR_DATALEN_LOW,
184 : WAITING_FOR_DATALEN_HIGH,
185 : WAITING_FOR_SEQ_LOW,
186 : WAITING_FOR_SEQ_HIGH,
187 : WAITING_FOR_CMDID,
188 : WAITING_FOR_DATA,
189 : WAITING_FOR_CRC_LOW,
190 : WAITING_FOR_CRC_HIGH,
191 : };
192 :
193 : // hardware model enum
194 : enum class HardwareModel : uint8_t {
195 : UNKNOWN = 0,
196 : A2,
197 : A8,
198 : ZR10,
199 : ZR30,
200 : ZT6,
201 : ZT30
202 : } _hardware_model;
203 :
204 : enum class HdrStatus : uint8_t {
205 : OFF = 0,
206 : ON = 1,
207 : };
208 :
209 : enum class RecordingStatus : uint8_t {
210 : OFF = 0,
211 : ON = 1,
212 : NO_CARD = 2,
213 : DATA_LOSS = 3,
214 : };
215 :
216 : enum class GimbalMotionMode : uint8_t {
217 : LOCK = 0,
218 : FOLLOW = 1,
219 : FPV = 2,
220 : };
221 :
222 : enum class GimbalMountingDirection : uint8_t {
223 : UNDEFINED = 0,
224 : NORMAL = 1,
225 : UPSIDE_DOWN = 2,
226 : };
227 :
228 : enum class VideoOutputStatus : uint8_t {
229 : HDMI = 0,
230 : CVBS = 1,
231 : };
232 :
233 : // Response message for "Acquire Gimbal Confuguration Information" (0x0A)
234 : typedef struct {
235 : uint8_t _reserved1;
236 : HdrStatus hdr_status;
237 : uint8_t _reserved3;
238 : RecordingStatus record_status;
239 : GimbalMotionMode motion_mode;
240 : GimbalMountingDirection mounting_dir;
241 : VideoOutputStatus video_mode;
242 : } GimbalConfigInfo;
243 : static_assert(sizeof(GimbalConfigInfo) == 7, "GimbalConfigInfo must be 7 bytes");
244 :
245 : // camera image types (aka lens)
246 : enum class CameraImageType : uint8_t {
247 : MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE = 0,
248 : MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM = 1,
249 : MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL = 2,
250 : MAIN_ZOOM_SUB_THERMAL = 3,
251 : MAIN_ZOOM_SUB_WIDEANGLE = 4,
252 : MAIN_WIDEANGLE_SUB_THERMAL = 5,
253 : MAIN_WIDEANGLE_SUB_ZOOM = 6,
254 : MAIN_THERMAL_SUB_ZOOM = 7,
255 : MAIN_THERMAL_SUB_WIDEANGLE = 8
256 : };
257 :
258 : typedef struct {
259 : uint8_t major;
260 : uint8_t minor;
261 : uint8_t patch;
262 : } Version;
263 : typedef struct {
264 : Version camera;
265 : Version gimbal;
266 : Version zoom;
267 : bool received; // true once version information has been received
268 : } FirmwareVersion;
269 :
270 : // reading incoming packets from gimbal and confirm they are of the correct format
271 : // results are held in the _parsed_msg structure
272 : void read_incoming_packets();
273 :
274 : // process successfully decoded packets held in the _parsed_msg structure
275 : void process_packet();
276 :
277 : // send packet to gimbal
278 : // returns true on success, false if outgoing serial buffer is full
279 : bool send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len);
280 : bool send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte);
281 :
282 : // request info from gimbal
283 1 : void request_firmware_version() { send_packet(SiyiCommandId::ACQUIRE_FIRMWARE_VERSION, nullptr, 0); }
284 1 : void request_hardware_id() { send_packet(SiyiCommandId::HARDWARE_ID, nullptr, 0); }
285 2 : void request_configuration() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO, nullptr, 0); }
286 : void request_function_feedback_info() { send_packet(SiyiCommandId::FUNCTION_FEEDBACK_INFO, nullptr, 0); }
287 43 : void request_gimbal_attitude() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE, nullptr, 0); }
288 22 : void request_rangefinder_distance() { send_packet(SiyiCommandId::READ_RANGEFINDER, nullptr, 0); }
289 :
290 : // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100
291 : // yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
292 : void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef);
293 :
294 : // Set gimbal's motion mode if it has changed. Use force=true to always send.
295 : // FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
296 : // LOCK: roll, pitch and yaw are all in earth-frame
297 : // FPV: roll, pitch and yaw are all in body-frame
298 : // Returns true if message successfully sent to Gimbal
299 : bool set_motion_mode(const GimbalMotionMode mode, const bool force=false);
300 :
301 : // Siyi can send either rates or angles
302 127 : uint8_t natively_supported_mount_target_types() const override {
303 127 : return NATIVE_ANGLES_AND_RATES_ONLY;
304 : };
305 :
306 : // send target pitch and yaw rates to gimbal
307 : void send_target_rates(const MountRateTarget &rate_rads) override;
308 :
309 : // send target pitch and yaw angles to gimbal
310 : void send_target_angles(const MountAngleTarget &angle_rad) override;
311 :
312 : // send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1
313 : bool send_zoom_rate(float zoom_value);
314 :
315 : // send zoom multiple command to camera. e.g. 1x, 10x, 30x
316 : bool send_zoom_mult(float zoom_mult);
317 :
318 : // get zoom multiple max
319 : float get_zoom_mult_max() const;
320 :
321 : // update zoom controller
322 : void update_zoom_control();
323 :
324 : // get model name string, returns nullptr if hardware id is unknown
325 : const char* get_model_name() const;
326 :
327 : // Checks that the firmware version on the Gimbal meets the minimum supported version.
328 : void check_firmware_version() const;
329 :
330 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
331 : // get thermal min/max if available at 5hz
332 : void request_thermal_minmax();
333 : #endif
334 :
335 : // internal variables
336 : bool _got_hardware_id; // true once hardware id ha been received
337 :
338 : FirmwareVersion _fw_version; // firmware version (for reporting for GCS)
339 :
340 : // buffer holding bytes from latest packet. This is only used to calculate the crc
341 : uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];
342 : uint8_t _msg_buff_len;
343 : static constexpr uint8_t _msg_buff_data_start = 8; // data starts at this byte of _msg_buff
344 :
345 : // parser state and unpacked fields
346 : struct PACKED {
347 : uint16_t data_len; // expected number of data bytes
348 : uint8_t command_id; // command id
349 : uint16_t data_bytes_received; // number of data bytes received so far
350 : uint16_t crc16; // latest message's crc
351 : ParseState state; // state of incoming message processing
352 : } _parsed_msg;
353 :
354 : // variables for sending packets to gimbal
355 : uint32_t _last_send_ms; // system time (in milliseconds) of last packet sent to gimbal
356 : uint16_t _last_seq; // last sequence number used (should be increment for each send)
357 :
358 : // actual attitude received from gimbal
359 : Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
360 : Vector3f _current_rates_rads; // current angular rates in rad/s (x=roll, y=pitch, z=yaw)
361 : uint32_t _last_current_angle_rad_ms; // system time _current_angle_rad was updated
362 : uint32_t _last_req_current_angle_rad_ms; // system time that this driver last requested current angle
363 :
364 : // absolute zoom control. only used for A8 that does not support abs zoom control
365 : ZoomType _zoom_type; // current zoom type
366 : float _zoom_rate_target; // current zoom rate target
367 : float _zoom_mult; // most recent actual zoom multiple received from camera
368 : uint32_t _last_zoom_control_ms; // system time that zoom control was last run
369 :
370 : // Configuration info received from gimbal
371 : GimbalConfigInfo _config_info;
372 :
373 : // rangefinder variables
374 : uint32_t _last_rangefinder_req_ms; // system time of last request for rangefinder distance
375 : uint32_t _last_rangefinder_dist_ms; // system time of last successful read of rangefinder distance
376 : float _rangefinder_dist_m; // distance received from rangefinder
377 :
378 : // sending of attitude and position to gimbal
379 : uint32_t _last_attitude_send_ms;
380 : void send_attitude_position(void);
381 :
382 : // hardware lookup table indexed by HardwareModel enum values (see above)
383 : struct HWInfo {
384 : uint8_t hwid[2];
385 : const char* model_name;
386 : };
387 : static const HWInfo hardware_lookup_table[];
388 :
389 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
390 : // thermal variables
391 : struct {
392 : uint32_t last_req_ms; // system time of last request for thermal min/max temperatures
393 : uint32_t last_update_ms; // system time of last update of thermal min/max temperatures
394 : float max_C; // thermal max temp in C
395 : float min_C; // thermal min temp in C
396 : Vector2ui max_pos; // thermal max temp position on image in pixels. x=0 is left, y=0 is top
397 : Vector2ui min_pos; // thermal min temp position on image in pixels. x=0 is left, y=0 is top
398 : } _thermal;
399 : #endif
400 :
401 : // count of SET_TIME packets, we send 5 times to cope with packet loss
402 : uint8_t sent_time_count;
403 : };
404 :
405 : #endif // HAL_MOUNT_SIYI_ENABLED
|
