Line data Source code
1 : /*
2 : Viewpro gimbal driver using custom serial protocol
3 :
4 : Packet format (courtesy of Viewpro's SDK document)
5 :
6 : -------------------------------------------------------------------------------------------
7 : Field Index Bytes Description
8 : -------------------------------------------------------------------------------------------
9 : Header 0~2 3 0x55 0xAA 0xDC
10 : Length 3 1 bit0~5: body length, n=all bytes from byte3 to checksum, min=4, max=63
11 : bits6~7: frame counter (increment by 1 compared to previous message sent)
12 : Frame Id 4 1
13 : Data 5~n+1 n 1st byte is command id (?)
14 : Checksum n+2 1 XOR of byte3 to n+1 (inclusive)
15 : */
16 :
17 : #pragma once
18 :
19 : #include "AP_Mount_config.h"
20 :
21 : #if HAL_MOUNT_VIEWPRO_ENABLED
22 :
23 : #include "AP_Mount_Backend_Serial.h"
24 :
25 : #include <AP_HAL/AP_HAL.h>
26 : #include <AP_Math/AP_Math.h>
27 : #include <AP_Common/AP_Common.h>
28 : #include <AP_HAL/utility/sparse-endian.h>
29 :
30 : #define AP_MOUNT_VIEWPRO_PACKETLEN_MAX 63 // maximum number of bytes in a packet sent to or received from the gimbal
31 :
32 : class AP_Mount_Viewpro : public AP_Mount_Backend_Serial
33 : {
34 :
35 : public:
36 : // Constructor
37 : using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
38 :
39 : /* Do not allow copies */
40 : CLASS_NO_COPY(AP_Mount_Viewpro);
41 :
42 : // update mount position - should be called periodically
43 : void update() override;
44 :
45 : // return true if healthy
46 : bool healthy() const override;
47 :
48 : // return true if this mount accepts roll targets
49 0 : bool has_roll_control() const override { return false; }
50 :
51 : // has_pan_control - returns true if this mount can control its pan (required for multicopters)
52 6 : bool has_pan_control() const override { return yaw_range_valid(); };
53 :
54 : //
55 : // camera controls
56 : //
57 :
58 : // take a picture. returns true on success
59 : bool take_picture() override;
60 :
61 : // start or stop video recording
62 : // set start_recording = true to start record, false to stop recording
63 : bool record_video(bool start_recording) override;
64 :
65 : // set zoom specified as a rate or percentage
66 : bool set_zoom(ZoomType zoom_type, float zoom_value) override;
67 :
68 : // set focus specified as rate, percentage or auto
69 : // focus in = -1, focus hold = 0, focus out = 1
70 : SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
71 :
72 : // set tracking to none, point or rectangle (see TrackingType enum)
73 : // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
74 : // p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
75 : bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) override;
76 :
77 : // set camera lens as a value from 0 to 5
78 : bool set_lens(uint8_t lens) override;
79 :
80 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
81 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
82 : bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
83 :
84 1 : bool has_camera_information() const override { return true; }
85 : // return camera vendor name
86 1 : void get_camera_vendor_name(char *buf, uint8_t buflen) const override { strncpy(buf, "Viewpro", buflen); }
87 : // return camera model name
88 1 : void get_camera_model_name(char *buf, uint8_t buflen) const override {
89 1 : if (!_got_model_name) {
90 0 : return;
91 : }
92 1 : strncpy(buf, _model_name, buflen);
93 : }
94 : // return camera firmware version
95 1 : uint32_t get_camera_firmware_version() const override { return _firmware_version; }
96 : // return current camera lens ID
97 1 : uint8_t get_camera_lens_id() const override { return (uint8_t)_image_sensor; }
98 : // return camera capability flags
99 1 : uint32_t get_camera_cap_flags() const override {
100 : return (CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
101 : CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
102 : CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM |
103 : CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS |
104 : CAMERA_CAP_FLAGS_HAS_TRACKING_POINT |
105 1 : CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE);
106 : }
107 :
108 : // send camera settings message to GCS
109 : void send_camera_settings(mavlink_channel_t chan) const override;
110 :
111 : //
112 : // rangefinder
113 : //
114 :
115 : // get rangefinder distance. Returns true on success
116 : bool get_rangefinder_distance(float& distance_m) const override;
117 :
118 : // enable/disable rangefinder. Returns true on success
119 : bool set_rangefinder_enable(bool enable) override;
120 :
121 : protected:
122 :
123 : // Viewpro can send either rates or angles
124 4703 : uint8_t natively_supported_mount_target_types() const override {
125 4703 : return NATIVE_ANGLES_AND_RATES_ONLY;
126 : };
127 :
128 : // get attitude as a quaternion. returns true on success
129 : bool get_attitude_quaternion(Quaternion& att_quat) override;
130 :
131 : private:
132 :
133 : // send text prefix string to reduce flash cost
134 : static const char* send_text_prefix;
135 :
136 : // packet frame ids
137 : enum class FrameId : uint8_t {
138 : HANDSHAKE = 0x00, // handshake sent to gimbal
139 : U = 0x01, // communication configuration control (this packet is sent to gimbal)
140 : V = 0x02, // communication configuration status (this is the reply to U)
141 : HEARTBEAT = 0x10, // heartbeat received from gimbal
142 : A1 = 0x1A, // target angles (sent)
143 : C1 = 0x1C, // camera controls commonly used (sent)
144 : E1 = 0x1E, // tracking controls commonly used (sent)
145 : C2 = 0x2C, // camera controls infrequently used (sent)
146 : E2 = 0x2E, // tracking controls infrequently used (sent)
147 : T1_F1_B1_D1 = 0x40, // actual roll, pitch, yaw angles (received)
148 : M_AHRS = 0xB1, // vehicle attitude and position (sent)
149 : };
150 :
151 : // U communication configuration control commands
152 : enum class CommConfigCmd : uint8_t {
153 : QUERY_FIRMWARE_VER = 0xD0,
154 : QUERY_MODEL = 0xE4,
155 : };
156 :
157 : // A1 servo status enum (used in A1, B1 packets)
158 : enum class ServoStatus : uint8_t {
159 : MANUAL_SPEED_MODE = 0x01,
160 : FOLLOW_YAW = 0x03,
161 : MANUAL_ABSOLUTE_ANGLE_MODE = 0x0B,
162 : FOLLOW_YAW_DISABLE = 0x0A,
163 : };
164 :
165 : // C1 image sensor choice
166 : enum class ImageSensor : uint8_t {
167 : NO_ACTION = 0x00, // no image sensor is affected
168 : EO1 = 0x01, // electro-optical, aka rgb
169 : IR = 0x02, // infrared, aka thermal
170 : EO1_IR_PIP = 0x03, // rgb is main, IR is picture-in-picture
171 : IR_EO1_PIP = 0x04, // thermal is main, rgb is picture-in-picture
172 : FUSION = 0x05, // rgb and thermal are fused
173 : IR1_13MM = 0x06, // only valid for 1352 module
174 : IR2_52MM = 0x07, // only valid for 1352 module
175 : };
176 :
177 : // C1 camera commands
178 : enum class CameraCommand : uint8_t {
179 : NO_ACTION = 0x00,
180 : STOP_FOCUS_AND_ZOOM = 0x01,
181 : ZOOM_OUT = 0x08,
182 : ZOOM_IN = 0x09,
183 : FOCUS_PLUS = 0x0A,
184 : FOCUS_MINUS = 0x0B,
185 : TAKE_PICTURE = 0x13,
186 : START_RECORD = 0x14,
187 : STOP_RECORD = 0x15,
188 : AUTO_FOCUS = 0x19,
189 : MANUAL_FOCUS = 0x1A,
190 : IR_ZOOM_OUT = 0x1B,
191 : IR_ZOOM_IN = 0x1C
192 : };
193 :
194 : // C1 rangefinder commands
195 : enum class LRFCommand : uint8_t {
196 : NO_ACTION = 0x00,
197 : SINGLE_RANGING = 0x01,
198 : CONTINUOUS_RANGING_START = 0x02,
199 : LPCL_CONTINUOUS_RANGING_START = 0x03,
200 : STOP_RANGING = 0x05
201 : };
202 :
203 : // C2 camera commands
204 : enum class CameraCommand2 : uint8_t {
205 : SET_EO_ZOOM = 0x53
206 : };
207 :
208 : // D1 recording status (received from gimbal)
209 : enum class RecordingStatus : uint8_t {
210 : RECORDING_STOPPED = 0x00,
211 : RECORDING = 0x01,
212 : PICTURE_MODE = 0x02
213 : };
214 :
215 : // E1 tracking commands
216 : enum class TrackingCommand : uint8_t {
217 : STOP = 0x01,
218 : START = 0x03,
219 : SET_POINT = 0x0A,
220 : SET_RECT_TOPLEFT = 0x0B,
221 : SET_RECT_BOTTOMRIGHT = 0x0C,
222 : };
223 :
224 : // E1 tracking source (e.g. which camera)
225 : enum class TrackingSource : uint8_t {
226 : EO1 = 0x01, // electro-optical, aka rgb
227 : IR = 0x02, // infrared, aka thermal
228 : EO2 = 0x03, // electro-optical, aka rgb
229 : };
230 :
231 : // E2 tracking commands2
232 : enum class TrackingCommand2 : uint8_t {
233 : SET_POINT = 0x0A,
234 : SET_RECT_TOPLEFT = 0x0B,
235 : SET_RECT_BOTTOMRIGHT = 0x0C,
236 : };
237 :
238 : // F1 tracking status (received from gimbal)
239 : enum class TrackingStatus : uint8_t {
240 : STOPPED = 0x00, // not tracking
241 : SEARCHING = 0x01, // searching
242 : TRACKING = 0x02, // locked onto a target
243 : LOST = 0x03, // lost target
244 : };
245 :
246 : // parsing state
247 : enum class ParseState : uint8_t {
248 : WAITING_FOR_HEADER1,
249 : WAITING_FOR_HEADER2,
250 : WAITING_FOR_HEADER3,
251 : WAITING_FOR_LENGTH,
252 : WAITING_FOR_FRAMEID,
253 : WAITING_FOR_DATA,
254 : WAITING_FOR_CRC
255 : };
256 :
257 : // packet formats
258 : union HandshakePacket {
259 : struct {
260 : FrameId frame_id; // always 0x00
261 : uint8_t unused; // always 0x00
262 : } content;
263 : uint8_t bytes[sizeof(content)];
264 : };
265 :
266 : // U packed used to send communication configuration control commands
267 : // gimbal replies with V packet
268 : union UPacket {
269 : struct {
270 : FrameId frame_id; // always 0x01
271 : CommConfigCmd control_cmd; // see CommConfigCmd enum above
272 : uint8_t params[9]; // parameters (unused)
273 : } content;
274 : uint8_t bytes[sizeof(content)];
275 : };
276 :
277 : // A1 used to send target angles and rates
278 : union A1Packet {
279 : struct {
280 : FrameId frame_id; // always 0x1A
281 : ServoStatus servo_status; // see ServoStatus enum above
282 : be16_t yaw_be; // target yaw angle or rate msb
283 : be16_t pitch_be; // target pitch angle or rate msb
284 : uint8_t unused[4]; // unused
285 : } content;
286 : uint8_t bytes[sizeof(content)];
287 : };
288 :
289 : // C1 used to send camera commands (commonly used)
290 : union C1Packet {
291 : struct PACKED {
292 : FrameId frame_id; // always 0x1C
293 : be16_t sensor_zoom_cmd_be;
294 : } content;
295 : uint8_t bytes[sizeof(content)];
296 : };
297 :
298 : // C2 used to send camera commands (less commonly used)
299 : union C2Packet {
300 : struct {
301 : FrameId frame_id; // always 0x2C
302 : CameraCommand2 cmd; // see CameraCommand2 enum above
303 : be16_t value_be; // value
304 : } content;
305 : uint8_t bytes[sizeof(content)];
306 : };
307 :
308 : // E1 used to send tracking commands
309 : union E1Packet {
310 : struct {
311 : FrameId frame_id; // always 0x1E
312 : TrackingSource source : 3; // see TrackingSource enum above
313 : uint8_t unused : 5; // param1 (unused)
314 : TrackingCommand cmd; // see TrackingCommand enum above
315 : uint8_t param2; // param2
316 : } content;
317 : uint8_t bytes[sizeof(content)];
318 : };
319 :
320 : // E2 used to send tracking commands2
321 : union E2Packet {
322 : struct {
323 : FrameId frame_id; // always 0x2E
324 : TrackingCommand2 cmd; // see TrackingCommand2 enum above
325 : be16_t param1_be;
326 : be16_t param2_be;
327 : } content;
328 : uint8_t bytes[sizeof(content)];
329 : };
330 :
331 : // M_AHRS used to send vehicle attitude and position to gimbal
332 : union M_AHRSPacket {
333 : struct PACKED {
334 : FrameId frame_id; // always 0xB1
335 : uint8_t data_type; // should be 0x07. Bit0: Attitude, Bit1: GPS, Bit2 Gyro
336 : uint8_t unused2to8[7]; // unused
337 : be16_t roll_be; // vehicle roll angle. 1bit=360deg/65536
338 : be16_t pitch_be; // vehicle pitch angle. 1bit=360deg/65536
339 : be16_t yaw_be; // vehicle yaw angle. 1bit=360deg/65536
340 : be16_t date_be; // bit0~6:year, bit7~10:month, bit11~15:day
341 : uint8_t seconds_utc[3]; // seconds. 1bit = 0.01sec
342 : be16_t gps_yaw_be; // GPS yaw
343 : uint8_t position_mark_bitmask; // bit0:new position, bit1:clock fix calced, bit2:horiz calced, bit3:alt calced
344 : be32_t latitude_be; // latitude. 1bit = 10e-7
345 : be32_t longitude_be; // longitude. 1bit = 10e-7
346 : be32_t height_be; // height. 1bit = 1mm
347 : be16_t ground_speed_N_be; // ground speed in North direction. 1bit = 0.01m/s
348 : be16_t ground_speed_E_be; // ground speed in East direction. 1bit = 0.01m/s
349 : be16_t vdop_be; // GPS vdop. 1bit = 0.01
350 : be16_t ground_speed_D_be; // speed downwards. 1bit = 0.01m/s
351 : } content;
352 : uint8_t bytes[sizeof(content)];
353 : };
354 :
355 : // reading incoming packets from gimbal and confirm they are of the correct format
356 : // results are held in the _parsed_msg structure
357 : void read_incoming_packets();
358 :
359 : // process successfully decoded packets held in the _parsed_msg structure
360 : void process_packet();
361 :
362 : // calculate crc of the received message
363 : uint8_t calc_crc(const uint8_t *buf, uint8_t length) const;
364 :
365 : // get the length and frame count byte (3rd byte of all messages)
366 : // length is all bytes after the header including CRC
367 : uint8_t get_length_and_frame_count_byte(uint8_t length);
368 :
369 : // send packet to gimbal
370 : // returns true on success, false if outgoing serial buffer is full
371 : bool send_packet(const uint8_t* databuff, uint8_t databuff_len);
372 :
373 : // send handshake, gimbal will respond with T1_F1_B1_D1 packet that includes current angles
374 : void send_handshake();
375 :
376 : // set gimbal's lock vs follow mode
377 : // lock should be true if gimbal should maintain an earth-frame target
378 : // lock is false to follow / maintain a body-frame target
379 : bool set_lock(bool lock);
380 :
381 : // send communication configuration command (aka U packet), gimbal will respond with a V packet
382 : bool send_comm_config_cmd(CommConfigCmd cmd);
383 :
384 : // send target pitch and yaw rates to gimbal
385 : void send_target_rates(const MountRateTarget &rate_rads) override;
386 :
387 : // send target pitch and yaw angles to gimbal
388 : void send_target_angles(const MountAngleTarget &angle_rad) override;
389 :
390 : // send camera command, affected image sensor and value (e.g. zoom speed)
391 : bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd = LRFCommand::NO_ACTION);
392 :
393 : // send camera command2 and corresponding value (e.g. zoom as absolute value)
394 : bool send_camera_command2(CameraCommand2 cmd, uint16_t value);
395 :
396 : // send tracking command and corresponding value (normally zero)
397 : bool send_tracking_command(TrackingCommand cmd, uint8_t value);
398 :
399 : // send camera command2 and corresponding parameter values
400 : bool send_tracking_command2(TrackingCommand2 cmd, int16_t param1, int16_t param2);
401 :
402 : // send vehicle attitude and position to gimbal
403 : bool send_m_ahrs();
404 :
405 : // internal variables
406 : uint8_t _msg_buff[AP_MOUNT_VIEWPRO_PACKETLEN_MAX]; // buffer holding latest bytes from gimbal
407 : uint8_t _msg_buff_len; // number of bytes held in msg buff
408 : static constexpr uint8_t _msg_buff_data_start = 2; // data starts at this byte of _msg_buff
409 :
410 : // parser state and unpacked fields
411 : struct {
412 : uint8_t data_len; // expected number of data bytes
413 : uint8_t frame_id; // frame id (equivalent to command id)
414 : uint16_t data_bytes_received; // number of data bytes received so far
415 : uint8_t crc; // latest message's crc
416 : ParseState state; // state of incoming message processing
417 : } _parsed_msg;
418 :
419 : // variables for sending packets to gimbal
420 : uint8_t _last_frame_counter; // frame counter sent in last message
421 : uint32_t _last_update_ms; // system time (in milliseconds) that angle or rate targets were last sent
422 : Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
423 : uint32_t _last_current_angle_rad_ms; // system time _current_angle_rad was updated (used for health reporting)
424 : bool _recording; // recording status received from gimbal
425 : bool _last_lock; // last lock mode sent to gimbal
426 : TrackingStatus _last_tracking_status; // last tracking status received from gimbal (used to notify users)
427 : ImageSensor _image_sensor; // user selected image sensor (aka camera lens)
428 : float _zoom_times; // zoom times received from gimbal
429 : uint32_t _firmware_version; // firmware version from gimbal
430 : bool _got_firmware_version; // true once we have received the firmware version
431 : char _model_name[11] {}; // model name received from gimbal, always null-terminated
432 : bool _got_model_name; // true once we have received model name
433 : float _rangefinder_dist_m; // latest rangefinder distance (in meters)
434 : };
435 :
436 : #endif // HAL_MOUNT_VIEWPRO_ENABLED
|
