forked from ArduPilot/ardupilot
/
AP_Mount_Siyi.h
351 lines (282 loc) · 12.7 KB
/
AP_Mount_Siyi.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
/*
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.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
{
public:
// Constructor
using AP_Mount_Backend::AP_Mount_Backend;
/* Do not allow copies */
CLASS_NO_COPY(AP_Mount_Siyi);
// init - performs any required initialisation for this instance
void init() override;
// 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;
// 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
AP_HAL::UARTDriver *_uart; // uart connected to gimbal
bool _initialised; // true once the driver has been initialised
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