Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_TOPOTEK_ENABLED
4 :
5 : #include "AP_Mount_Topotek.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_AHRS/AP_AHRS.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_RTC/AP_RTC.h>
11 :
12 : extern const AP_HAL::HAL& hal;
13 :
14 : #define ANGULAR_VELOCITY_CONVERSION 1.220740379 // gimbal angular velocity conversion ratio
15 : #define TRACK_TOTAL_WIDTH 1920 // track the maximum width of the image range
16 : #define TRACK_TOTAL_HEIGHT 1080 // track the maximum height of the image range
17 : #define TRACK_RANGE 60 // the size of the image at point tracking
18 : #define AP_MOUNT_TOPOTEK_UPDATE_INTERVAL_MS 100 // resend angle or rate targets to gimbal at this interval
19 : #define AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS 1000 // timeout for health and rangefinder readings
20 : #define AP_MOUNT_TOPOTEK_PACKETLEN_MIN 12 // packet length not including the data segment
21 : #define AP_MOUNT_TOPOTEK_DATALEN_MAX (AP_MOUNT_TOPOTEK_PACKETLEN_MAX - AP_MOUNT_TOPOTEK_PACKETLEN_MIN) // data segment lens can be no more tha this
22 :
23 : // 3 character identifiers
24 : # define AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE "CAP" // take picture, data bytes: 01:RGB + thermal, 02:RGB, 03:thermal, 05:RGB + thermal (with temp measurement)
25 : # define AP_MOUNT_TOPOTEK_ID3CHAR_RECORD_VIDEO "REC" // record video, data bytes: 00:stop, 01:start, 0A:toggle start/stop
26 : # define AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM "ZMC" // control zoom, data bytes: 00:stop, 01:zoom out, 02:zoom in
27 : # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_ZOOM "ZOM" // get zoom, no data bytes
28 : # define AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS "FCC" // control focus, data bytes: 00:stop, 01:focus+, 02:focus-, 0x10:auto focus, 0x11:manual focus, 0x12:manu focus (save), 0x13:auto focus (save)
29 : # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_FOCUS "FOC" // get focus, no data bytes
30 : # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_ZOOM_AND_FOCU "ZFP" // set zoom and focus
31 : # define AP_MOUNT_TOPOTEK_ID3CHAR_DAY_NIGHT_SWITCHING "IRC" // set day/night setting, data bytes: 00:day, 01:night, 0A:toggle state
32 : # define AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING "TRC" // get/set image tracking, data bytes: 00:get status (use with "r"), 01:stop (use with "w")
33 : # define AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING "LOC" // start image tracking
34 : # define AP_MOUNT_TOPOTEK_ID3CHAR_LRF "LRF" // laser rangefinder control, data bytes: 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement
35 : # define AP_MOUNT_TOPOTEK_ID3CHAR_PIP "PIP" // set picture-in-picture setting, data bytes: // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
36 : # define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GIA" // get gimbal attitude, data bytes: 00:stop, 01:start
37 : # define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity
38 : # define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY
39 : # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00
40 : # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME "PA2" // get model name, data bytes always 00
41 : # define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE "PTZ" // set gimbal mode, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:home position, 06:lock, 07:follow, 08:lock/follow toggle, 09:calibration, 0A:one button down
42 : # define AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE "YPR" // set the rate yaw, pitch and roll targets of the gimbal yaw in range -99 ~ +99
43 : # define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE "GIY" // set the yaw angle target in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec)
44 : # define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE_BF "GAY" // set the yaw angle target in body-frame in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec)
45 : # define AP_MOUNT_TOPOTEK_ID3CHAR_PITCH_ANGLE "GIP" // set the pitch angle target in the range -90 ~ 90, speed 0 ~ 99 (0.1deg/sec)
46 : # define AP_MOUNT_TOPOTEK_ID3CHAR_ROLL_ANGLE "GIR" // set the roll angle target in the range -90 ~ 90, speed 0 ~ 99 (0.1deg/sec)
47 : # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_LAT "LAT" // set the gimbal's latitude
48 : # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_LON "LON" // set the gimbal's longitude
49 : # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_ALT "ALT" // set the gimbal's altitude
50 : # define AP_MOUNT_TOPOTEK_ID3CHAR_SET_AZIMUTH "AZI" // set the gimbal's yaw (aka azimuth)
51 :
52 : #define AP_MOUNT_TOPOTEK_DEBUG 0
53 : #define debug(fmt, args ...) do { if (AP_MOUNT_TOPOTEK_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Topotek: " fmt, ## args); } } while (0)
54 :
55 : const char* AP_Mount_Topotek::send_message_prefix = "Mount: Topotek";
56 :
57 : // update mount position - should be called periodically
58 10408 : void AP_Mount_Topotek::update()
59 : {
60 10408 : AP_Mount_Backend::update();
61 :
62 : // exit immediately if not initialised
63 10408 : if (!_initialised) {
64 0 : return;
65 : }
66 :
67 : // reading incoming packets from gimbal
68 10408 : read_incoming_packets();
69 :
70 : // everything below updates at 10hz
71 10408 : uint32_t now_ms = AP_HAL::millis();
72 10408 : if ((now_ms - _last_req_current_info_ms) < 100) {
73 8326 : return;
74 : }
75 2082 : _last_req_current_info_ms = now_ms;
76 :
77 : // re-send the stop zoom command a second time to prevent data transmission errors.
78 2082 : if (_last_zoom_stop) {
79 0 : _last_zoom_stop = false;
80 0 : send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM, true, 0);
81 : }
82 :
83 : // re-send the stop focus command a second time to prevent data transmission errors.
84 2082 : if (_last_focus_stop) {
85 0 : _last_focus_stop = false;
86 0 : send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0);
87 : }
88 :
89 : // send GPS-related information to the gimbal
90 2082 : send_location_info();
91 :
92 : // calls below here called at 1hz
93 2082 : _last_req_step++;
94 2082 : if (_last_req_step >= 10) {
95 207 : _last_req_step = 0;
96 : }
97 2082 : switch (_last_req_step) {
98 207 : case 0:
99 : // get gimbal version
100 207 : if (!_got_gimbal_version) {
101 2 : request_gimbal_version();
102 : }
103 207 : break;
104 209 : case 2:
105 : // request gimbal attitude at 1hz
106 : // gimbal will continue to send attitude information during the next period
107 209 : request_gimbal_attitude();
108 209 : break;
109 209 : case 4:
110 : // request memory card information
111 209 : request_gimbal_sdcard_info();
112 209 : break;
113 209 : case 6:
114 : // request tracking info
115 209 : request_track_status();
116 209 : break;
117 207 : case 8:
118 : // get gimbal model name
119 207 : if (!_got_gimbal_model_name) {
120 2 : request_gimbal_model_name();
121 : }
122 207 : break;
123 : }
124 :
125 : // handle tracking state
126 2082 : if (_is_tracking) {
127 : // cancel tracking if mode has changed
128 0 : if (_last_mode != _mode) {
129 0 : _last_mode = _mode;
130 0 : cancel_tracking();
131 : }
132 0 : return;
133 : }
134 2082 : _last_mode = _mode;
135 :
136 : // update based on mount mode
137 2082 : update_mnt_target();
138 :
139 : // send target angles or rates depending on the target type
140 2082 : send_target_to_gimbal();
141 : }
142 :
143 : // return true if healthy
144 2134 : bool AP_Mount_Topotek::healthy() const
145 : {
146 : // exit immediately if not initialised
147 2134 : if (!_initialised) {
148 0 : return false;
149 : }
150 :
151 : // unhealthy if attitude information not received recently
152 2134 : const uint32_t last_current_angle_ms = _last_current_angle_ms;
153 2134 : return (AP_HAL::millis() - last_current_angle_ms < AP_MOUNT_TOPOTEK_HEALTH_TIMEOUT_MS);
154 : }
155 :
156 : // take a picture. returns true on success
157 0 : bool AP_Mount_Topotek::take_picture()
158 : {
159 : // exit immediately if not initialised
160 0 : if (!_initialised) {
161 0 : return false;
162 : }
163 :
164 : // exit immediately if the memory card is abnormal
165 0 : if (!_sdcard_status) {
166 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix);
167 0 : return false;
168 : }
169 :
170 : // sample command: #TPUD2wCAP01
171 0 : return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_CAPTURE, true, 1);
172 : }
173 :
174 : // start or stop video recording. returns true on success
175 : // set start_recording = true to start record, false to stop recording
176 0 : bool AP_Mount_Topotek::record_video(bool start_recording)
177 : {
178 : // exit immediately if not initialised
179 0 : if (!_initialised) {
180 0 : return false;
181 : }
182 :
183 : // exit immediately if the memory card is abnormal
184 0 : if (!_sdcard_status) {
185 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s SD card error", send_message_prefix);
186 0 : return false;
187 : }
188 :
189 : // sample command: #TPUD2wREC01
190 0 : return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_RECORD_VIDEO, true, start_recording ? 1 : 0);
191 : }
192 :
193 : // set zoom specified as a rate
194 0 : bool AP_Mount_Topotek::set_zoom(ZoomType zoom_type, float zoom_value)
195 : {
196 : // exit immediately if not initialised
197 0 : if (!_initialised) {
198 0 : return false;
199 : }
200 :
201 : // zoom rate
202 0 : if (zoom_type == ZoomType::RATE) {
203 : uint8_t zoom_cmd;
204 0 : if (is_zero(zoom_value)) {
205 : // stop zoom
206 0 : zoom_cmd = 0;
207 0 : _last_zoom_stop = true;
208 0 : } else if (zoom_value < 0) {
209 : // zoom out
210 0 : zoom_cmd = 1;
211 : } else {
212 : // zoom in
213 0 : zoom_cmd = 2;
214 : }
215 : // sample command: #TPUM2wZMC00
216 0 : return send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_ZOOM, true, zoom_cmd);
217 : }
218 :
219 : // unsupported zoom type
220 0 : return false;
221 : }
222 :
223 : // set focus specified as rate, percentage or auto
224 : // focus in = -1, focus hold = 0, focus out = 1
225 0 : SetFocusResult AP_Mount_Topotek::set_focus(FocusType focus_type, float focus_value)
226 : {
227 : // exit immediately if not initialised
228 0 : if (!_initialised) {
229 0 : return SetFocusResult::FAILED;
230 : }
231 :
232 0 : switch (focus_type) {
233 0 : case FocusType::RATE: {
234 : // focus stop
235 : uint8_t focus_cmd;
236 0 : if (is_zero(focus_value)) {
237 0 : focus_cmd = 0;
238 0 : _last_focus_stop = true;
239 0 : } else if (focus_value < 0) {
240 : // focus-
241 0 : focus_cmd = 2;
242 : } else {
243 : // focus+
244 0 : focus_cmd = 1;
245 : }
246 : // send focus command and switch to manual focus
247 : // sample command: #TPUM2wFCC00
248 0 : if (send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, focus_cmd) &&
249 0 : send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0x11)) {
250 0 : return SetFocusResult::ACCEPTED;
251 : }
252 0 : return SetFocusResult::FAILED;
253 : }
254 0 : case FocusType::PCT:
255 : // not supported
256 0 : return SetFocusResult::INVALID_PARAMETERS;
257 0 : case FocusType::AUTO:
258 : // auto focus
259 0 : if (send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_CONTROL_FOCUS, true, 0x10)) {
260 0 : return SetFocusResult::ACCEPTED;
261 : }
262 0 : return SetFocusResult::FAILED;
263 : }
264 :
265 : // unsupported focus type
266 0 : return SetFocusResult::INVALID_PARAMETERS;
267 : }
268 :
269 : // set tracking to none, point or rectangle (see TrackingType enum)
270 : // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
271 : // p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
272 0 : bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
273 : {
274 : // exit immediately if not initialised
275 0 : if (!_initialised) {
276 0 : return false;
277 : }
278 :
279 : // local variables holding tracker center and width
280 : int16_t track_center_x, track_center_y, track_width, track_height;
281 0 : bool send_tracking_cmd = false;
282 :
283 0 : switch (tracking_type) {
284 :
285 0 : case TrackingType::TRK_NONE:
286 0 : return cancel_tracking();
287 :
288 0 : case TrackingType::TRK_POINT: {
289 : // calculate tracking center, width and height
290 0 : track_center_x = (int16_t)((p1.x*TRACK_TOTAL_WIDTH - 960) / 0.96);
291 0 : track_center_y = (int16_t)((p1.y*TRACK_TOTAL_HEIGHT - 540) / 0.54);
292 0 : track_width = (int16_t)(TRACK_RANGE / 0.96);
293 0 : track_height = (int16_t)(TRACK_RANGE / 0.54);
294 0 : send_tracking_cmd = true;
295 0 : break;
296 : }
297 :
298 0 : case TrackingType::TRK_RECTANGLE:
299 : // calculate upper left and bottom right points
300 : // handle case where p1 and p2 are in an unexpected order
301 0 : int16_t upper_leftx = (int16_t)(MIN(p1.x, p2.x)*TRACK_TOTAL_WIDTH);
302 0 : int16_t upper_lefty = (int16_t)(MIN(p1.y, p2.y)*TRACK_TOTAL_HEIGHT);
303 0 : int16_t bottom_rightx = (int16_t)(MAX(p1.x, p2.x)*TRACK_TOTAL_WIDTH);
304 0 : int16_t bottom_righty = (int16_t)(MAX(p1.y, p2.y)*TRACK_TOTAL_HEIGHT);
305 :
306 : // calculated width and height and sanity check
307 0 : const int16_t frame_selection_width = bottom_rightx - upper_leftx;
308 0 : const int16_t frame_selection_height = bottom_righty - upper_lefty;
309 0 : if (frame_selection_width <= 0 || frame_selection_height <= 0) {
310 0 : return false;
311 : }
312 :
313 : // calculate tracking center
314 0 : track_center_x = (int16_t)((((upper_leftx + bottom_rightx) * 0.5) - 960) / 0.96);
315 0 : track_center_y = (int16_t)((((upper_lefty + bottom_righty) * 0.5) - 540) / 0.54);
316 :
317 : // tracking range after conversion
318 0 : track_width = (int16_t)(frame_selection_width / 0.96);
319 0 : track_height = (int16_t)(frame_selection_height / 0.54);
320 :
321 0 : send_tracking_cmd = true;
322 0 : break;
323 : }
324 :
325 0 : if (send_tracking_cmd) {
326 : // set the gimbal to the ready-to-track state when the gimbal tracking status is stopped
327 0 : if (_last_tracking_state == TrackingStatus::STOPPED_TRACKING) {
328 0 : send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 2);
329 : }
330 :
331 : // prepare data bytes
332 : uint8_t databuff[10];
333 0 : databuff[0] = HIGHBYTE(track_center_x);
334 0 : databuff[1] = LOWBYTE(track_center_x);
335 0 : databuff[2] = HIGHBYTE(track_center_y);
336 0 : databuff[3] = LOWBYTE(track_center_y);
337 0 : databuff[4] = HIGHBYTE(track_width);
338 0 : databuff[5] = LOWBYTE(track_width);
339 0 : databuff[6] = HIGHBYTE(track_height);
340 0 : databuff[7] = LOWBYTE(track_height);
341 0 : databuff[8] = 0;
342 0 : databuff[9] = (tracking_type == TrackingType::TRK_POINT) ? 9 : 1; // when tracking point, enable fuzzy click function
343 :
344 : // send tracking command
345 0 : bool res = send_variablelen_packet(HeaderType::VARIABLE_LEN,
346 : AddressByte::SYSTEM_AND_IMAGE,
347 : AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING,
348 : true,
349 : (uint8_t*)databuff, ARRAY_SIZE(databuff));
350 :
351 : // display error message on failure
352 0 : if (!res) {
353 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s tracking failed", send_message_prefix);
354 : }
355 :
356 0 : return res;
357 : }
358 :
359 : // should never reach here
360 0 : return false;
361 : }
362 :
363 : // send command to gimbal to cancel tracking (if necessary)
364 : // returns true on success, false on failure to send message
365 0 : bool AP_Mount_Topotek::cancel_tracking()
366 : {
367 : // exit immediately if not initialised
368 0 : if (!_initialised) {
369 0 : return false;
370 : }
371 :
372 : // if gimbal is tracking-in-progress change to waiting state, otherwise stop
373 0 : const uint8_t track_set = _last_tracking_state == TrackingStatus::TRACKING_IN_PROGRESS ? 1 : 0;
374 :
375 : // send tracking command
376 0 : return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, track_set);
377 : }
378 :
379 : // set camera picture-in-picture mode
380 0 : bool AP_Mount_Topotek::set_lens(uint8_t lens)
381 : {
382 : // exit immediately if not initialised
383 0 : if (!_initialised) {
384 0 : return false;
385 : }
386 :
387 : // sanity check lens number
388 : // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
389 : // sample command: #TPUD2wPIP0A
390 0 : if (lens > 3) {
391 0 : return false;
392 : }
393 :
394 : // send pip command
395 0 : return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_PIP, true, lens);
396 : }
397 :
398 : #if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
399 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
400 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
401 0 : bool AP_Mount_Topotek::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
402 : {
403 : // exit immediately if not initialised
404 0 : if (!_initialised) {
405 0 : return false;
406 : }
407 :
408 : // maps primary and secondary source to pip setting
409 : // pip settings 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next
410 : // sample command: #TPUD2wPIP0A
411 0 : uint8_t pip_setting = 0;
412 0 : switch (primary_source) {
413 0 : case 0: // Default (RGB)
414 : FALLTHROUGH;
415 : case 1: // RGB
416 0 : switch (secondary_source) {
417 0 : case 0: // RGB + Default (None)
418 0 : pip_setting = 0; // main only
419 0 : break;
420 0 : case 2: // PIP RGB+IR
421 0 : pip_setting = 1; // main+sub
422 0 : break;
423 0 : default:
424 0 : return false;
425 : }
426 0 : break;
427 0 : case 2: // IR
428 0 : switch (secondary_source) {
429 0 : case 0: // IR + Default (None)
430 0 : pip_setting = 3; // sub only
431 0 : break;
432 0 : case 1: // IR+RGB
433 0 : pip_setting = 2; // sub+main
434 0 : break;
435 0 : default:
436 0 : return false;
437 : }
438 0 : break;
439 0 : default:
440 0 : return false;
441 : }
442 :
443 : // send pip command
444 0 : return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_PIP, true, pip_setting);
445 : }
446 : #endif // HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
447 :
448 : // send camera settings message to GCS
449 0 : void AP_Mount_Topotek::send_camera_settings(mavlink_channel_t chan) const
450 : {
451 : // exit immediately if not initialised
452 0 : if (!_initialised) {
453 0 : return;
454 : }
455 :
456 : // send CAMERA_SETTINGS message
457 0 : mavlink_msg_camera_settings_send(
458 : chan,
459 : AP_HAL::millis(), // time_boot_ms
460 0 : _recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
461 : NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown
462 : NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
463 : }
464 :
465 : // get rangefinder distance. Returns true on success
466 2080 : bool AP_Mount_Topotek::get_rangefinder_distance(float& distance_m) const
467 : {
468 : // if not healthy or negative distance return false
469 : // healthy() checks attitude timeout which is in same message as rangefinder distance
470 2080 : if (!healthy() || (_measure_dist_m < 0)) {
471 2080 : return false;
472 : }
473 :
474 0 : distance_m = _measure_dist_m;
475 0 : return true;
476 : }
477 :
478 : // enable/disable rangefinder. Returns true on success
479 0 : bool AP_Mount_Topotek::set_rangefinder_enable(bool enable)
480 : {
481 : // exit immediately if not initialised
482 0 : if (!_initialised) {
483 0 : return false;
484 : }
485 :
486 : // 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement
487 : // sample command: #TPUM2wLRF00
488 0 : return send_fixedlen_packet(AddressByte::LENS, AP_MOUNT_TOPOTEK_ID3CHAR_LRF, true, enable ? 3 : 0);
489 : }
490 :
491 : // get attitude as a quaternion. returns true on success
492 3121 : bool AP_Mount_Topotek::get_attitude_quaternion(Quaternion& att_quat)
493 : {
494 3121 : att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z);
495 3121 : return true;
496 : }
497 :
498 : // reading incoming packets from gimbal and confirm they are of the correct format
499 10408 : void AP_Mount_Topotek::read_incoming_packets()
500 : {
501 : // check for bytes on the serial port
502 10408 : int16_t nbytes = MIN(_uart->available(), 1024U);
503 10408 : if (nbytes <= 0 ) {
504 7695 : return;
505 : }
506 :
507 : // flag to allow cases below to reset parser state
508 2713 : bool reset_parser = false;
509 :
510 : // process bytes received
511 64589 : for (int16_t i = 0; i < nbytes; i++) {
512 : uint8_t b;
513 61876 : if (!_uart->read(b)) {
514 0 : continue;
515 : }
516 :
517 : // add latest byte to buffer
518 61876 : _msg_buff[_msg_buff_len++] = b;
519 :
520 : // protect against overly long messages
521 61876 : if (_msg_buff_len >= AP_MOUNT_TOPOTEK_PACKETLEN_MAX) {
522 0 : reset_parser = true;
523 : }
524 :
525 : // process byte depending upon current state
526 61876 : switch (_parser.state) {
527 :
528 2736 : case ParseState::WAITING_FOR_HEADER1:
529 2736 : if (b == '#') {
530 2736 : _parser.state = ParseState::WAITING_FOR_HEADER2;
531 2736 : break;
532 : }
533 0 : reset_parser = true;
534 0 : break;
535 :
536 2736 : case ParseState::WAITING_FOR_HEADER2:
537 2736 : if (b == 't' || b == 'T') {
538 2736 : _parser.state = ParseState::WAITING_FOR_HEADER3;
539 2736 : break;
540 : }
541 0 : reset_parser = true;
542 0 : break;
543 :
544 2736 : case ParseState::WAITING_FOR_HEADER3:
545 2736 : if (b == 'p' || b == 'P') {
546 2736 : _parser.state = ParseState::WAITING_FOR_ADDR1;
547 2736 : break;
548 : }
549 0 : reset_parser = true;
550 0 : break;
551 :
552 5472 : case ParseState::WAITING_FOR_ADDR1:
553 : case ParseState::WAITING_FOR_ADDR2:
554 5472 : if (b == 'U' || b =='M' || b == 'D' || b =='E' || b =='P' || b =='G') {
555 : // advance to next state
556 5472 : _parser.state = (ParseState)((uint8_t)_parser.state+1);
557 5472 : break;
558 : }
559 0 : reset_parser = true;
560 0 : break;
561 :
562 2736 : case ParseState::WAITING_FOR_DATALEN: {
563 : // sanity check data length
564 : uint8_t data_len;
565 2736 : if (hex_char_to_nibble(b, data_len) && data_len <= AP_MOUNT_TOPOTEK_DATALEN_MAX) {
566 2736 : _parser.data_len = data_len;
567 2736 : _parser.state = ParseState::WAITING_FOR_CONTROL;
568 2736 : break;
569 : }
570 0 : reset_parser = true;
571 0 : break;
572 : }
573 :
574 2736 : case ParseState::WAITING_FOR_CONTROL:
575 : // r or w
576 2736 : if (b == 'r' || b == 'w') {
577 2736 : _parser.state = ParseState::WAITING_FOR_ID1;
578 2736 : break;
579 : }
580 0 : reset_parser = true;
581 0 : break;
582 :
583 8208 : case ParseState::WAITING_FOR_ID1:
584 : case ParseState::WAITING_FOR_ID2:
585 : case ParseState::WAITING_FOR_ID3:
586 : // check all uppercase letters and numbers. eg 'GAC'
587 8208 : if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) {
588 : // advance to next state
589 8208 : _parser.state = (ParseState)((uint8_t)_parser.state+1);
590 8208 : break;
591 : }
592 0 : reset_parser = true;
593 0 : break;
594 :
595 29044 : case ParseState::WAITING_FOR_DATA: {
596 : // normally hex numbers in char form (e.g. '0A')
597 29044 : const uint8_t data_bytes_received = _msg_buff_len - (AP_MOUNT_TOPOTEK_PACKETLEN_MIN - 2);
598 :
599 : // sanity check to protect against programming errors
600 29044 : if (data_bytes_received > AP_MOUNT_TOPOTEK_DATALEN_MAX) {
601 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
602 0 : reset_parser = true;
603 0 : break;
604 : }
605 :
606 : // advance parser state once expected number of bytes have been received
607 29044 : if (data_bytes_received == _parser.data_len) {
608 2736 : _parser.state = ParseState::WAITING_FOR_CRC_LOW;
609 : }
610 29044 : break;
611 : }
612 :
613 2736 : case ParseState::WAITING_FOR_CRC_LOW:
614 2736 : _parser.state = ParseState::WAITING_FOR_CRC_HIGH;
615 2736 : break;
616 :
617 2736 : case ParseState::WAITING_FOR_CRC_HIGH:
618 : // this is the last byte in the message so reset the parser
619 2736 : reset_parser = true;
620 :
621 : // sanity check to protect against programming errors
622 2736 : if (_msg_buff_len < AP_MOUNT_TOPOTEK_PACKETLEN_MIN) {
623 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
624 0 : break;
625 : }
626 :
627 : // calculate and check CRC
628 2736 : const uint8_t crc_value = calculate_crc(_msg_buff, _msg_buff_len - 2);
629 2736 : const char crc_char1 = hex2char((crc_value >> 4) & 0x0f);
630 2736 : const char crc_char2 = hex2char((crc_value) & 0x0f);
631 2736 : if (crc_char1 != _msg_buff[_msg_buff_len - 2] || crc_char2 != _msg_buff[_msg_buff_len-1]) {
632 : debug("CRC expected:%x got:%c%c", (int)crc_value, crc_char1, crc_char2);
633 0 : break;
634 : }
635 :
636 : // CRC is OK, call function to process the message
637 4012 : for (uint8_t count = 0; count < AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM; count++) {
638 4012 : if (strncmp((const char*)_msg_buff + 7, (const char*)(uart_recv_cmd_compare_list[count].uart_cmd_key), 3) == 0) {
639 2736 : (this->*(uart_recv_cmd_compare_list[count].func))();
640 2736 : break;
641 : }
642 : }
643 : }
644 :
645 : // handle reset of parser
646 61876 : if (reset_parser) {
647 2736 : _parser.state = ParseState::WAITING_FOR_HEADER1;
648 2736 : _msg_buff_len = 0;
649 2736 : reset_parser = false;
650 : }
651 : }
652 : }
653 :
654 : // request gimbal attitude
655 209 : void AP_Mount_Topotek::request_gimbal_attitude()
656 : {
657 : // sample command: #TPUG2wGIA01
658 209 : send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT, true, 1);
659 209 : }
660 :
661 : // request gimbal memory card information
662 209 : void AP_Mount_Topotek::request_gimbal_sdcard_info()
663 : {
664 : // request remaining capacity
665 : // sample command including CRC: #TPUD2rSDC003E
666 : // 00:get remaining capacity, 01:get total capacity
667 209 : send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD, false, 0);
668 209 : }
669 :
670 : // request gimbal tracking status
671 209 : void AP_Mount_Topotek::request_track_status()
672 : {
673 : // 00:get status (use with "r"), 01:stop (use with "w")
674 : // sample command: #TPUD2rTRC00
675 209 : send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, false, 0);
676 209 : }
677 :
678 : // request gimbal version
679 2 : void AP_Mount_Topotek::request_gimbal_version()
680 : {
681 : // sample command: #TPUD2rVSN00
682 2 : send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION, false, 0);
683 2 : }
684 :
685 : // request gimbal model name
686 2 : void AP_Mount_Topotek::request_gimbal_model_name()
687 : {
688 : // sample command: #TPUG2rPA200
689 2 : send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME, false, 0);
690 2 : }
691 :
692 : // send angle target in radians to gimbal
693 2082 : void AP_Mount_Topotek::send_target_angles(const MountAngleTarget& angle_rad)
694 : {
695 : // gimbal's earth-frame angle control drifts so always use body frame
696 : // set gimbal's lock state if it has changed
697 2082 : if (!set_gimbal_lock(false)) {
698 0 : return;
699 : }
700 :
701 : // calculate and send yaw target
702 : // sample command #tpUG6wGIY
703 2082 : const char* format_str = "%04X%02X";
704 2082 : const uint8_t speed = 99;
705 2082 : const uint16_t yaw_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.get_bf_yaw()) * 100, MAX(-18000, _params.yaw_angle_min * 100), MIN(18000, _params.yaw_angle_max * 100));
706 :
707 : uint8_t databuff[7];
708 2082 : hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, yaw_angle_cd, speed);
709 2082 : send_variablelen_packet(HeaderType::VARIABLE_LEN,
710 : AddressByte::GIMBAL,
711 : AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE_BF,
712 : true,
713 : (uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
714 :
715 : // send pitch target
716 : // sample command: #tpUG6wGIP
717 2082 : const uint16_t pitch_angle_cd = (uint16_t)constrain_int16(-degrees(angle_rad.pitch) * 100, -9000, 9000);
718 2082 : hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, pitch_angle_cd, speed);
719 2082 : send_variablelen_packet(HeaderType::VARIABLE_LEN,
720 : AddressByte::GIMBAL,
721 : AP_MOUNT_TOPOTEK_ID3CHAR_PITCH_ANGLE,
722 : true,
723 : (uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
724 :
725 : // send roll target
726 : // sample command: #tpUG6wGIR
727 2082 : const uint16_t roll_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.roll) * 100, -18000, 18000);
728 2082 : hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), format_str, roll_angle_cd, speed);
729 2082 : send_variablelen_packet(HeaderType::VARIABLE_LEN,
730 : AddressByte::GIMBAL,
731 : AP_MOUNT_TOPOTEK_ID3CHAR_ROLL_ANGLE,
732 : true,
733 : (uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
734 : }
735 :
736 : // send rate target in rad/s to gimbal
737 0 : void AP_Mount_Topotek::send_target_rates(const MountRateTarget& rate_rads)
738 : {
739 : // set gimbal's lock state if it has changed
740 0 : if (!set_gimbal_lock(rate_rads.yaw_is_ef)) {
741 0 : return;
742 : }
743 :
744 : // convert and constrain rates
745 0 : const uint8_t roll_angle_speed = constrain_int16(degrees(rate_rads.roll) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
746 0 : const uint8_t pitch_angle_speed = constrain_int16(-degrees(rate_rads.pitch) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
747 0 : const uint8_t yaw_angle_speed = constrain_int16(degrees(rate_rads.yaw) * ANGULAR_VELOCITY_CONVERSION, -99, 99);
748 :
749 : // send stop rotation command three times if target roll, pitch and yaw are zero
750 0 : if (roll_angle_speed == 0 && pitch_angle_speed == 0 && yaw_angle_speed == 0) {
751 0 : if (_stop_order_count < 3) {
752 : // sample command: #TPUG2wPTZ00
753 0 : if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE, true, 0)) {
754 0 : _stop_order_count++;
755 : }
756 : }
757 0 : return;
758 : }
759 0 : _stop_order_count = 0;
760 :
761 : // prepare and send command
762 : // sample command: #tpUG6wYPR
763 : uint8_t databuff[7];
764 0 : hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X%02X%02X", yaw_angle_speed, pitch_angle_speed, roll_angle_speed);
765 0 : send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE, true, databuff, ARRAY_SIZE(databuff)-1);
766 : }
767 :
768 : // send time and date to gimbal
769 19 : bool AP_Mount_Topotek::send_time_to_gimbal()
770 : {
771 : #if AP_RTC_ENABLED
772 : // get date and time
773 : // year is the regular Gregorian year, month is 0~11, day is 1~31, hour is 0~23, minute is 0~59, second is 0~60 (1 leap second), ms is 0~999
774 : uint16_t year, ms;
775 : uint8_t month, day, hour, min, sec;
776 19 : if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) {
777 12 : return false;
778 : }
779 :
780 : // sample command: #tpUDCwUTCHHMMSSDDMMYY
781 : uint8_t databuff[13];
782 7 : hal.util->snprintf((char*)databuff, ARRAY_SIZE(databuff), "%02d%02d%02d%02d%02d%02d", hour, min, sec, day, month + 1, year - 2000);
783 7 : return send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TIME, true, (uint8_t*)databuff, ARRAY_SIZE(databuff)-1);
784 : #else
785 : return false;
786 : #endif
787 : }
788 :
789 : // send GPS-related information to the gimbal
790 2082 : bool AP_Mount_Topotek::send_location_info()
791 : {
792 : // get current location
793 2082 : Location loc;
794 2082 : int32_t alt_amsl_cm = 0;
795 2082 : if (!AP::ahrs().get_location(loc) || !loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
796 112 : return false;
797 : }
798 :
799 : // convert latitude and longitude to positive angles in degrees
800 1970 : const double latitude = labs(loc.lat) * 1e-7;
801 1970 : const double longitude = labs(loc.lng) * 1e-7;
802 :
803 : // get the degree part
804 1970 : const int16_t lat_deg = (int16_t)latitude;
805 1970 : const int16_t lng_deg = (int16_t)longitude;
806 :
807 : // get the minute part
808 1970 : const double lat_min = (latitude - lat_deg) * 60.0;
809 1970 : const double lng_min = (longitude - lng_deg) * 60.0;
810 :
811 : // prepare and send latitude
812 : // first byte is N or S, followed by GPS coordinates in degree division format, in the format of ddmm.mmmm
813 : // first byte is zero and will also be transmitted. same as the data format in $GPGGA
814 : // sample command: #tpUDAwLATNddmm.mmmm
815 : uint8_t databuff_lat[11];
816 1970 : hal.util->snprintf((char*)databuff_lat, ARRAY_SIZE(databuff_lat), "%c%02d%07.4f", loc.lat > 0 ? 'N':'S', lat_deg, lat_min);
817 1970 : if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_LAT, true, (uint8_t*)databuff_lat, ARRAY_SIZE(databuff_lat)-1)) {
818 0 : return false;
819 : }
820 :
821 : // prepare and send longitude
822 : // sample command: #tpUDBwLONEdddmm.mmmm
823 : uint8_t databuff_lon[12];
824 1970 : hal.util->snprintf((char*)databuff_lon, ARRAY_SIZE(databuff_lon), "%c%03d%07.4f", loc.lng > 0 ? 'E':'W', lng_deg, lng_min);
825 1970 : if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_LON, true, (uint8_t*)databuff_lon, ARRAY_SIZE(databuff_lon)-1)) {
826 0 : return false;
827 : }
828 :
829 : // get the height in meters
830 1970 : float alt_amsl_m = alt_amsl_cm * 0.01;
831 :
832 : // prepare and send vehicle altitude
833 : // sample command: #tpUD8wALT000000.0, similar format to $GPGGA
834 : uint8_t databuff_alt[9];
835 1970 : hal.util->snprintf((char*)databuff_alt, ARRAY_SIZE(databuff_alt), "%08.1f", alt_amsl_m);
836 1970 : if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_ALT, true, (uint8_t*)databuff_alt, ARRAY_SIZE(databuff_alt)-1)) {
837 0 : return false;
838 : }
839 :
840 : // prepare and send vehicle yaw
841 : // sample command: #tpUD5wAZI359.9, similar format to $GPRMC
842 1970 : const float veh_yaw_deg = AP::ahrs().get_yaw_deg();
843 : uint8_t databuff_azimuth[6];
844 1970 : hal.util->snprintf((char*)databuff_azimuth, ARRAY_SIZE(databuff_azimuth), "%05.1f", veh_yaw_deg);
845 1970 : if (!send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_SET_AZIMUTH, true, (uint8_t*)databuff_azimuth, ARRAY_SIZE(databuff_azimuth)-1)) {
846 0 : return false;
847 : }
848 :
849 1970 : return true;
850 : }
851 :
852 : // attitude information analysis of gimbal
853 2314 : void AP_Mount_Topotek::gimbal_angle_analyse()
854 : {
855 : // consume current angles
856 : uint32_t yaw_raw, pitch_raw, roll_raw;
857 2314 : if (!hex_chars_to_uint32((const char*)&_msg_buff[10], 4, yaw_raw) ||
858 4628 : !hex_chars_to_uint32((const char*)&_msg_buff[14], 4, pitch_raw) ||
859 2314 : !hex_chars_to_uint32((const char*)&_msg_buff[18], 4, roll_raw)) {
860 0 : return;
861 : }
862 2314 : const int16_t yaw_angle_cd = wrap_180_cd((int16_t)yaw_raw);
863 2314 : const int16_t pitch_angle_cd = -(int16_t)pitch_raw;
864 2314 : const int16_t roll_angle_cd = (int16_t)roll_raw;
865 :
866 : // convert cd to radians
867 2314 : _current_angle_rad.x = cd_to_rad(roll_angle_cd);
868 2314 : _current_angle_rad.y = cd_to_rad(pitch_angle_cd);
869 2314 : _current_angle_rad.z = cd_to_rad(yaw_angle_cd);
870 2314 : _last_current_angle_ms = AP_HAL::millis();
871 :
872 2314 : return;
873 : }
874 :
875 : // gimbal video information analysis
876 0 : void AP_Mount_Topotek::gimbal_record_analyse()
877 : {
878 0 : _recording = (_msg_buff[10] == '1' || _msg_buff[11] == '1');
879 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording %s", send_message_prefix, _recording ? "ON" : "OFF");
880 0 : }
881 :
882 : // information analysis of gimbal storage card
883 209 : void AP_Mount_Topotek::gimbal_sdcard_analyse()
884 : {
885 209 : if (('N' == _msg_buff[10]) && ('N' == _msg_buff[11]) && ('N' == _msg_buff[12]) && ('N' == _msg_buff[13])) {
886 : // memory card exception
887 0 : _sdcard_status = false;
888 0 : return;
889 : }
890 209 : _sdcard_status = true;
891 :
892 : // send UTC time to the camera
893 209 : if (_sent_time_count < 7) {
894 19 : if (send_time_to_gimbal()) {
895 7 : _sent_time_count++;
896 : }
897 : }
898 :
899 209 : return;
900 : }
901 :
902 : // gimbal tracking information analysis
903 209 : void AP_Mount_Topotek::gimbal_track_analyse()
904 : {
905 : // ignore tracking state if unchanged
906 209 : TrackingStatus tracking_state = (TrackingStatus)_msg_buff[11];
907 209 : if (tracking_state == _last_tracking_state) {
908 209 : return;
909 : }
910 0 : _last_tracking_state = tracking_state;
911 :
912 : // inform user
913 0 : const char* tracking_str = "tracking";
914 0 : switch (tracking_state) {
915 0 : case TrackingStatus::STOPPED_TRACKING:
916 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s stopped", send_message_prefix, tracking_str);
917 0 : _is_tracking = false;
918 0 : break;
919 0 : case TrackingStatus::WAITING_FOR_TRACKING:
920 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s waiting", send_message_prefix, tracking_str);
921 0 : _is_tracking = false;
922 0 : break;
923 0 : case TrackingStatus::TRACKING_IN_PROGRESS:
924 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s started", send_message_prefix, tracking_str);
925 0 : _is_tracking = true;
926 0 : break;
927 0 : case TrackingStatus::LENS_UNSUPPORT_TRACK:
928 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s unsupported lens", send_message_prefix, tracking_str);
929 0 : break;
930 : }
931 : }
932 :
933 : // gimbal distance information analysis
934 0 : void AP_Mount_Topotek::gimbal_dist_info_analyse()
935 : {
936 0 : if ('E' == _msg_buff[10] && 'R' == _msg_buff[11] && 'R' ==_msg_buff[12]) {
937 0 : _measure_dist_m = -1.0f;
938 0 : return;
939 : }
940 :
941 : // distance is in meters in the format, "12345.6" where each digit is in decimal
942 : uint8_t d0, d1, d2, d3, d4, d5;
943 0 : if (!hex_char_to_nibble(_msg_buff[10], d0) ||
944 0 : !hex_char_to_nibble(_msg_buff[11], d1) ||
945 0 : !hex_char_to_nibble(_msg_buff[12], d2) ||
946 0 : !hex_char_to_nibble(_msg_buff[13], d3) ||
947 0 : !hex_char_to_nibble(_msg_buff[14], d4) ||
948 0 : !hex_char_to_nibble(_msg_buff[16], d5)) {
949 0 : return;
950 : }
951 0 : _measure_dist_m = d0 * 10000.0 + d1 * 1000.0 + d2 * 100.0 + d3 * 10.0 + d4 + d5 * 0.1;
952 : }
953 :
954 : // gimbal basic information analysis
955 2 : void AP_Mount_Topotek::gimbal_version_analyse()
956 : {
957 : // version array with index 0=major, 1=minor, 2=patch
958 2 : uint8_t version[3] {};
959 :
960 : // extract firmware version
961 : // the version can be in the format "1.2.3" or "123"
962 : uint8_t data_buf_len;
963 2 : if (!hex_char_to_nibble(_msg_buff[5], data_buf_len)) {
964 0 : return;
965 : }
966 :
967 : // check for "."
968 2 : bool contains_period = false;
969 12 : for (uint8_t i = 0; i < data_buf_len; i++) {
970 10 : contains_period |= _msg_buff[10 + i] == '.';
971 : }
972 :
973 : // if contains period, extract version number
974 2 : uint32_t ver_num = 0;
975 2 : uint8_t ver_count = 0;
976 2 : if (contains_period) {
977 12 : for (uint8_t i = 0; i < data_buf_len; i++) {
978 10 : if (_msg_buff[10 + i] != '.') {
979 : uint8_t digit;
980 6 : if (!hex_char_to_nibble(_msg_buff[10 + i], digit)) {
981 0 : return;
982 : }
983 6 : ver_num = ver_num * 10 + digit;
984 : } else {
985 4 : version[ver_count++] = ver_num;
986 4 : ver_num = 0;
987 : }
988 10 : if (ver_count >= ARRAY_SIZE(version)) {
989 0 : break;
990 : }
991 : }
992 : } else {
993 : uint8_t d;
994 0 : if (data_buf_len >= 1) {
995 0 : if (!hex_char_to_nibble(_msg_buff[10], d)) { return; }
996 0 : version[0] = d;
997 : }
998 0 : if (data_buf_len >= 2) {
999 0 : if (!hex_char_to_nibble(_msg_buff[11], d)) { return; }
1000 0 : version[1] = d;
1001 : }
1002 0 : if (data_buf_len >= 3) {
1003 0 : if (!hex_char_to_nibble(_msg_buff[12], d)) { return; }
1004 0 : version[2] = d;
1005 : }
1006 : }
1007 2 : _firmware_ver = (version[2] << 16) | (version[1] << 8) | (version[0]);
1008 :
1009 : // display gimbal model and firmware version to user
1010 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s v%u.%u.%u",
1011 : send_message_prefix,
1012 : version[0], // major version
1013 : version[1], // minor version
1014 : version[2]); // patch version
1015 :
1016 2 : _got_gimbal_version = true;
1017 : }
1018 :
1019 : // gimbal model name message analysis
1020 2 : void AP_Mount_Topotek::gimbal_model_name_analyse()
1021 : {
1022 : uint8_t len;
1023 2 : if (!hex_char_to_nibble(_msg_buff[5], len)) {
1024 0 : return;
1025 : }
1026 2 : memset(_model_name, 0, sizeof(_model_name));
1027 2 : memcpy(_model_name, _msg_buff + 10, MIN((uint8_t)(sizeof(_model_name)-1), len));
1028 :
1029 : // display gimbal model name to user
1030 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_message_prefix, _model_name);
1031 :
1032 2 : _got_gimbal_model_name = true;
1033 : }
1034 :
1035 : // calculate checksum
1036 17500 : uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const
1037 : {
1038 17500 : uint8_t crc = 0;
1039 327346 : for (uint16_t i = 0; i<len; i++) {
1040 309846 : crc += cmd[i];
1041 : }
1042 17500 : return(crc);
1043 : }
1044 :
1045 : // hexadecimal to character conversion
1046 49764 : uint8_t AP_Mount_Topotek::hex2char(uint8_t data) const
1047 : {
1048 49764 : if ((9 >= data)) {
1049 32412 : return (data + '0');
1050 : } else {
1051 17352 : return (data - 10 + 'A');
1052 : }
1053 : }
1054 :
1055 :
1056 : // send a fixed length packet
1057 631 : bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value)
1058 : {
1059 : uint8_t databuff[3];
1060 631 : hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X", value);
1061 631 : return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1);
1062 : }
1063 :
1064 : // send variable length packet
1065 14764 : bool AP_Mount_Topotek::send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len)
1066 : {
1067 : // exit immediately if not initialised
1068 14764 : if (!_initialised) {
1069 0 : return false;
1070 : }
1071 :
1072 : // calculate and sanity check packet size
1073 14764 : const uint16_t packet_size = AP_MOUNT_TOPOTEK_PACKETLEN_MIN + databuff_len;
1074 14764 : if (packet_size > AP_MOUNT_TOPOTEK_PACKETLEN_MAX) {
1075 : debug("send_packet data buff too large");
1076 0 : return false;
1077 : }
1078 :
1079 : // check for sufficient space in outgoing buffer
1080 14764 : if (_uart->txspace() < packet_size) {
1081 : debug("tx buffer full");
1082 0 : return false;
1083 : }
1084 :
1085 : // create buffer for holding outgoing packet
1086 14764 : uint8_t send_buff[packet_size];
1087 14764 : uint8_t send_buff_ofs = 0;
1088 :
1089 : // packet header (bytes 0 ~ 2)
1090 14764 : send_buff[send_buff_ofs++] = '#';
1091 14764 : send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'T' : 't';
1092 14764 : send_buff[send_buff_ofs++] = (header == HeaderType::FIXED_LEN) ? 'P' : 'p';
1093 :
1094 : // address (bytes 3, 4)
1095 14764 : send_buff[send_buff_ofs++] = (uint8_t)source_address();
1096 14764 : send_buff[send_buff_ofs++] = (uint8_t)address;
1097 :
1098 : // data length (byte 5)
1099 14764 : send_buff[send_buff_ofs++] = hex2char(databuff_len);
1100 :
1101 : // control byte (byte 6)
1102 14764 : send_buff[send_buff_ofs++] = write ? (uint8_t)ControlByte::WRITE : (uint8_t)ControlByte::READ;
1103 :
1104 : // identified (bytes 7 ~ 9)
1105 14764 : send_buff[send_buff_ofs++] = id[0];
1106 14764 : send_buff[send_buff_ofs++] = id[1];
1107 14764 : send_buff[send_buff_ofs++] = id[2];
1108 :
1109 : // data
1110 14764 : if (databuff_len != 0) {
1111 14764 : memcpy(&send_buff[send_buff_ofs], databuff, databuff_len);
1112 14764 : send_buff_ofs += databuff_len;
1113 : }
1114 :
1115 : // crc
1116 14764 : uint8_t crc = calculate_crc(send_buff, send_buff_ofs);
1117 14764 : send_buff[send_buff_ofs++] = hex2char((crc >> 4) & 0x0f);
1118 14764 : send_buff[send_buff_ofs++] = hex2char(crc & 0x0f);
1119 :
1120 : // send packet
1121 14764 : _uart->write(send_buff, send_buff_ofs);
1122 14764 : return true;
1123 14764 : }
1124 :
1125 : // set gimbal's lock vs follow mode
1126 : // lock should be true if gimbal should maintain an earth-frame target
1127 : // lock is false to follow / maintain a body-frame target
1128 2082 : bool AP_Mount_Topotek::set_gimbal_lock(bool lock)
1129 : {
1130 2082 : if (_last_lock == lock) {
1131 2082 : return true;
1132 : }
1133 :
1134 : // send message and update lock state
1135 0 : if (send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE, true, lock ? 6 : 7)) {
1136 0 : _last_lock = lock;
1137 0 : return true;
1138 : }
1139 0 : return false;
1140 : }
1141 :
1142 : #endif // HAL_MOUNT_TOPOTEK_ENABLED
|
