Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_SIYI_ENABLED
4 :
5 : #include "AP_Mount_Siyi.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_AHRS/AP_AHRS.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
11 : #include <AP_RTC/AP_RTC.h>
12 :
13 : extern const AP_HAL::HAL& hal;
14 :
15 : #define AP_MOUNT_SIYI_HEADER1 0x55 // first header byte
16 : #define AP_MOUNT_SIYI_HEADER2 0x66 // second header byte
17 : #define AP_MOUNT_SIYI_PACKETLEN_MIN 10 // minimum number of bytes in a packet. this is a packet with no data bytes
18 : #define AP_MOUNT_SIYI_DATALEN_MAX (AP_MOUNT_SIYI_PACKETLEN_MAX-AP_MOUNT_SIYI_PACKETLEN_MIN) // max bytes for data portion of packet
19 : #define AP_MOUNT_SIYI_RATE_MAX_RADS radians(90) // maximum physical rotation rate of gimbal in radans/sec
20 : #define AP_MOUNT_SIYI_PITCH_P 1.50 // pitch controller P gain (converts pitch angle error to target rate)
21 : #define AP_MOUNT_SIYI_YAW_P 1.50 // yaw controller P gain (converts yaw angle error to target rate)
22 : #define AP_MOUNT_SIYI_TIMEOUT_MS 1000 // timeout for health and rangefinder readings
23 : #define AP_MOUNT_SIYI_THERM_TIMEOUT_MS 3000// timeout for temp min/max readings
24 :
25 : #define AP_MOUNT_SIYI_DEBUG 0
26 : #define debug(fmt, args ...) do { if (AP_MOUNT_SIYI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Siyi: " fmt, ## args); } } while (0)
27 :
28 : // hardware lookup table indexed by HardwareModel enum values
29 : const AP_Mount_Siyi::HWInfo AP_Mount_Siyi::hardware_lookup_table[] {
30 : {{'0','0'}, "Unknown"},
31 : {{'7','5'}, "A2"},
32 : {{'7','3'}, "A8"},
33 : {{'6','B'}, "ZR10"},
34 : {{'7','8'}, "ZR30"},
35 : {{'8','3'}, "ZT6"},
36 : {{'7','A'}, "ZT30"},
37 : };
38 :
39 : // update mount position - should be called periodically
40 129 : void AP_Mount_Siyi::update()
41 : {
42 129 : AP_Mount_Backend::update();
43 :
44 : // exit immediately if not initialised
45 129 : if (!_initialised) {
46 0 : return;
47 : }
48 :
49 : // reading incoming packets from gimbal
50 129 : read_incoming_packets();
51 :
52 : // request firmware version during startup at 1hz
53 : // during regular operation request configuration at 1hz
54 129 : uint32_t now_ms = AP_HAL::millis();
55 129 : if ((now_ms - _last_send_ms) >= 1000) {
56 3 : _last_send_ms = now_ms;
57 3 : if (!_got_hardware_id) {
58 1 : request_hardware_id();
59 1 : return;
60 2 : } else if (!_fw_version.received) {
61 1 : request_firmware_version();
62 1 : return;
63 : } else {
64 1 : request_configuration();
65 : }
66 :
67 : #if AP_RTC_ENABLED
68 : // send UTC time to the camera
69 1 : if (sent_time_count < 5) {
70 : uint64_t utc_usec;
71 1 : if (AP::rtc().get_utc_usec(utc_usec) &&
72 0 : send_packet(SiyiCommandId::SET_TIME, (const uint8_t *)&utc_usec, sizeof(utc_usec))) {
73 0 : sent_time_count++;
74 : }
75 : }
76 : #endif
77 : }
78 :
79 : // request attitude at regular intervals
80 127 : if ((now_ms - _last_req_current_angle_rad_ms) >= 50) {
81 43 : request_gimbal_attitude();
82 43 : _last_req_current_angle_rad_ms = now_ms;
83 : }
84 :
85 : // request rangefinder distance from ZT30 at 10hz
86 127 : if ((_hardware_model == HardwareModel::ZT30) && (now_ms - _last_rangefinder_req_ms > 100)) {
87 22 : request_rangefinder_distance();
88 22 : _last_rangefinder_req_ms = now_ms;
89 : }
90 :
91 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
92 : // request thermal min/max from ZT30 or ZT6
93 127 : request_thermal_minmax();
94 : #endif
95 :
96 : // send attitude to gimbal at 10Hz
97 127 : if (now_ms - _last_attitude_send_ms > 100) {
98 22 : _last_attitude_send_ms = now_ms;
99 22 : send_attitude_position();
100 : }
101 :
102 : // run zoom control
103 127 : update_zoom_control();
104 :
105 : // update based on mount mode
106 127 : update_mnt_target();
107 :
108 : // send target angles or rates depending on the target type
109 127 : send_target_to_gimbal();
110 : }
111 :
112 : // return true if healthy
113 2 : bool AP_Mount_Siyi::healthy() const
114 : {
115 : // unhealthy until gimbal has been found and replied with firmware version info
116 2 : if (!_initialised || !_fw_version.received) {
117 1 : return false;
118 : }
119 :
120 : // unhealthy if attitude information NOT received recently
121 1 : const uint32_t now_ms = AP_HAL::millis();
122 1 : if (now_ms - _last_current_angle_rad_ms > AP_MOUNT_SIYI_TIMEOUT_MS) {
123 0 : return false;
124 : }
125 :
126 : // if we get this far return healthy
127 1 : return true;
128 : }
129 :
130 : // get attitude as a quaternion. returns true on success
131 38 : bool AP_Mount_Siyi::get_attitude_quaternion(Quaternion& att_quat)
132 : {
133 : // Order of rotation for Siyi gimbals is (yaw, roll, pitch), which is 312 order
134 38 : att_quat.from_vector312(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z);
135 38 : return true;
136 : }
137 :
138 : // reading incoming packets from gimbal and confirm they are of the correct format
139 : // results are held in the _parsed_msg structure
140 129 : void AP_Mount_Siyi::read_incoming_packets()
141 : {
142 : // check for bytes on the serial port
143 129 : int16_t nbytes = MIN(_uart->available(), 1024U);
144 129 : if (nbytes <= 0 ) {
145 84 : return;
146 : }
147 :
148 : // flag to allow cases below to reset parser state
149 45 : bool reset_parser = false;
150 :
151 : // process bytes received
152 1289 : for (int16_t i = 0; i < nbytes; i++) {
153 : uint8_t b;
154 1244 : if (!_uart->read(b)) {
155 0 : continue;
156 : }
157 :
158 1244 : _msg_buff[_msg_buff_len++] = b;
159 :
160 : // protect against overly long messages
161 1244 : if (_msg_buff_len >= AP_MOUNT_SIYI_PACKETLEN_MAX) {
162 0 : reset_parser = true;
163 : }
164 :
165 : // process byte depending upon current state
166 1244 : switch (_parsed_msg.state) {
167 :
168 67 : case ParseState::WAITING_FOR_HEADER_LOW:
169 67 : if (b == AP_MOUNT_SIYI_HEADER1) {
170 67 : _parsed_msg.state = ParseState::WAITING_FOR_HEADER_HIGH;
171 : } else {
172 0 : reset_parser = true;
173 : }
174 67 : break;
175 :
176 67 : case ParseState::WAITING_FOR_HEADER_HIGH:
177 67 : if (b == AP_MOUNT_SIYI_HEADER2) {
178 67 : _parsed_msg.state = ParseState::WAITING_FOR_CTRL;
179 : } else {
180 0 : reset_parser = true;
181 : }
182 67 : break;
183 :
184 67 : case ParseState::WAITING_FOR_CTRL:
185 67 : _parsed_msg.state = ParseState::WAITING_FOR_DATALEN_LOW;
186 67 : break;
187 :
188 67 : case ParseState::WAITING_FOR_DATALEN_LOW:
189 67 : _parsed_msg.data_len = b;
190 67 : _parsed_msg.state = ParseState::WAITING_FOR_DATALEN_HIGH;
191 67 : break;
192 :
193 67 : case ParseState::WAITING_FOR_DATALEN_HIGH:
194 67 : _parsed_msg.data_len |= ((uint16_t)b << 8);
195 : // sanity check data length
196 67 : if (_parsed_msg.data_len <= AP_MOUNT_SIYI_DATALEN_MAX) {
197 67 : _parsed_msg.state = ParseState::WAITING_FOR_SEQ_LOW;
198 : } else {
199 0 : reset_parser = true;
200 : debug("data len too large:%u (>%u)", (unsigned)_parsed_msg.data_len, (unsigned)AP_MOUNT_SIYI_DATALEN_MAX);
201 : }
202 67 : break;
203 :
204 67 : case ParseState::WAITING_FOR_SEQ_LOW:
205 67 : _parsed_msg.state = ParseState::WAITING_FOR_SEQ_HIGH;
206 67 : break;
207 :
208 67 : case ParseState::WAITING_FOR_SEQ_HIGH:
209 67 : _parsed_msg.state = ParseState::WAITING_FOR_CMDID;
210 67 : break;
211 :
212 67 : case ParseState::WAITING_FOR_CMDID:
213 67 : _parsed_msg.command_id = b;
214 67 : _parsed_msg.data_bytes_received = 0;
215 67 : if (_parsed_msg.data_len > 0) {
216 67 : _parsed_msg.state = ParseState::WAITING_FOR_DATA;
217 : } else {
218 0 : _parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
219 : }
220 67 : break;
221 :
222 574 : case ParseState::WAITING_FOR_DATA:
223 574 : _parsed_msg.data_bytes_received++;
224 574 : if (_parsed_msg.data_bytes_received >= _parsed_msg.data_len) {
225 67 : _parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
226 : }
227 574 : break;
228 :
229 67 : case ParseState::WAITING_FOR_CRC_LOW:
230 67 : _parsed_msg.crc16 = b;
231 67 : _parsed_msg.state = ParseState::WAITING_FOR_CRC_HIGH;
232 67 : break;
233 :
234 67 : case ParseState::WAITING_FOR_CRC_HIGH:
235 67 : _parsed_msg.crc16 |= ((uint16_t)b << 8);
236 :
237 : // check crc
238 67 : const uint16_t expected_crc = crc16_ccitt(_msg_buff, _msg_buff_len-2, 0);
239 67 : if (expected_crc == _parsed_msg.crc16) {
240 : // successfully received a message, do something with it
241 67 : process_packet();
242 : #if AP_MOUNT_SIYI_DEBUG
243 : } else {
244 : debug("crc expected:%x got:%x", (unsigned)expected_crc, (unsigned)_parsed_msg.crc16);
245 : #endif
246 : }
247 67 : reset_parser = true;
248 67 : break;
249 : }
250 :
251 : // handle reset of parser
252 1244 : if (reset_parser) {
253 67 : _parsed_msg.state = ParseState::WAITING_FOR_HEADER_LOW;
254 67 : _msg_buff_len = 0;
255 67 : reset_parser = false;
256 : }
257 : }
258 : }
259 :
260 : // process successfully decoded packets held in the _parsed_msg structure
261 67 : void AP_Mount_Siyi::process_packet()
262 : {
263 : #if AP_MOUNT_SIYI_DEBUG
264 : // flag to warn of unexpected data buffer length
265 : bool unexpected_len = false;
266 : #endif
267 :
268 : // process packet depending upon command id
269 67 : switch ((SiyiCommandId)_parsed_msg.command_id) {
270 :
271 1 : case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION: {
272 1 : if (_parsed_msg.data_bytes_received != 12 && // ZR10 firmware version reply is 12bytes
273 0 : _parsed_msg.data_bytes_received != 8) { // A8 firmware version reply is 8 bytes
274 : #if AP_MOUNT_SIYI_DEBUG
275 : unexpected_len = true;
276 : #endif
277 0 : break;
278 : }
279 :
280 : // consume and display camera firmware version
281 1 : _fw_version = {};
282 1 : _fw_version.camera.major = _msg_buff[_msg_buff_data_start+2]; // firmware major version
283 1 : _fw_version.camera.minor = _msg_buff[_msg_buff_data_start+1]; // firmware minor version
284 1 : _fw_version.camera.patch = _msg_buff[_msg_buff_data_start+0]; // firmware revision (aka patch)
285 :
286 1 : _fw_version.gimbal.major = _msg_buff[_msg_buff_data_start+6]; // firmware major version
287 1 : _fw_version.gimbal.minor = _msg_buff[_msg_buff_data_start+5]; // firmware minor version
288 1 : _fw_version.gimbal.patch = _msg_buff[_msg_buff_data_start+4]; // firmware revision (aka patch)
289 :
290 : // camera firmware version may be all zero soon after startup. giveup and try again later
291 1 : if (_fw_version.camera.major == 0 && _fw_version.camera.minor == 0 && _fw_version.camera.patch == 0) {
292 0 : break;
293 : }
294 :
295 1 : _fw_version.received = true;
296 :
297 : // display camera info to user
298 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: Siyi camera fw v%u.%u.%u",
299 : _fw_version.camera.major,
300 : _fw_version.camera.minor,
301 : _fw_version.camera.patch);
302 :
303 : // display gimbal info to user
304 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: Siyi gimbal fw v%u.%u.%u",
305 : _fw_version.gimbal.major,
306 : _fw_version.gimbal.minor,
307 : _fw_version.gimbal.patch);
308 :
309 : // display zoom firmware version for those that have it
310 1 : if (_parsed_msg.data_bytes_received >= 12) {
311 1 : _fw_version.zoom.major = _msg_buff[_msg_buff_data_start+10];
312 1 : _fw_version.zoom.minor = _msg_buff[_msg_buff_data_start+ 9];
313 1 : _fw_version.zoom.patch = _msg_buff[_msg_buff_data_start+ 8];
314 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: Siyi zoom fw v%u.%u.%u",
315 : _fw_version.zoom.major,
316 : _fw_version.zoom.minor,
317 : _fw_version.zoom.patch);
318 : }
319 :
320 : // report to the user if gimbal firmware is not up-to-date
321 1 : check_firmware_version();
322 :
323 1 : break;
324 : }
325 :
326 1 : case SiyiCommandId::HARDWARE_ID: {
327 : // lookup first two digits of hardware id
328 1 : const uint8_t hwid0 = _msg_buff[_msg_buff_data_start];
329 1 : const uint8_t hwid1 = _msg_buff[_msg_buff_data_start+1];
330 7 : for (uint8_t i=1; i<ARRAY_SIZE(hardware_lookup_table); i++) {
331 6 : if (hwid0 == hardware_lookup_table[i].hwid[0] && hwid1 == hardware_lookup_table[i].hwid[1]) {
332 1 : _hardware_model = (HardwareModel)i;
333 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: Siyi %s", get_model_name());
334 : }
335 : }
336 1 : _got_hardware_id = true;
337 1 : break;
338 : }
339 :
340 0 : case SiyiCommandId::AUTO_FOCUS:
341 : #if AP_MOUNT_SIYI_DEBUG
342 : if (_parsed_msg.data_bytes_received != 1) {
343 : unexpected_len = true;
344 : break;
345 : }
346 : debug("AutoFocus:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
347 : #endif
348 0 : break;
349 :
350 0 : case SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS: {
351 0 : if (_parsed_msg.data_bytes_received != 2) {
352 : #if AP_MOUNT_SIYI_DEBUG
353 : unexpected_len = true;
354 : #endif
355 0 : break;
356 : }
357 0 : _zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1;
358 : debug("ZoomMult:%4.1f", (double)_zoom_mult);
359 0 : break;
360 : }
361 :
362 0 : case SiyiCommandId::MANUAL_FOCUS:
363 : #if AP_MOUNT_SIYI_DEBUG
364 : if (_parsed_msg.data_bytes_received != 1) {
365 : unexpected_len = true;
366 : break;
367 : }
368 : debug("ManualFocus:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
369 : #endif
370 0 : break;
371 :
372 0 : case SiyiCommandId::GIMBAL_ROTATION:
373 : #if AP_MOUNT_SIYI_DEBUG
374 : if (_parsed_msg.data_bytes_received != 1) {
375 : unexpected_len = true;
376 : break;
377 : }
378 : debug("GimbRot:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
379 : #endif
380 0 : break;
381 :
382 0 : case SiyiCommandId::CENTER:
383 : #if AP_MOUNT_SIYI_DEBUG
384 : if (_parsed_msg.data_bytes_received != 1) {
385 : unexpected_len = true;
386 : break;
387 : }
388 : debug("Center:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
389 : #endif
390 0 : break;
391 :
392 0 : case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO: {
393 0 : const RecordingStatus prev_record_status = _config_info.record_status;
394 :
395 : // Update Gimbal Config Information
396 0 : size_t config_sz = MIN(_parsed_msg.data_bytes_received, sizeof(_config_info));
397 0 : memcpy(&_config_info, &_msg_buff[_msg_buff_data_start], config_sz);
398 :
399 : // Alert user if recording status changed
400 0 : if (prev_record_status != _config_info.record_status) {
401 0 : const char * msg = "?";
402 0 : MAV_SEVERITY sev = MAV_SEVERITY_INFO;
403 0 : switch (_config_info.record_status) {
404 0 : case RecordingStatus::OFF:
405 0 : msg = "OFF";
406 0 : break;
407 0 : case RecordingStatus::ON:
408 0 : msg = "ON";
409 0 : break;
410 0 : case RecordingStatus::NO_CARD:
411 0 : msg = "NO CARD!";
412 0 : sev = MAV_SEVERITY_WARNING;
413 0 : break;
414 0 : case RecordingStatus::DATA_LOSS:
415 0 : msg = "DATA LOSS!";
416 0 : sev = MAV_SEVERITY_WARNING;
417 0 : break;
418 : }
419 : (void)msg; // in case GCS_SEND_TEXT not available
420 : (void)sev; // in case GCS_SEND_TEXT not available
421 0 : GCS_SEND_TEXT(sev, "Siyi: recording %s", msg);
422 : }
423 :
424 : debug(
425 : "GimConf hdr:%u rec:%u foll:%u mntdir:%u vid:%u",
426 : (uint8_t)_config_info.hdr_status,
427 : (uint8_t)_config_info.record_status,
428 : (uint8_t)_config_info.motion_mode,
429 : (uint8_t)_config_info.mounting_dir,
430 : (uint8_t)_config_info.video_mode
431 : );
432 0 : break;
433 : }
434 :
435 0 : case SiyiCommandId::FUNCTION_FEEDBACK_INFO: {
436 0 : if (_parsed_msg.data_bytes_received != 1) {
437 : #if AP_MOUNT_SIYI_DEBUG
438 : unexpected_len = true;
439 : #endif
440 0 : break;
441 : }
442 0 : const uint8_t func_feedback_info = _msg_buff[_msg_buff_data_start];
443 0 : const char* err_prefix = "Mount: Siyi";
444 : (void)err_prefix; // in case !HAL_GCS_ENABLED
445 : switch ((FunctionFeedbackInfo)func_feedback_info) {
446 0 : case FunctionFeedbackInfo::SUCCESS:
447 : debug("FnFeedB success");
448 0 : break;
449 0 : case FunctionFeedbackInfo::FAILED_TO_TAKE_PHOTO:
450 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s failed to take picture", err_prefix);
451 0 : break;
452 0 : case FunctionFeedbackInfo::HDR_ON:
453 : debug("HDR on");
454 0 : break;
455 0 : case FunctionFeedbackInfo::HDR_OFF:
456 : debug("HDR off");
457 0 : break;
458 0 : case FunctionFeedbackInfo::FAILED_TO_RECORD_VIDEO:
459 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s failed to record video", err_prefix);
460 0 : break;
461 0 : default:
462 : debug("FnFeedB unexpected val:%u", (unsigned)func_feedback_info);
463 : }
464 0 : break;
465 : }
466 :
467 0 : case SiyiCommandId::PHOTO:
468 : // no ack should ever be sent by the gimbal
469 0 : break;
470 :
471 43 : case SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE: {
472 43 : if (_parsed_msg.data_bytes_received != 12) {
473 : #if AP_MOUNT_SIYI_DEBUG
474 : unexpected_len = true;
475 : #endif
476 0 : break;
477 : }
478 43 : _last_current_angle_rad_ms = AP_HAL::millis();
479 43 : _current_angle_rad.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1); // yaw angle
480 43 : _current_angle_rad.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.1); // pitch angle
481 43 : _current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]) * 0.1); // roll angle
482 43 : _current_rates_rads.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1); // yaw rate
483 43 : _current_rates_rads.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1); // pitch rate
484 43 : _current_rates_rads.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1); // roll rate
485 43 : break;
486 : }
487 :
488 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
489 0 : case SiyiCommandId::GET_TEMP_FULL_IMAGE: {
490 0 : if (_parsed_msg.data_bytes_received != 12) {
491 : #if AP_MOUNT_SIYI_DEBUG
492 : unexpected_len = true;
493 : #endif
494 0 : break;
495 : }
496 0 : _thermal.last_update_ms = AP_HAL::millis();
497 0 : _thermal.max_C = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.01;
498 0 : _thermal.min_C = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.01;
499 0 : _thermal.max_pos.x = UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]);
500 0 : _thermal.max_pos.y = UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]);
501 0 : _thermal.min_pos.x = UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]);
502 0 : _thermal.min_pos.y = UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]);
503 0 : break;
504 : }
505 : #endif
506 :
507 22 : case SiyiCommandId::READ_RANGEFINDER: {
508 22 : _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]);
509 22 : _last_rangefinder_dist_ms = AP_HAL::millis();
510 22 : break;
511 : }
512 :
513 0 : default:
514 : debug("Unhandled CmdId:%u", (unsigned)_parsed_msg.command_id);
515 0 : break;
516 : }
517 :
518 : #if AP_MOUNT_SIYI_DEBUG
519 : // handle unexpected data buffer length
520 : if (unexpected_len) {
521 : debug("CmdId:%u unexpected len:%u", (unsigned)_parsed_msg.command_id, (unsigned)_parsed_msg.data_bytes_received);
522 : }
523 : #endif
524 67 : }
525 :
526 : // methods to send commands to gimbal
527 : // returns true on success, false if outgoing serial buffer is full
528 220 : bool AP_Mount_Siyi::send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len)
529 : {
530 220 : if (!_initialised) {
531 0 : return false;
532 : }
533 : // calculate and sanity check packet size
534 220 : const uint16_t packet_size = AP_MOUNT_SIYI_PACKETLEN_MIN + databuff_len;
535 220 : if (packet_size > AP_MOUNT_SIYI_PACKETLEN_MAX) {
536 : debug("send_packet data buff too large");
537 0 : return false;
538 : }
539 :
540 : // check for sufficient space in outgoing buffer
541 220 : if (_uart->txspace() < packet_size) {
542 0 : return false;
543 : }
544 :
545 : // buffer for holding outgoing packet
546 220 : uint8_t send_buff[packet_size];
547 220 : uint8_t send_buff_ofs = 0;
548 :
549 : // packet header
550 220 : send_buff[send_buff_ofs++] = AP_MOUNT_SIYI_HEADER1;
551 220 : send_buff[send_buff_ofs++] = AP_MOUNT_SIYI_HEADER2;
552 :
553 : // CTRL. Always request ACK
554 220 : send_buff[send_buff_ofs++] = 1;
555 :
556 : // Data_len. protocol supports uint16_t but messages are never longer than 22 bytes
557 220 : send_buff[send_buff_ofs++] = databuff_len;
558 220 : send_buff[send_buff_ofs++] = 0;
559 :
560 : // SEQ (sequence)
561 220 : send_buff[send_buff_ofs++] = LOWBYTE(_last_seq);
562 220 : send_buff[send_buff_ofs++] = HIGHBYTE(_last_seq++);
563 :
564 : // CMD_ID
565 220 : send_buff[send_buff_ofs++] = (uint8_t)cmd_id;
566 :
567 : // DATA
568 220 : if (databuff_len != 0) {
569 151 : memcpy(&send_buff[send_buff_ofs], databuff, databuff_len);
570 151 : send_buff_ofs += databuff_len;
571 : }
572 :
573 : // CRC16
574 220 : const uint16_t crc = crc16_ccitt(send_buff, send_buff_ofs, 0);
575 220 : send_buff[send_buff_ofs++] = LOWBYTE(crc);
576 220 : send_buff[send_buff_ofs++] = HIGHBYTE(crc);
577 :
578 : // send packet
579 220 : _uart->write(send_buff, send_buff_ofs);
580 :
581 220 : return true;
582 220 : }
583 :
584 : // send a packet with a single data byte to gimbal
585 : // returns true on success, false if outgoing serial buffer is full
586 2 : bool AP_Mount_Siyi::send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte)
587 : {
588 2 : return send_packet(cmd_id, &data_byte, 1);
589 : }
590 :
591 : // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100
592 : // yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
593 127 : void AP_Mount_Siyi::rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef)
594 : {
595 : // send lock/follow value
596 127 : const GimbalMotionMode mode = yaw_is_ef ? GimbalMotionMode::LOCK : GimbalMotionMode::FOLLOW;
597 127 : if (!set_motion_mode(mode)) {
598 : // couldn't set mode, so don't send rotation
599 0 : return;
600 : }
601 :
602 127 : const uint8_t yaw_and_pitch_rates[] {(uint8_t)yaw_scalar, (uint8_t)pitch_scalar};
603 127 : send_packet(SiyiCommandId::GIMBAL_ROTATION, yaw_and_pitch_rates, ARRAY_SIZE(yaw_and_pitch_rates));
604 : }
605 :
606 : // Set gimbal's motion mode if it has changed. Use force=true to always send.
607 : // FOLLOW: roll and pitch are in earth-frame, yaw is in body-frame
608 : // LOCK: roll, pitch and yaw are all in earth-frame
609 : // FPV: roll, pitch and yaw are all in body-frame
610 : // Returns true if mode successfully sent to Gimbal
611 127 : bool AP_Mount_Siyi::set_motion_mode(const GimbalMotionMode mode, const bool force)
612 : {
613 127 : if (!force && (mode == _config_info.motion_mode)) {
614 : // we're already in the right mode...
615 126 : return true;
616 : }
617 :
618 1 : PhotoFunction data = PhotoFunction::LOCK_MODE;
619 1 : switch (mode) {
620 0 : case GimbalMotionMode::LOCK: data = PhotoFunction::LOCK_MODE; break;
621 1 : case GimbalMotionMode::FOLLOW: data = PhotoFunction::FOLLOW_MODE; break;
622 0 : case GimbalMotionMode::FPV: data = PhotoFunction::FPV_MODE; break;
623 : }
624 1 : bool sent = send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)data);
625 1 : if (sent) {
626 : // assume the mode is set correctly until told otherwise
627 1 : _config_info.motion_mode = mode;
628 1 : request_configuration();
629 : }
630 1 : return sent;
631 : }
632 :
633 : // send target pitch and yaw rates to gimbal
634 0 : void AP_Mount_Siyi::send_target_rates(const MountRateTarget &rate_rads)
635 : {
636 0 : const float pitch_rads = rate_rads.pitch;
637 0 : const float yaw_rads = rate_rads.yaw;
638 0 : const bool yaw_is_ef = rate_rads.yaw_is_ef;
639 :
640 0 : const float pitch_rate_scalar = constrain_float(100.0 * pitch_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
641 0 : const float yaw_rate_scalar = constrain_float(100.0 * yaw_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
642 0 : rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef);
643 0 : }
644 :
645 : // send target pitch and yaw angles to gimbal
646 127 : void AP_Mount_Siyi::send_target_angles(const MountAngleTarget &angle_rad)
647 : {
648 127 : const float pitch_rad = angle_rad.pitch;
649 127 : const float yaw_rad = angle_rad.yaw;
650 127 : bool yaw_is_ef = angle_rad.yaw_is_ef;
651 :
652 : // stop gimbal if no recent actual angles
653 127 : uint32_t now_ms = AP_HAL::millis();
654 127 : if (now_ms - _last_current_angle_rad_ms >= 200) {
655 1 : rotate_gimbal(0, 0, false);
656 1 : return;
657 : }
658 :
659 : // if gimbal mounting direction is 2 i.e. upside down, then transform the angles
660 126 : Vector3f current_angle_transformed = _current_angle_rad;
661 126 : if (_config_info.mounting_dir == GimbalMountingDirection::UPSIDE_DOWN) {
662 0 : current_angle_transformed.y = -wrap_PI(_current_angle_rad.y + M_PI);
663 0 : current_angle_transformed.z = -_current_angle_rad.z;
664 : }
665 :
666 : // use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100)
667 126 : const float pitch_err_rad = (pitch_rad - current_angle_transformed.y);
668 126 : const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
669 :
670 : // convert yaw angle to body-frame
671 126 : float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw_rad()) : yaw_rad;
672 :
673 : // enforce body-frame yaw angle limits. If beyond limits always use body-frame control
674 126 : const float yaw_bf_min = radians(_params.yaw_angle_min);
675 126 : const float yaw_bf_max = radians(_params.yaw_angle_max);
676 126 : if (yaw_bf_rad < yaw_bf_min || yaw_bf_rad > yaw_bf_max) {
677 0 : yaw_bf_rad = constrain_float(yaw_bf_rad, yaw_bf_min, yaw_bf_max);
678 0 : yaw_is_ef = false;
679 : }
680 :
681 : // use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100)
682 126 : const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z);
683 126 : const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
684 :
685 : // rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100
686 126 : rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef);
687 : }
688 :
689 : // take a picture. returns true on success
690 0 : bool AP_Mount_Siyi::take_picture()
691 : {
692 0 : return send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)PhotoFunction::TAKE_PICTURE);
693 : }
694 :
695 : // start or stop video recording. returns true on success
696 : // set start_recording = true to start record, false to stop recording
697 0 : bool AP_Mount_Siyi::record_video(bool start_recording)
698 : {
699 : // exit immediately if not initialised to reduce mismatch
700 : // between internal and actual state of recording
701 0 : if (!_initialised) {
702 0 : return false;
703 : }
704 :
705 0 : bool success = true;
706 0 : bool send_toggle = false;
707 0 : if (start_recording) {
708 0 : switch (_config_info.record_status) {
709 0 : case RecordingStatus::ON:
710 : // already recording...
711 0 : break;
712 : // assume that DATA_LOSS is the same as OFF
713 0 : case RecordingStatus::DATA_LOSS:
714 : case RecordingStatus::OFF:
715 0 : send_toggle = true;
716 0 : break;
717 0 : case RecordingStatus::NO_CARD:
718 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Siyi: can't start recording: No Card");
719 0 : success = false;
720 0 : break;
721 : }
722 : } else {
723 0 : switch (_config_info.record_status) {
724 0 : case RecordingStatus::ON:
725 0 : send_toggle = true;
726 0 : break;
727 : // assume that DATA_LOSS is the same as OFF
728 0 : case RecordingStatus::DATA_LOSS:
729 : case RecordingStatus::OFF:
730 : case RecordingStatus::NO_CARD:
731 : // already off...
732 0 : break;
733 : }
734 : }
735 :
736 0 : if (send_toggle) {
737 0 : success = send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)PhotoFunction::RECORD_VIDEO_TOGGLE);
738 : }
739 :
740 : // request recording state update from gimbal
741 0 : request_configuration();
742 :
743 0 : return success;
744 : }
745 :
746 : // send zoom rate command to camera. zoom out = -1, hold = 0, zoom in = 1
747 0 : bool AP_Mount_Siyi::send_zoom_rate(float zoom_value)
748 : {
749 0 : uint8_t zoom_step = 0;
750 0 : if (zoom_value > 0) {
751 : // zoom in
752 0 : zoom_step = 1;
753 : }
754 0 : if (zoom_value < 0) {
755 : // zoom out. Siyi API specifies -1 should be sent as 255
756 0 : zoom_step = UINT8_MAX;
757 : }
758 0 : return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, zoom_step);
759 : }
760 :
761 : // send zoom multiple command to camera. e.g. 1x, 10x, 30x
762 : // only works on ZR10 and ZR30
763 0 : bool AP_Mount_Siyi::send_zoom_mult(float zoom_mult)
764 : {
765 : // separate zoom_mult into integral and fractional parts
766 : float intpart;
767 0 : uint8_t fracpart = (uint8_t)constrain_int16(modf(zoom_mult, &intpart) * 10, 0, UINT8_MAX);
768 :
769 : // create and send 2 byte array
770 0 : const uint8_t zoom_mult_data[] {(uint8_t)(intpart), fracpart};
771 0 : return send_packet(SiyiCommandId::ABSOLUTE_ZOOM, zoom_mult_data, ARRAY_SIZE(zoom_mult_data));
772 : }
773 :
774 : // get zoom multiple max
775 0 : float AP_Mount_Siyi::get_zoom_mult_max() const
776 : {
777 0 : switch (_hardware_model) {
778 0 : case HardwareModel::UNKNOWN:
779 0 : return 0;
780 0 : case HardwareModel::A2:
781 : case HardwareModel::A8:
782 : case HardwareModel::ZT6:
783 : // a8, zt6 have 6x digital zoom
784 0 : return 6;
785 0 : case HardwareModel::ZR10:
786 : case HardwareModel::ZR30:
787 : case HardwareModel::ZT30:
788 : // 30x hybrid zoom (optical + digital)
789 0 : return 30;
790 : }
791 0 : return 0;
792 : }
793 :
794 : // set zoom specified as a rate or percentage
795 0 : bool AP_Mount_Siyi::set_zoom(ZoomType zoom_type, float zoom_value)
796 : {
797 0 : switch (zoom_type) {
798 0 : case ZoomType::RATE:
799 0 : if (send_zoom_rate(zoom_value)) {
800 0 : _zoom_type = zoom_type;
801 0 : _zoom_rate_target = zoom_value;
802 0 : return true;
803 : }
804 0 : return false;
805 0 : case ZoomType::PCT: {
806 : // absolute zoom
807 0 : float zoom_mult_max = get_zoom_mult_max();
808 0 : if (is_positive(zoom_mult_max)) {
809 : // convert zoom percentage (0~100) to target zoom multiple (e.g. 0~6x or 0~30x)
810 0 : const float zoom_mult = linear_interpolate(1, zoom_mult_max, zoom_value, 0, 100);
811 0 : if (send_zoom_mult(zoom_mult)) {
812 0 : _zoom_type = zoom_type;
813 0 : return true;
814 : }
815 0 : return false;
816 : }
817 0 : return false;
818 : }
819 : }
820 :
821 : // unsupported zoom type
822 0 : return false;
823 : }
824 :
825 : // update zoom controller
826 127 : void AP_Mount_Siyi::update_zoom_control()
827 : {
828 127 : if (_zoom_type == ZoomType::RATE) {
829 : // limit updates to 1hz
830 0 : const uint32_t now_ms = AP_HAL::millis();
831 0 : if (now_ms - _last_zoom_control_ms < 1000) {
832 0 : return;
833 : }
834 0 : _last_zoom_control_ms = now_ms;
835 :
836 : // only send zoom rate target if it's non-zero because if zero it has already been sent
837 : // and sending zero rate also triggers autofocus
838 0 : if (!is_zero(_zoom_rate_target)) {
839 0 : send_zoom_rate(_zoom_rate_target);
840 : }
841 : }
842 : }
843 :
844 : // set focus specified as rate, percentage or auto
845 : // focus in = -1, focus hold = 0, focus out = 1
846 0 : SetFocusResult AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value)
847 : {
848 0 : switch (focus_type) {
849 0 : case FocusType::RATE: {
850 0 : uint8_t focus_step = 0;
851 0 : if (focus_value > 0) {
852 0 : focus_step = 1;
853 0 : } else if (focus_value < 0) {
854 : // Siyi API specifies -1 should be sent as 255
855 0 : focus_step = UINT8_MAX;
856 : }
857 0 : if (!send_1byte_packet(SiyiCommandId::MANUAL_FOCUS, (uint8_t)focus_step)) {
858 0 : return SetFocusResult::FAILED;
859 : }
860 0 : return SetFocusResult::ACCEPTED;
861 : }
862 0 : case FocusType::PCT:
863 : // not supported
864 0 : return SetFocusResult::INVALID_PARAMETERS;
865 0 : case FocusType::AUTO:
866 0 : if (!send_1byte_packet(SiyiCommandId::AUTO_FOCUS, 1)) {
867 0 : return SetFocusResult::FAILED;
868 : }
869 0 : return SetFocusResult::ACCEPTED;
870 : }
871 :
872 : // unsupported focus type
873 0 : return SetFocusResult::INVALID_PARAMETERS;
874 : }
875 :
876 : // set camera lens as a value from 0 to 8
877 0 : bool AP_Mount_Siyi::set_lens(uint8_t lens)
878 : {
879 : CameraImageType selected_lens;
880 :
881 0 : switch (_hardware_model) {
882 :
883 0 : case HardwareModel::ZT30: {
884 : // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
885 : static const CameraImageType cam_image_type_table[] {
886 : CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3
887 : CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL, // 5
888 : CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7
889 : CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0
890 : CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1
891 : CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2
892 : CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE, // 4
893 : CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM, // 6
894 : CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE, // 8
895 : };
896 :
897 : // sanity check lens values
898 0 : if (lens >= ARRAY_SIZE(cam_image_type_table)) {
899 0 : return false;
900 : }
901 0 : selected_lens = cam_image_type_table[lens];
902 0 : break;
903 : }
904 :
905 0 : case HardwareModel::ZT6: {
906 : // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
907 : static const CameraImageType cam_image_type_table[] {
908 : CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3
909 : CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7
910 : CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0
911 : };
912 :
913 : // sanity check lens values
914 0 : if (lens >= ARRAY_SIZE(cam_image_type_table)) {
915 0 : return false;
916 : }
917 0 : selected_lens = cam_image_type_table[lens];
918 0 : break;
919 : }
920 :
921 0 : default:
922 : // set lens not supported on this camera
923 0 : return false;
924 : }
925 :
926 : // send desired image type to camera
927 0 : return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)selected_lens);
928 : }
929 :
930 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
931 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
932 0 : bool AP_Mount_Siyi::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
933 : {
934 : // only supported on ZT30. sanity check lens values
935 0 : if (_hardware_model != HardwareModel::ZT30) {
936 0 : return false;
937 : }
938 :
939 : // maps primary and secondary source to siyi camera image type
940 : CameraImageType cam_image_type;
941 0 : switch (primary_source) {
942 0 : case 0: // Default (RGB)
943 : FALLTHROUGH;
944 : case 1: // RGB
945 0 : switch (secondary_source) {
946 0 : case 0: // RGB + Default (None)
947 0 : cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; // 3
948 0 : break;
949 0 : case 2: // PIP RGB+IR
950 0 : cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE; // 0
951 0 : break;
952 0 : case 4: // PIP RGB+RGB_WIDEANGLE
953 0 : cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL; // 2
954 0 : break;
955 0 : default:
956 0 : return false;
957 : }
958 0 : break;
959 0 : case 2: // IR
960 0 : switch (secondary_source) {
961 0 : case 0: // IR + Default (None)
962 0 : cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM; // 7
963 0 : break;
964 0 : default:
965 0 : return false;
966 : }
967 0 : break;
968 0 : case 4: // RGB_WIDEANGLE
969 0 : switch (secondary_source) {
970 0 : case 0: // RGB_WIDEANGLE + Default (None)
971 0 : cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL; // 5
972 0 : break;
973 0 : case 2: // PIP RGB_WIDEANGLE+IR
974 0 : cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM; // 1
975 0 : break;
976 0 : default:
977 0 : return false;
978 : }
979 0 : break;
980 0 : default:
981 0 : return false;
982 : }
983 :
984 : // send desired image type to camera
985 0 : return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type);
986 : }
987 :
988 : // return focal length in mm for the current hardware model
989 2 : float AP_Mount_Siyi::get_camera_focal_length_mm() const
990 : {
991 : // To-Do: check these values are correct for A2, ZR30, ZT30
992 2 : switch (_hardware_model) {
993 0 : case HardwareModel::UNKNOWN:
994 : case HardwareModel::A2:
995 : case HardwareModel::A8:
996 : case HardwareModel::ZT6:
997 0 : return 21;
998 2 : case HardwareModel::ZR10:
999 : case HardwareModel::ZR30:
1000 : case HardwareModel::ZT30:
1001 : // focal length range from 5.15 ~ 47.38
1002 2 : return 5.15;
1003 : }
1004 0 : return 0;
1005 : }
1006 :
1007 : // send camera settings message to GCS
1008 0 : void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const
1009 : {
1010 0 : const uint8_t mode_id = (_config_info.record_status == RecordingStatus::ON) ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE;
1011 0 : const float zoom_mult_max = get_zoom_mult_max();
1012 0 : float zoom_pct = 0.0;
1013 0 : if (is_positive(zoom_mult_max)) {
1014 0 : zoom_pct = linear_interpolate(0, 100, _zoom_mult, 1.0, zoom_mult_max);
1015 : }
1016 :
1017 : // send CAMERA_SETTINGS message
1018 0 : mavlink_msg_camera_settings_send(
1019 : chan,
1020 : AP_HAL::millis(), // time_boot_ms
1021 : mode_id, // camera mode (0:image, 1:video, 2:image survey)
1022 : zoom_pct, // zoomLevel float, percentage from 0 to 100, NaN if unknown
1023 : NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
1024 0 : }
1025 :
1026 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
1027 : // send camera thermal range message to GCS
1028 0 : void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const
1029 : {
1030 0 : const uint32_t now_ms = AP_HAL::millis();
1031 0 : bool timeout = now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS;
1032 :
1033 : // send CAMERA_THERMAL_RANGE message
1034 0 : mavlink_msg_camera_thermal_range_send(
1035 : chan,
1036 : now_ms, // time_boot_ms
1037 0 : _instance + 1, // video stream id (assume one-to-one mapping with camera id)
1038 0 : _instance + 1, // camera device id
1039 : timeout ? NaNf : _thermal.max_C, // max in degC
1040 0 : timeout ? NaNf : _thermal.max_pos.x, // max x position
1041 0 : timeout ? NaNf : _thermal.max_pos.y, // max y position
1042 : timeout ? NaNf : _thermal.min_C, // min in degC
1043 0 : timeout ? NaNf : _thermal.min_pos.x, // min x position
1044 0 : timeout ? NaNf : _thermal.min_pos.y);// min y position
1045 0 : }
1046 : #endif
1047 :
1048 : // change camera settings not normally used by autopilot
1049 : // THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot
1050 : // THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C)
1051 : // THERMAL_RAW_DATA: 0:Disable Raw Data (30fps), 1:Enable Raw Data (25fps)
1052 0 : bool AP_Mount_Siyi::change_setting(CameraSetting setting, float value)
1053 : {
1054 0 : switch (setting) {
1055 0 : case CameraSetting::THERMAL_PALETTE:
1056 0 : return send_1byte_packet(SiyiCommandId::SET_THERMAL_PALETTE, (uint8_t)value);
1057 0 : case CameraSetting::THERMAL_GAIN:
1058 0 : return send_1byte_packet(SiyiCommandId::SET_THERMAL_GAIN, (uint8_t)value);
1059 0 : case CameraSetting::THERMAL_RAW_DATA:
1060 0 : return send_1byte_packet(SiyiCommandId::SET_THERMAL_RAW_DATA, (uint8_t)value);
1061 : }
1062 :
1063 : // invalid setting so return false
1064 0 : return false;
1065 : }
1066 :
1067 : // get model name string. returns "Unknown" if hardware model is not yet known
1068 3 : const char* AP_Mount_Siyi::get_model_name() const
1069 : {
1070 3 : uint8_t model_idx = (uint8_t)_hardware_model;
1071 3 : if (model_idx < ARRAY_SIZE(hardware_lookup_table)) {
1072 3 : return hardware_lookup_table[model_idx].model_name;
1073 : }
1074 0 : return hardware_lookup_table[0].model_name;
1075 : }
1076 :
1077 : // get rangefinder distance. Returns true on success
1078 25 : bool AP_Mount_Siyi::get_rangefinder_distance(float& distance_m) const
1079 : {
1080 : // only supported on ZT30
1081 25 : if (_hardware_model != HardwareModel::ZT30) {
1082 0 : return false;
1083 : }
1084 :
1085 : // unhealthy if distance not received recently
1086 25 : const uint32_t now_ms = AP_HAL::millis();
1087 25 : if (now_ms - _last_rangefinder_dist_ms > AP_MOUNT_SIYI_TIMEOUT_MS) {
1088 0 : return false;
1089 : }
1090 :
1091 25 : distance_m = _rangefinder_dist_m;
1092 25 : return true;
1093 : }
1094 :
1095 : // Checks that the firmware version on the Gimbal meets the minimum supported version.
1096 1 : void AP_Mount_Siyi::check_firmware_version() const
1097 : {
1098 1 : if (!_fw_version.received) {
1099 : debug("Can't check firmware if we haven't received it...");
1100 0 : return;
1101 : }
1102 :
1103 1 : if (!_got_hardware_id) {
1104 : debug("Can't check firmware without Hardware ID!");
1105 0 : return;
1106 : }
1107 :
1108 1 : FirmwareVersion minimum_ver {};
1109 1 : switch (_hardware_model) {
1110 0 : case HardwareModel::A8:
1111 0 : minimum_ver.camera = {.major = 0, .minor = 2, .patch = 1};
1112 0 : break;
1113 :
1114 0 : case HardwareModel::ZT6:
1115 0 : minimum_ver.camera = {.major = 0, .minor = 1, .patch = 9};
1116 0 : break;
1117 :
1118 1 : case HardwareModel::A2:
1119 : case HardwareModel::ZR10:
1120 : case HardwareModel::ZR30:
1121 : case HardwareModel::ZT30:
1122 : // TBD
1123 1 : break;
1124 :
1125 0 : case HardwareModel::UNKNOWN:
1126 : debug("Can't check FW on unknown hardware model!");
1127 0 : return;
1128 : }
1129 :
1130 1 : const uint32_t minimum_camera_val = (minimum_ver.camera.major << 16) + (minimum_ver.camera.minor << 8) + minimum_ver.camera.patch;
1131 1 : const uint32_t firmware_camera_val = (_fw_version.camera.major << 16) + (_fw_version.camera.minor << 8) + _fw_version.camera.patch;
1132 :
1133 1 : const bool is_camera_supported = firmware_camera_val >= minimum_camera_val;
1134 :
1135 1 : if (!is_camera_supported) {
1136 0 : GCS_SEND_TEXT(
1137 : MAV_SEVERITY_WARNING,
1138 : "Mount: Siyi running old camera fw (need v%u.%u.%u)",
1139 : minimum_ver.camera.major, minimum_ver.camera.minor, minimum_ver.camera.patch
1140 : );
1141 : }
1142 : }
1143 :
1144 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
1145 : // get thermal min/max if available at 5hz
1146 127 : void AP_Mount_Siyi::request_thermal_minmax()
1147 : {
1148 : // only supported on ZT6 and ZT30
1149 127 : if (_hardware_model != HardwareModel::ZT6 &&
1150 127 : _hardware_model != HardwareModel::ZT30) {
1151 0 : return;
1152 : }
1153 :
1154 : // check for timeout
1155 127 : uint32_t now_ms = AP_HAL::millis();
1156 127 : if ((now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS) &&
1157 100 : (now_ms - _thermal.last_req_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS)) {
1158 : // request thermal min/max at 5hz
1159 1 : send_1byte_packet(SiyiCommandId::GET_TEMP_FULL_IMAGE, 2);
1160 1 : _thermal.last_req_ms = now_ms;
1161 : }
1162 : }
1163 : #endif
1164 :
1165 : /*
1166 : send ArduPilot attitude and position to gimbal
1167 : */
1168 22 : void AP_Mount_Siyi::send_attitude_position(void)
1169 : {
1170 22 : const auto &ahrs = AP::ahrs();
1171 : struct {
1172 : uint32_t time_boot_ms;
1173 : float roll, pitch, yaw;
1174 : float rollspeed, pitchspeed, yawspeed;
1175 : } attitude;
1176 :
1177 : // get attitude as euler 321
1178 22 : const auto &gyro = ahrs.get_gyro();
1179 22 : const uint32_t now_ms = AP_HAL::millis();
1180 :
1181 22 : attitude.time_boot_ms = now_ms;
1182 22 : attitude.roll = ahrs.get_roll_rad();
1183 22 : attitude.pitch = ahrs.get_pitch_rad();
1184 22 : attitude.yaw = ahrs.get_yaw_rad();
1185 22 : attitude.rollspeed = gyro.x;
1186 22 : attitude.pitchspeed = gyro.y;
1187 22 : attitude.yawspeed = gyro.z;
1188 :
1189 22 : send_packet(SiyiCommandId::EXTERNAL_ATTITUDE, (const uint8_t *)&attitude, sizeof(attitude));
1190 :
1191 : // send location and velocity
1192 : struct {
1193 : uint32_t time_boot_ms;
1194 : int32_t lat, lon;
1195 : int32_t alt_msl, alt_ellipsoid;
1196 : Vector3l velocity_ned_int32;
1197 22 : } position;
1198 22 : Location loc;
1199 22 : Vector3f velocity_ned;
1200 22 : float undulation = 0;
1201 22 : if (!ahrs.get_location(loc) ||
1202 0 : !ahrs.get_velocity_NED(velocity_ned)) {
1203 22 : return;
1204 : }
1205 0 : AP::gps().get_undulation(undulation);
1206 :
1207 0 : position.time_boot_ms = now_ms;
1208 0 : position.lat = loc.lat;
1209 0 : position.lon = loc.lng;
1210 0 : position.alt_msl = loc.alt;
1211 0 : position.alt_ellipsoid = position.alt_msl - undulation*100;
1212 :
1213 : // convert velocity to int32 and scale to mm/s
1214 0 : position.velocity_ned_int32.x = velocity_ned.x * 1000;
1215 0 : position.velocity_ned_int32.y = velocity_ned.y * 1000;
1216 0 : position.velocity_ned_int32.z = velocity_ned.z * 1000;
1217 :
1218 0 : send_packet(SiyiCommandId::POSITION_DATA, (const uint8_t *)&position, sizeof(position));
1219 : }
1220 :
1221 : #endif // HAL_MOUNT_SIYI_ENABLED
|
