Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_VIEWPRO_ENABLED
4 :
5 : #include "AP_Mount_Viewpro.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_AHRS/AP_AHRS.h>
9 : #include <AP_GPS/AP_GPS.h>
10 : #include <AP_RTC/AP_RTC.h>
11 : #include <GCS_MAVLink/GCS.h>
12 : #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
13 :
14 : extern const AP_HAL::HAL& hal;
15 :
16 : #define AP_MOUNT_VIEWPRO_HEADER1 0x55 // first header byte
17 : #define AP_MOUNT_VIEWPRO_HEADER2 0xAA // second header byte
18 : #define AP_MOUNT_VIEWPRO_HEADER3 0xDC // third header byte
19 : #define AP_MOUNT_VIEWPRO_PACKETLEN_MIN 5 // min number of bytes in a packet (3 header bytes, length, crc)
20 : #define AP_MOUNT_VIEWPRO_DATALEN_MAX (AP_MOUNT_VIEWPRO_PACKETLEN_MAX-AP_MOUNT_VIEWPRO_PACKETLEN_MIN) // max bytes for data portion of packet
21 : #define AP_MOUNT_VIEWPRO_HEALTH_TIMEOUT_MS 1000 // state will become unhealthy if no attitude is received within this timeout
22 : #define AP_MOUNT_VIEWPRO_UPDATE_INTERVAL_MS 100 // resend angle or rate targets to gimbal at this interval
23 : #define AP_MOUNT_VIEWPRO_EO_ZOOM_SPEED 0x07 // hard-coded zoom speed (fast)
24 : #define AP_MOUNT_VIEWPRO_IR_ZOOM_SPEED 1
25 : #define AP_MOUNT_VIEWPRO_ZOOM_MAX 10 // hard-coded absolute zoom times max
26 : #define AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT (65536.0 / M_2PI) // scalar to convert radians to the viewpro angle scaling
27 : #define AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG (360.0 / 65536.0) // scalar to convert viewpro angle scaling to degrees
28 :
29 : #define AP_MOUNT_VIEWPRO_DEBUG 0
30 : #define debug(fmt, args ...) do { if (AP_MOUNT_VIEWPRO_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Viewpro: " fmt, ## args); } } while (0)
31 :
32 : const char* AP_Mount_Viewpro::send_text_prefix = "Viewpro:";
33 :
34 : // update mount position - should be called periodically
35 10279 : void AP_Mount_Viewpro::update()
36 : {
37 10279 : AP_Mount_Backend::update();
38 :
39 : // exit immediately if not initialised
40 10279 : if (!_initialised) {
41 0 : return;
42 : }
43 :
44 : // below here we sent angle or rate targets
45 : // throttle sends of target angles or rates
46 10279 : uint32_t now_ms = AP_HAL::millis();
47 10279 : if (now_ms - _last_update_ms < AP_MOUNT_VIEWPRO_UPDATE_INTERVAL_MS) {
48 8223 : return;
49 : }
50 2056 : _last_update_ms = now_ms;
51 :
52 : // reading incoming packets from gimbal
53 2056 : read_incoming_packets();
54 :
55 : // request model name
56 2056 : if (!_got_model_name) {
57 1 : send_comm_config_cmd(CommConfigCmd::QUERY_MODEL);
58 : }
59 :
60 : // request firmware version
61 2056 : if (!_got_firmware_version) {
62 1 : send_comm_config_cmd(CommConfigCmd::QUERY_FIRMWARE_VER);
63 : }
64 :
65 : // send handshake
66 2056 : send_handshake();
67 :
68 : // send vehicle attitude and position
69 2056 : send_m_ahrs();
70 :
71 : // if tracking is active we do not send new targets to the gimbal
72 2056 : if (_last_tracking_status == TrackingStatus::SEARCHING || _last_tracking_status == TrackingStatus::TRACKING) {
73 0 : return;
74 : }
75 :
76 : // update based on mount mode
77 2056 : update_mnt_target();
78 :
79 : // send target angles or rates depending on the target type
80 2056 : send_target_to_gimbal();
81 : }
82 :
83 : // return true if healthy
84 2105 : bool AP_Mount_Viewpro::healthy() const
85 : {
86 : // unhealthy until gimbal has been found and replied with firmware version info
87 2105 : if (!_initialised) {
88 0 : return false;
89 : }
90 :
91 : // unhealthy if attitude information NOT received recently
92 2105 : const uint32_t now_ms = AP_HAL::millis();
93 2105 : if (now_ms - _last_current_angle_rad_ms > AP_MOUNT_VIEWPRO_HEALTH_TIMEOUT_MS) {
94 0 : return false;
95 : }
96 :
97 : // if we get this far return healthy
98 2105 : return true;
99 : }
100 :
101 : // get attitude as a quaternion. returns true on success
102 3083 : bool AP_Mount_Viewpro::get_attitude_quaternion(Quaternion& att_quat)
103 : {
104 3083 : att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z);
105 3083 : return true;
106 : }
107 :
108 : // reading incoming packets from gimbal and confirm they are of the correct format
109 : // results are held in the _parsed_msg structure
110 2056 : void AP_Mount_Viewpro::read_incoming_packets()
111 : {
112 : // check for bytes on the serial port
113 2056 : int16_t nbytes = MIN(_uart->available(), 1024U);
114 2056 : if (nbytes <= 0 ) {
115 0 : return;
116 : }
117 :
118 : // flag to allow cases below to reset parser state
119 2056 : bool reset_parser = false;
120 :
121 : // process bytes received
122 196387 : for (int16_t i = 0; i < nbytes; i++) {
123 : uint8_t b;
124 194331 : if (!_uart->read(b)) {
125 0 : continue;
126 : }
127 :
128 194331 : _msg_buff[_msg_buff_len++] = b;
129 :
130 : // protect against overly long messages
131 194331 : if (_msg_buff_len > AP_MOUNT_VIEWPRO_PACKETLEN_MAX) {
132 0 : reset_parser = true;
133 : debug("vp buff full s:%u len:%u", (unsigned)_parsed_msg.state, (unsigned)_msg_buff_len);
134 : }
135 :
136 : // process byte depending upon current state
137 194331 : switch (_parsed_msg.state) {
138 :
139 4136 : case ParseState::WAITING_FOR_HEADER1:
140 4136 : if (b == AP_MOUNT_VIEWPRO_HEADER1) {
141 : // throw away byte
142 4136 : _msg_buff_len = 0;
143 4136 : _parsed_msg.state = ParseState::WAITING_FOR_HEADER2;
144 : } else {
145 0 : reset_parser = true;
146 : }
147 4136 : break;
148 :
149 4136 : case ParseState::WAITING_FOR_HEADER2:
150 4136 : if (b == AP_MOUNT_VIEWPRO_HEADER2) {
151 : // throw away byte
152 4136 : _msg_buff_len = 0;
153 4136 : _parsed_msg.state = ParseState::WAITING_FOR_HEADER3;
154 : } else {
155 0 : reset_parser = true;
156 : }
157 4136 : break;
158 :
159 4136 : case ParseState::WAITING_FOR_HEADER3:
160 4136 : if (b == AP_MOUNT_VIEWPRO_HEADER3) {
161 : // throw away byte
162 4136 : _msg_buff_len = 0;
163 4136 : _parsed_msg.state = ParseState::WAITING_FOR_LENGTH;
164 : } else {
165 0 : reset_parser = true;
166 : }
167 4136 : break;
168 :
169 4136 : case ParseState::WAITING_FOR_LENGTH:
170 : // length held in bits 0 ~ 5. length includes this length byte, frame id and final crc
171 : // ignore frame counter held in bits 6~7
172 4136 : _parsed_msg.data_len = b & 0x3F;
173 4136 : _parsed_msg.state = ParseState::WAITING_FOR_FRAMEID;
174 4136 : break;
175 :
176 4136 : case ParseState::WAITING_FOR_FRAMEID:
177 4136 : _parsed_msg.frame_id = b;
178 4136 : _parsed_msg.data_bytes_received = 0;
179 4136 : _parsed_msg.state = ParseState::WAITING_FOR_DATA;
180 4136 : break;
181 :
182 169515 : case ParseState::WAITING_FOR_DATA:
183 169515 : _parsed_msg.data_bytes_received++;
184 : // check if we have received all data bytes. subtract 3 to remove length byte, frame id and final crc
185 169515 : if (_parsed_msg.data_bytes_received >= _parsed_msg.data_len - 3) {
186 4136 : _parsed_msg.state = ParseState::WAITING_FOR_CRC;
187 : }
188 169515 : break;
189 :
190 4136 : case ParseState::WAITING_FOR_CRC: {
191 4136 : _parsed_msg.crc = b;
192 4136 : const uint16_t expected_crc = calc_crc(_msg_buff, _msg_buff_len-1);
193 4136 : if (expected_crc == _parsed_msg.crc) {
194 : // successfully received a message, do something with it
195 4136 : process_packet();
196 : } else {
197 : debug("crc expected:%x got:%x", (unsigned)expected_crc, (unsigned)_parsed_msg.crc);
198 : }
199 4136 : reset_parser = true;
200 4136 : break;
201 : }
202 : }
203 :
204 : // handle reset of parser
205 194331 : if (reset_parser) {
206 4136 : _msg_buff_len = 0;
207 4136 : _parsed_msg.state = ParseState::WAITING_FOR_HEADER1;
208 4136 : reset_parser = false;
209 : }
210 : }
211 : }
212 :
213 : // process successfully decoded packets held in the _parsed_msg structure
214 4136 : void AP_Mount_Viewpro::process_packet()
215 : {
216 : // process packet depending upon frame id
217 4136 : switch ((FrameId)_parsed_msg.frame_id) {
218 :
219 0 : case FrameId::HANDSHAKE:
220 0 : break;
221 :
222 2 : case FrameId::V: {
223 2 : const CommConfigCmd control_cmd = (CommConfigCmd)_msg_buff[_msg_buff_data_start];
224 : switch (control_cmd) {
225 1 : case CommConfigCmd::QUERY_FIRMWARE_VER: {
226 : // firmware version, length is 20 bytes but we expect format of "S" + yyyymmdd
227 1 : const uint8_t fw_major_str[3] {_msg_buff[_msg_buff_data_start+4], _msg_buff[_msg_buff_data_start+5], 0x0};
228 1 : const uint8_t fw_minor_str[3] {_msg_buff[_msg_buff_data_start+6], _msg_buff[_msg_buff_data_start+7], 0x0};
229 1 : const uint8_t fw_patch_str[3] {_msg_buff[_msg_buff_data_start+8], _msg_buff[_msg_buff_data_start+9], 0x0};
230 1 : const uint8_t major_ver = atoi((const char*)fw_major_str) & 0xFF;
231 1 : const uint8_t minor_ver = atoi((const char*)fw_minor_str) & 0xFF;
232 1 : const uint8_t patch_ver = atoi((const char*)fw_patch_str) & 0xFF;
233 1 : _firmware_version = (patch_ver << 16) | (minor_ver << 8) | major_ver;
234 1 : _got_firmware_version = true;
235 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s fw:%u.%u.%u", send_text_prefix, (unsigned)major_ver, (unsigned)minor_ver, (unsigned)patch_ver);
236 1 : break;
237 : }
238 1 : case CommConfigCmd::QUERY_MODEL:
239 1 : if (_parsed_msg.data_bytes_received == 0) {
240 0 : break;
241 : }
242 : // gimbal model, length is 10 bytes
243 1 : memset(_model_name, '\0', sizeof(_model_name));
244 1 : memcpy(_model_name, &_msg_buff[_msg_buff_data_start+1], MIN(sizeof(_model_name)-1, (size_t)(_parsed_msg.data_bytes_received-1)));
245 1 : _got_model_name = true;
246 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, _model_name);
247 1 : break;
248 0 : default:
249 : // unsupported control command
250 0 : break;
251 : }
252 2 : break;
253 : }
254 :
255 4134 : case FrameId::T1_F1_B1_D1: {
256 : // T1 holds target info including target lean angles
257 : // F1 holds tracker sensor status (which camera, tracking vs lost)
258 : // B1 section holds actual lean angles
259 : // D1 section holds camera status including zoom level
260 : //const int8_t servo_status = (_msg_buff[_msg_buff_data_start+24] & 0xF0) >> 4;
261 4134 : const TrackingStatus tracking_status = (TrackingStatus)((_msg_buff[_msg_buff_data_start+22] & 0x18) >> 3);
262 4134 : if (tracking_status != _last_tracking_status) {
263 0 : _last_tracking_status = tracking_status;
264 : switch (tracking_status) {
265 0 : case TrackingStatus::STOPPED:
266 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s tracking OFF", send_text_prefix);
267 0 : break;
268 0 : case TrackingStatus::SEARCHING:
269 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s tracking searching", send_text_prefix);
270 0 : break;
271 0 : case TrackingStatus::TRACKING:
272 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s tracking ON", send_text_prefix);
273 0 : break;
274 0 : case TrackingStatus::LOST:
275 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s tracking Lost", send_text_prefix);
276 0 : break;
277 : }
278 : }
279 :
280 4134 : _last_current_angle_rad_ms = AP_HAL::millis();
281 4134 : _current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+23] & 0x0F, _msg_buff[_msg_buff_data_start+24]) * (180.0/4095.0) - 90.0); // roll angle
282 4134 : _current_angle_rad.z = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+25], _msg_buff[_msg_buff_data_start+26]) * AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG); // yaw angle
283 4134 : _current_angle_rad.y = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+27], _msg_buff[_msg_buff_data_start+28]) * AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG); // pitch angle
284 : debug("r:%4.1f p:%4.1f y:%4.1f", (double)degrees(_current_angle_rad.x), (double)degrees(_current_angle_rad.y), (double)degrees(_current_angle_rad.z));
285 :
286 : // get active image sensor. D1's image sensor values are one value lower than C1's
287 4134 : _image_sensor = ImageSensor((_msg_buff[_msg_buff_data_start+29] & 0x07) + 1);
288 :
289 : // get recording status
290 4134 : const RecordingStatus recording_status = (RecordingStatus)(_msg_buff[_msg_buff_data_start+32] & 0x07);
291 4134 : const bool recording = (recording_status == RecordingStatus::RECORDING);
292 4134 : if (recording != _recording) {
293 0 : _recording = recording;
294 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording %s", send_text_prefix, _recording ? "ON" : "OFF");
295 : }
296 :
297 4134 : switch (_image_sensor) {
298 4134 : default:
299 : case ImageSensor::EO1:
300 : case ImageSensor::EO1_IR_PIP:
301 : //optical zoom times
302 4134 : _zoom_times = UINT16_VALUE(_msg_buff[_msg_buff_data_start+39], _msg_buff[_msg_buff_data_start+40]) * 0.1;
303 4134 : break;
304 0 : case ImageSensor::IR:
305 : case ImageSensor::IR_EO1_PIP:
306 : //ir zoom times
307 0 : _zoom_times = ((_msg_buff[_msg_buff_data_start+29] >> 3) & 0x0F) + 1;
308 0 : break;
309 : }
310 : // get laser rangefinder distance
311 4134 : _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+33], _msg_buff[_msg_buff_data_start+34]) * 0.1;
312 4134 : break;
313 : }
314 :
315 0 : default:
316 : debug("Unhandled FrameId:%u", (unsigned)_parsed_msg.frame_id);
317 0 : break;
318 : }
319 4136 : }
320 :
321 : // calculate crc of the received message
322 10230 : uint8_t AP_Mount_Viewpro::calc_crc(const uint8_t *buf, uint8_t len) const
323 : {
324 10230 : uint8_t res = 0;
325 303945 : for (uint8_t i=0; i<len; i++) {
326 293715 : res = (res ^ buf[i]) & 0xFF;
327 : }
328 10230 : return res;
329 : }
330 :
331 : // calculate the length and frame count byte (3rd byte of all messages)
332 : // length is all bytes after the header including CRC
333 6094 : uint8_t AP_Mount_Viewpro::get_length_and_frame_count_byte(uint8_t length)
334 : {
335 : // increment frame counter
336 6094 : _last_frame_counter = (_last_frame_counter + 1) & 0x03;
337 6094 : return ((_last_frame_counter << 6) | (length & 0x3F));
338 : }
339 :
340 : // send packet to gimbal. databuff includes everything after the length-and-frame-counter, does not include crc
341 : // returns true on success, false if outgoing serial buffer is full
342 6094 : bool AP_Mount_Viewpro::send_packet(const uint8_t* databuff, uint8_t databuff_len)
343 : {
344 6094 : if (!_initialised) {
345 0 : return false;
346 : }
347 :
348 : // calculate and sanity check packet size
349 6094 : const uint16_t packet_size = AP_MOUNT_VIEWPRO_PACKETLEN_MIN + databuff_len;
350 6094 : if (packet_size > AP_MOUNT_VIEWPRO_PACKETLEN_MAX) {
351 : debug("send_packet data buff too large");
352 0 : return false;
353 : }
354 :
355 : // check for sufficient space in outgoing buffer
356 6094 : if (_uart->txspace() < packet_size) {
357 : debug("tx space too low (%u < %u)", (unsigned)_uart->txspace(), (unsigned)packet_size);
358 0 : return false;
359 : }
360 :
361 : // buffer for holding outgoing packet
362 6094 : uint8_t send_buff[packet_size];
363 6094 : uint8_t send_buff_ofs = 0;
364 :
365 : // packet header
366 6094 : send_buff[send_buff_ofs++] = AP_MOUNT_VIEWPRO_HEADER1;
367 6094 : send_buff[send_buff_ofs++] = AP_MOUNT_VIEWPRO_HEADER2;
368 6094 : send_buff[send_buff_ofs++] = AP_MOUNT_VIEWPRO_HEADER3;
369 :
370 : // length and frame counter. length is databuffer length + 2 (1 for length, 1 for crc)
371 6094 : send_buff[send_buff_ofs++] = get_length_and_frame_count_byte(databuff_len + 2);
372 :
373 : // data
374 6094 : if (databuff_len != 0) {
375 6094 : memcpy(&send_buff[send_buff_ofs], databuff, databuff_len);
376 6094 : send_buff_ofs += databuff_len;
377 : }
378 :
379 : // crc
380 6094 : const uint8_t crc = calc_crc(&send_buff[3], send_buff_ofs-3);
381 6094 : send_buff[send_buff_ofs++] = crc;
382 :
383 : // write packet to serial port
384 6094 : _uart->write(send_buff, send_buff_ofs);
385 :
386 :
387 6094 : return true;
388 6094 : }
389 :
390 : // send handshake, gimbal will respond with T1_F1_B1_D1 paket that includes current angles
391 2056 : void AP_Mount_Viewpro::send_handshake()
392 : {
393 2056 : const HandshakePacket hs_packet {
394 : .content = {
395 : frame_id: FrameId::HANDSHAKE,
396 : unused: 0
397 : }
398 : };
399 2056 : send_packet(hs_packet.bytes, sizeof(hs_packet.bytes));
400 2056 : }
401 :
402 : // set gimbal's lock vs follow mode
403 : // lock should be true if gimbal should maintain an earth-frame target
404 : // lock is false to follow / maintain a body-frame target
405 2056 : bool AP_Mount_Viewpro::set_lock(bool lock)
406 : {
407 : // do not send if lock mode has already been sent recently
408 2056 : if (_last_lock == lock) {
409 2056 : return true;
410 : }
411 :
412 : // fill in packet
413 0 : const A1Packet a1_packet {
414 : .content = {
415 : frame_id: FrameId::A1,
416 0 : servo_status: lock ? ServoStatus::FOLLOW_YAW_DISABLE : ServoStatus::FOLLOW_YAW
417 : }
418 0 : };
419 :
420 : // send targets to gimbal
421 0 : if (send_packet(a1_packet.bytes, sizeof(a1_packet.bytes))) {
422 0 : _last_lock = lock;
423 0 : return true;
424 : }
425 0 : return false;
426 : }
427 :
428 : // send communication configuration command (aka U packet), gimbal will respond with a V packet
429 2 : bool AP_Mount_Viewpro::send_comm_config_cmd(CommConfigCmd cmd)
430 : {
431 : // fill in packet
432 2 : const UPacket u_packet {
433 : .content = {
434 : frame_id: FrameId::U,
435 : control_cmd: cmd
436 : }
437 2 : };
438 :
439 : // send targets to gimbal
440 2 : return send_packet(u_packet.bytes, sizeof(u_packet.bytes));
441 : }
442 :
443 : // send target pitch and yaw rates to gimbal
444 0 : void AP_Mount_Viewpro::send_target_rates(const MountRateTarget &rate_rads)
445 : {
446 0 : const float pitch_rads = rate_rads.pitch;
447 0 : const float yaw_rads = rate_rads.yaw;
448 0 : const bool yaw_is_ef = rate_rads.yaw_is_ef;
449 :
450 : // set lock value
451 0 : if (!set_lock(yaw_is_ef)) {
452 0 : return;
453 : }
454 :
455 : // scale pitch and yaw to values gimbal understands
456 0 : const int16_t pitch_rate_output = -degrees(pitch_rads) * 100.0;
457 0 : const int16_t yaw_rate_output = degrees(yaw_rads) * 100.0;
458 :
459 : // fill in packet
460 0 : const A1Packet a1_packet {
461 : .content = {
462 : frame_id: FrameId::A1,
463 : servo_status: ServoStatus::MANUAL_SPEED_MODE,
464 0 : yaw_be: htobe16(yaw_rate_output),
465 0 : pitch_be: htobe16(pitch_rate_output)
466 : }
467 0 : };
468 :
469 : // send targets to gimbal
470 0 : send_packet(a1_packet.bytes, sizeof(a1_packet.bytes));
471 : }
472 :
473 : // send target pitch and yaw angles to gimbal
474 2056 : void AP_Mount_Viewpro::send_target_angles(const MountAngleTarget &angle_rad)
475 : {
476 2056 : const float pitch_rad = angle_rad.pitch;
477 2056 : const float yaw_rad = angle_rad.yaw;
478 :
479 : // gimbal does not support lock in angle control mode
480 2056 : if (!set_lock(false)) {
481 0 : return;
482 : }
483 :
484 : // convert yaw angle to body-frame
485 2056 : float yaw_bf_rad = angle_rad.yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw_rad()) : yaw_rad;
486 :
487 : // enforce body-frame yaw angle limits. If beyond limits always use body-frame control
488 2056 : const float yaw_bf_min = radians(_params.yaw_angle_min);
489 2056 : const float yaw_bf_max = radians(_params.yaw_angle_max);
490 2056 : if (yaw_bf_rad < yaw_bf_min || yaw_bf_rad > yaw_bf_max) {
491 0 : yaw_bf_rad = constrain_float(yaw_bf_rad, yaw_bf_min, yaw_bf_max);
492 : }
493 :
494 : // scale pitch and yaw to values gimbal understands
495 2056 : const int16_t pitch_angle_output = -pitch_rad * AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT;
496 2056 : const int16_t yaw_angle_output = yaw_bf_rad * AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT;
497 :
498 : // fill in packet
499 2056 : const A1Packet a1_packet {
500 : .content = {
501 : frame_id: FrameId::A1,
502 : servo_status: ServoStatus::MANUAL_ABSOLUTE_ANGLE_MODE,
503 2056 : yaw_be: htobe16(yaw_angle_output),
504 2056 : pitch_be: htobe16(pitch_angle_output)
505 : }
506 2056 : };
507 :
508 : // send targets to gimbal
509 2056 : send_packet(a1_packet.bytes, sizeof(a1_packet.bytes));
510 : }
511 :
512 : // send camera command, affected image sensor and value (e.g. zoom speed)
513 0 : bool AP_Mount_Viewpro::send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd)
514 : {
515 : // fill in 2 bytes containing sensor, zoom speed, operation command and LRF
516 : // bit0~2: sensor
517 : // bit3~5: zoom speed
518 : // bit6~12: operation command no
519 : // bit13~15: LRF command
520 0 : const uint16_t sensor_id = (uint16_t)img_sensor;
521 0 : const uint16_t zoom_speed = ((uint16_t)value & 0x07) << 3;
522 0 : const uint16_t operation_cmd = ((uint16_t)cmd & 0x7F) << 6;
523 0 : const uint16_t rangefinder_cmd = ((uint16_t)lrf_cmd & 0x07) << 13;
524 :
525 : // fill in packet
526 0 : const C1Packet c1_packet {
527 : .content = {
528 : frame_id: FrameId::C1,
529 0 : sensor_zoom_cmd_be: htobe16(sensor_id | zoom_speed | operation_cmd | rangefinder_cmd)
530 : }
531 0 : };
532 :
533 : // send packet to gimbal
534 0 : return send_packet(c1_packet.bytes, sizeof(c1_packet.bytes));
535 : }
536 :
537 : // send camera command2 and corresponding value (e.g. zoom as absolute value)
538 0 : bool AP_Mount_Viewpro::send_camera_command2(CameraCommand2 cmd, uint16_t value)
539 : {
540 : // fill in packet
541 0 : const C2Packet c2_packet {
542 : .content = {
543 : frame_id: FrameId::C2,
544 : cmd: cmd,
545 0 : value_be: htobe16(value)
546 : }
547 0 : };
548 :
549 : // send packet to gimbal
550 0 : return send_packet(c2_packet.bytes, sizeof(c2_packet.bytes));
551 : }
552 :
553 : // send tracking command and corresponding value (normally zero)
554 0 : bool AP_Mount_Viewpro::send_tracking_command(TrackingCommand cmd, uint8_t value)
555 : {
556 : // convert image sensor to tracking source
557 0 : TrackingSource tracking_source = TrackingSource::EO1;
558 0 : switch (_image_sensor) {
559 0 : case ImageSensor::NO_ACTION:
560 : case ImageSensor::EO1:
561 : case ImageSensor::EO1_IR_PIP:
562 : case ImageSensor::FUSION:
563 0 : tracking_source = TrackingSource::EO1;
564 0 : break;
565 0 : case ImageSensor::IR:
566 : case ImageSensor::IR_EO1_PIP:
567 : case ImageSensor::IR1_13MM:
568 : case ImageSensor::IR2_52MM:
569 0 : tracking_source = TrackingSource::IR;
570 0 : break;
571 : }
572 :
573 : // fill in packet
574 : // Packet creation is done long-hand here to support g++-7.5.0
575 0 : E1Packet e1_packet {};
576 0 : e1_packet.content.frame_id = FrameId::E1;
577 0 : e1_packet.content.source = tracking_source;
578 0 : e1_packet.content.cmd = cmd;
579 0 : e1_packet.content.param2 = value; // normally zero
580 :
581 : // send packet to gimbal
582 0 : return send_packet(e1_packet.bytes, sizeof(e1_packet.bytes));
583 : }
584 :
585 : // send camera command2 and corresponding parameter values
586 0 : bool AP_Mount_Viewpro::send_tracking_command2(TrackingCommand2 cmd, int16_t param1, int16_t param2)
587 : {
588 : // fill in packet
589 0 : const E2Packet e2_packet {
590 : .content = {
591 : frame_id: FrameId::E2,
592 : cmd: cmd,
593 0 : param1_be: htobe16(param1),
594 0 : param2_be: htobe16(param2),
595 : }
596 0 : };
597 :
598 : // send packet to gimbal
599 0 : return send_packet(e2_packet.bytes, sizeof(e2_packet.bytes));
600 : }
601 :
602 : // send vehicle attitude and position to gimbal
603 2056 : bool AP_Mount_Viewpro::send_m_ahrs()
604 : {
605 : // get current location
606 2056 : Location loc;
607 2056 : int32_t alt_amsl_cm = 0;
608 2056 : if (!AP::ahrs().get_location(loc) || !loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
609 76 : return false;
610 : }
611 :
612 : #if AP_RTC_ENABLED
613 : // get date and time
614 : uint16_t year, ms;
615 : uint8_t month, day, hour, min, sec;
616 1980 : if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) {
617 0 : year = month = day = hour = min = sec = ms = 0;
618 : }
619 1980 : uint16_t date = ((year-2000) & 0x7f) | (((month+1) & 0x0F) << 7) | ((day & 0x1F) << 11);
620 1980 : uint64_t second_hundredths = (((hour * 60 * 60) + (min * 60) + sec) * 100) + (ms * 0.1);
621 : #else
622 : const uint16_t date = 0;
623 : const uint64_t second_hundredths = 0;
624 : #endif
625 :
626 : // get vehicle velocity in m/s in NED Frame
627 1980 : Vector3f vel_NED;
628 1980 : IGNORE_RETURN(AP::ahrs().get_velocity_NED(vel_NED));
629 1980 : const float vel_yaw_rad = wrap_2PI(vel_NED.xy().angle());
630 :
631 : // get GPS vdop
632 1980 : uint16_t gps_vdop = AP::gps().get_vdop();
633 :
634 : // get vehicle yaw in the range 0 to 2*pi
635 1980 : const float veh_yaw_rad = wrap_2PI(AP::ahrs().get_yaw_rad());
636 :
637 : // fill in packet
638 1980 : const M_AHRSPacket mahrs_packet {
639 : .content = {
640 : frame_id: FrameId::M_AHRS,
641 : data_type: 0x07, // Bit0: Attitude, Bit1: GPS, Bit2 Gyro
642 : unused2to8 : {0, 0, 0, 0, 0, 0, 0},
643 1980 : roll_be: htobe16(AP::ahrs().get_roll_rad() * AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT), // vehicle roll angle. 1bit=360deg/65536
644 1980 : pitch_be: htobe16(-AP::ahrs().get_pitch_rad() * AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT), // vehicle pitch angle. 1bit=360deg/65536
645 1980 : yaw_be: htobe16(veh_yaw_rad * AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT), // vehicle yaw angle. 1bit=360deg/65536
646 1980 : date_be: htobe16(date), // bit0~6:year, bit7~10:month, bit11~15:day
647 1980 : seconds_utc: {uint8_t((second_hundredths & 0xFF0000ULL) >> 16), // seconds * 100 MSB. 1bit = 0.01sec
648 1980 : uint8_t((second_hundredths & 0xFF00ULL) >> 8), // seconds * 100 next MSB. 1bit = 0.01sec
649 : uint8_t(second_hundredths & 0xFFULL)}, // seconds * 100 LSB. 1bit = 0.01sec
650 1980 : gps_yaw_be: htobe16(vel_yaw_rad * AP_MOUNT_VIEWPRO_RAD_TO_OUTPUT), // GPS yaw
651 : position_mark_bitmask: 0x0F, // bit0:new position, bit1:clock fix calced, bit2:horiz calced, bit3:alt calced
652 1980 : latitude_be: htobe32(loc.lat), // latitude. 1bit = 10e-7
653 1980 : longitude_be: htobe32(loc.lng), // longitude. 1bit = 10e-7
654 1980 : height_be: htobe32(alt_amsl_cm * 10), // height. 1bit = 1mm
655 1980 : ground_speed_N_be: htobe16(vel_NED.x * 100), // ground speed in North direction. 1bit = 0.01m/s
656 1980 : ground_speed_E_be: htobe16(vel_NED.y * 100), // ground speed in East direction. 1bit = 0.01m/s
657 1980 : vdop_be: htobe16(gps_vdop), // GPS vdop. 1bit = 0.01
658 1980 : ground_speed_D_be: htobe16(vel_NED.z * 100) // speed downwards. 1bit = 0.01m/s
659 : }
660 3960 : };
661 :
662 : // send packet to gimbal
663 1980 : return send_packet(mahrs_packet.bytes, sizeof(mahrs_packet.bytes));
664 : }
665 :
666 : // take a picture. returns true on success
667 0 : bool AP_Mount_Viewpro::take_picture()
668 : {
669 : // exit immediately if not initialised
670 0 : if (!_initialised) {
671 0 : return false;
672 : }
673 :
674 0 : return send_camera_command(_image_sensor, CameraCommand::TAKE_PICTURE, 0);
675 : }
676 :
677 : // start or stop video recording. returns true on success
678 : // set start_recording = true to start record, false to stop recording
679 0 : bool AP_Mount_Viewpro::record_video(bool start_recording)
680 : {
681 : // exit immediately if not initialised
682 0 : if (!_initialised) {
683 0 : return false;
684 : }
685 :
686 0 : return send_camera_command(_image_sensor, start_recording ? CameraCommand::START_RECORD : CameraCommand::STOP_RECORD, 0);
687 : }
688 :
689 : // set zoom specified as a rate or percentage
690 0 : bool AP_Mount_Viewpro::set_zoom(ZoomType zoom_type, float zoom_value)
691 : {
692 : // exit immediately if not initialised
693 0 : if (!_initialised) {
694 0 : return false;
695 : }
696 :
697 : // zoom rate
698 0 : if (zoom_type == ZoomType::RATE) {
699 0 : uint8_t zoom_speed = 1;
700 0 : CameraCommand zoom_cmd = CameraCommand::STOP_FOCUS_AND_ZOOM;
701 :
702 0 : switch (_image_sensor) {
703 0 : default:
704 : case ImageSensor::EO1:
705 : case ImageSensor::EO1_IR_PIP:
706 0 : if (zoom_value < 0) {
707 0 : zoom_cmd = CameraCommand::ZOOM_OUT;
708 0 : } else if (zoom_value > 0) {
709 0 : zoom_cmd = CameraCommand::ZOOM_IN;
710 : }
711 0 : zoom_speed = AP_MOUNT_VIEWPRO_EO_ZOOM_SPEED;
712 0 : break;
713 0 : case ImageSensor::IR:
714 : case ImageSensor::IR_EO1_PIP:
715 0 : if (zoom_value < 0) {
716 0 : zoom_cmd = CameraCommand::IR_ZOOM_IN;
717 0 : } else if (zoom_value > 0) {
718 0 : zoom_cmd = CameraCommand::IR_ZOOM_OUT;
719 : }
720 0 : zoom_speed = AP_MOUNT_VIEWPRO_IR_ZOOM_SPEED;
721 0 : break;
722 : }
723 :
724 0 : return send_camera_command(_image_sensor, zoom_cmd, zoom_speed);
725 : }
726 :
727 : // zoom percentage
728 0 : if (zoom_type == ZoomType::PCT) {
729 : // convert zoom percentage (0 ~ 100) to zoom value (0 ~ max zoom * 10)
730 0 : return send_camera_command2(CameraCommand2::SET_EO_ZOOM, linear_interpolate(0, AP_MOUNT_VIEWPRO_ZOOM_MAX * 10, zoom_value, 0, 100));
731 : }
732 :
733 : // unsupported zoom type
734 0 : return false;
735 : }
736 :
737 : // set focus specified as rate, percentage or auto
738 : // focus in = -1, focus hold = 0, focus out = 1
739 0 : SetFocusResult AP_Mount_Viewpro::set_focus(FocusType focus_type, float focus_value)
740 : {
741 : // exit immediately if not initialised
742 0 : if (!_initialised) {
743 0 : return SetFocusResult::FAILED;
744 : }
745 :
746 0 : switch (focus_type) {
747 0 : case FocusType::RATE: {
748 0 : CameraCommand focus_cmd = CameraCommand::STOP_FOCUS_AND_ZOOM;
749 0 : if (focus_value < 0) {
750 0 : focus_cmd = CameraCommand::FOCUS_MINUS;
751 0 : } else if (focus_value > 0) {
752 0 : focus_cmd = CameraCommand::FOCUS_PLUS;
753 : }
754 0 : if (!send_camera_command(_image_sensor, focus_cmd, 0)) {
755 0 : return SetFocusResult::FAILED;
756 : }
757 0 : return SetFocusResult::ACCEPTED;
758 : }
759 0 : case FocusType::PCT:
760 : // not supported
761 0 : return SetFocusResult::INVALID_PARAMETERS;
762 0 : case FocusType::AUTO:
763 0 : if (!send_camera_command(_image_sensor, CameraCommand::AUTO_FOCUS, 0)) {
764 0 : return SetFocusResult::FAILED;
765 : }
766 0 : return SetFocusResult::ACCEPTED;
767 : }
768 :
769 : // unsupported focus type
770 0 : return SetFocusResult::INVALID_PARAMETERS;
771 : }
772 :
773 : // set tracking to none, point or rectangle (see TrackingType enum)
774 : // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
775 : // p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
776 0 : bool AP_Mount_Viewpro::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
777 : {
778 : // exit immediately if not initialised
779 0 : if (!_initialised) {
780 0 : return false;
781 : }
782 :
783 0 : switch (tracking_type) {
784 0 : case TrackingType::TRK_NONE:
785 0 : return send_tracking_command(TrackingCommand::STOP, 0);
786 : break;
787 0 : case TrackingType::TRK_POINT: {
788 0 : return (send_tracking_command(TrackingCommand::START, 0) &&
789 0 : send_tracking_command2(TrackingCommand2::SET_POINT, (p1.x - 0.5) * 2.0 * 960, (p1.y - 0.5) * 2.0 * 540));
790 : break;
791 : }
792 0 : case TrackingType::TRK_RECTANGLE:
793 0 : return (send_tracking_command(TrackingCommand::START, 0) &&
794 0 : send_tracking_command2(TrackingCommand2::SET_RECT_TOPLEFT, (p1.x - 0.5) * 2.0 * 960, (p1.y - 0.5) * 2.0 * 540) &&
795 0 : send_tracking_command2(TrackingCommand2::SET_RECT_BOTTOMRIGHT, (p2.x - 0.5) * 2.0 * 960, (p2.y - 0.5) * 2.0 * 540));
796 : break;
797 : }
798 :
799 : // should never reach here
800 0 : return false;
801 : }
802 :
803 : // set camera lens as a value from 0 to 5
804 0 : bool AP_Mount_Viewpro::set_lens(uint8_t lens)
805 : {
806 : // exit immediately if not initialised
807 0 : if (!_initialised) {
808 0 : return false;
809 : }
810 :
811 : // match lens to ImageSensor enum values and sanity check
812 0 : lens++;
813 0 : if (lens > (uint8_t)ImageSensor::IR2_52MM) {
814 0 : return false;
815 : }
816 :
817 : // if lens is zero use default lens
818 0 : ImageSensor new_image_sensor = ImageSensor(lens);
819 0 : return send_camera_command(new_image_sensor, CameraCommand::NO_ACTION, 0);
820 : }
821 :
822 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
823 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
824 0 : bool AP_Mount_Viewpro::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
825 : {
826 : // maps primary and secondary source to viewpro image sensor
827 : ImageSensor new_image_sensor;
828 0 : switch (primary_source) {
829 0 : case 0: // Default (RGB)
830 : FALLTHROUGH;
831 : case 1: // RGB
832 0 : switch (secondary_source) {
833 0 : case 0: // RGB + Default (None)
834 0 : new_image_sensor = ImageSensor::EO1;
835 0 : break;
836 0 : case 2: // PIP RGB+IR
837 0 : new_image_sensor = ImageSensor::EO1_IR_PIP;
838 0 : break;
839 0 : default:
840 0 : return false;
841 : }
842 0 : break;
843 0 : case 2: // IR
844 0 : switch (secondary_source) {
845 0 : case 0: // IR + Default (None)
846 0 : new_image_sensor = ImageSensor::IR;
847 0 : break;
848 0 : case 1: // PIP IR+RGB
849 0 : new_image_sensor = ImageSensor::IR_EO1_PIP;
850 0 : break;
851 0 : default:
852 0 : return false;
853 : }
854 0 : break;
855 0 : default:
856 0 : return false;
857 : }
858 :
859 : // send desired image type to camera
860 0 : return send_camera_command(new_image_sensor, CameraCommand::NO_ACTION, 0);
861 : }
862 :
863 : // send camera settings message to GCS
864 0 : void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const
865 : {
866 : // exit immediately if not initialised
867 0 : if (!_initialised) {
868 0 : return;
869 : }
870 :
871 : // convert zoom times (e.g. 1x ~ 20x) to target zoom level (e.g. 0 to 100)
872 0 : const float zoom_level = linear_interpolate(0, 100, _zoom_times, 1, AP_MOUNT_VIEWPRO_ZOOM_MAX);
873 :
874 : // send CAMERA_SETTINGS message
875 0 : mavlink_msg_camera_settings_send(
876 : chan,
877 : AP_HAL::millis(), // time_boot_ms
878 0 : _recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
879 : zoom_level, // zoomLevel float, percentage from 0 to 100, NaN if unknown
880 : NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
881 : }
882 :
883 : // get rangefinder distance. Returns true on success
884 2055 : bool AP_Mount_Viewpro::get_rangefinder_distance(float& distance_m) const
885 : {
886 : // if not healthy or zero distance return false
887 : // healthy() checks attitude timeout which is in same message as rangefinder distance
888 2055 : if (!healthy()) {
889 0 : return false;
890 : }
891 :
892 2055 : distance_m = _rangefinder_dist_m;
893 2055 : return true;
894 : }
895 :
896 : // enable/disable rangefinder. Returns true on success
897 0 : bool AP_Mount_Viewpro::set_rangefinder_enable(bool enable)
898 : {
899 0 : return send_camera_command(ImageSensor::NO_ACTION, CameraCommand::NO_ACTION, 0, enable ? LRFCommand::CONTINUOUS_RANGING_START : LRFCommand::STOP_RANGING);
900 : }
901 :
902 : #endif // HAL_MOUNT_VIEWPRO_ENABLED
|
