Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_XFROBOT_ENABLED
4 :
5 : #include "AP_Mount_XFRobot.h"
6 : #include <AP_HAL/AP_HAL.h>
7 : #include <AP_AHRS/AP_AHRS.h>
8 : #include <AP_GPS/AP_GPS.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_HAL/utility/sparse-endian.h>
11 :
12 : #define SEND_ATTITUDE_TARGET_MS 1000 // send angle targets to gimbal at least once per second
13 : #define HEALTH_TIMEOUT_MS 1000 // become unhealthy if attitude not received within 1 second
14 : #define RECORD_REQUEST_TIMEOUT_MS 1000 // requests to start/stop recording timeout in 1 second
15 : #define SEND_HEADER1 0xA8 // send packet header1
16 : #define SEND_HEADER2 0xE5 // send packet header2
17 : #define RECV_HEADER1 0x8A // receive packet header1
18 : #define RECV_HEADER2 0x5E // receive packet header2
19 : #define RECV_LENGTH_MIN 10 // receive packet minimum length in bytes
20 : #define PROTOCOL_VERSION 0x02 // protocol version
21 :
22 : const char* AP_Mount_XFRobot::send_text_prefix = "XFRobot:";
23 :
24 : ////////////////////////////////////////////////////////
25 : // packet structure from autopilot to gimbal
26 : // byte 0: header1 (0xA8)
27 : // byte 1: header2 (0xE5)
28 : // byte 2~3: length (uint16)
29 : // byte 4: protocol version (0x02)
30 : // below is "main data frame"
31 : // byte 5~6: roll control value
32 : // byte 7~8: pitch control value
33 : // byte 9~10: yaw control value
34 : // byte 11: status, Bit0:INS valid, Bit2:control values valid
35 : // byte 12~13: absolute roll angle of vehicle (int16, -18000 ~ +18000)
36 : // byte 14~15: absolute pitch angle of vehicle (int16, -9000 ~ +9000)
37 : // byte 16~17: absolute yaw angle of vehicle (uint16, 0 ~ 36000)
38 : // byte 18~19: North acceleration of vehicle (int16, cm/s/s)
39 : // byte 20~21: East acceleration of vehicle (int16, cm/s/s)
40 : // byte 22~23: Upward acceleration of vehicle (int16, cm/s/s)
41 : // byte 24~25: North speed of vehicle (int16, decimeter/s)
42 : // byte 26~27: East speed of vehicle (int16, decimeter/s)
43 : // byte 28~29: Upward speed of vehicle (int16, decimeter/s)
44 : // byte 30: request code of sub frame, header of requested sub data frame from GCU (aka camera)
45 : // above is end of main data frame
46 : // byte 31~36: reserved/unused
47 : // below is "sub data frame"
48 : // byte 37: header (0x01)
49 : // byte 38~41: longitude of vehicle (int32, 1E7)
50 : // byte 42~45: latitude of vehicle (int32, 1E7)
51 : // byte 46~49: altitude of vehicle (int32, mm)
52 : // byte 50: number of satellites
53 : // byte 51~54: GNSS microsecond (uint32)
54 : // byte 55~56: GNSS week number (uint16)
55 : // byte 57~60: relative altitude (int32, mm, can be zero if unavailable)
56 : // byte 61~68: reserved/unused
57 : // byte 69: order (uint8)
58 : // 0x00: Null (sent to ensure next command is not ignored)
59 : // 0x01: Calibration
60 : // 0x03: Neutral
61 : // 0x10: Angle control
62 : // 0x11: Head lock
63 : // 0x12: Head follow
64 : // 0x13: Orthoview
65 : // 0x14: Euler angle control
66 : // 0x15: Gaze Geo-coordinates, params: Longitude, Latitude, Altitude (12bytes)
67 : // 0x16: Gaze Geo-target lock
68 : // 0x17: Track, params: 0x01, TT, X0, X0, Y0, Y0, X1, X1, Y1, Y1 (10 bytes)
69 : // 0x1A: Click to aim, params: 0x01, X0, X0, Y0, Y0 (5 bytes)
70 : // 0x1C: FPV
71 : // byte 70 ~ S-3: parameter (variable length)
72 : // byte S-2: CRC high
73 : // byte S-1: CRC low
74 :
75 : // packet structure from GCU (aka camera)
76 : // byte 0: header1 (0x8A)
77 : // byte 1: header2 (0x5E)
78 : // byte 2~3: length (uint16)
79 : // byte 4: version
80 : // below is "main data frame"
81 : // byte 5: mode: 0x10:angle control, 0x11:head lock, 0x12: head follow, 0x13:orthoview, 0x14:euler angle control, 0x16:gaze, 0x17:track,0x1C:FPV
82 : // byte 6~7: status, Bit0:tracking success, Bit7:range and target coordinate valid, Bit8:ranging on, Bit9:night vision on, Bit10:lighting on, Bit12:Upward powered on
83 : // byte 8~9: horizontal target (int16, -1000 ~ +1000, rightward is positive)
84 : // byte 10~11: vertical target (int16, -1000 ~ +1000, downward is positive)
85 : // byte 12~13: x-axis angle of camera relative to vehicle (int16, -18000 ~ +18000)
86 : // byte 14~15: y-axis angle of camera relative to vehicle (int16, -18000 ~ +18000)
87 : // byte 16~17: z-axis angle of camera relative to vehicle (int16, -18000 ~ +18000)
88 : // byte 18~19: roll angle of camera (absolute) (int16, -9000 ~ +9000)
89 : // byte 20~21: pitch angle of camera (absolute) (int16, -18000 ~ +18000)
90 : // byte 22~23: yaw angle of camera (absolute) (uint16, 0 ~ 36000)
91 : // byte 24~25: X-axis absolute angular velocity of camera (int16, centi-degrees/s)
92 : // byte 26~27: Y-axis absolute angular velocity of camera (int16, centi-degrees/s)
93 : // byte 28~29: Z-axis absolute angular velocity of camera (int16, centi-degrees/s)
94 : // byte 30~36: reserved/unused
95 : // above is end of "main data frame", below is "sub data frame"
96 : // byte 37: header (0x01)
97 : // byte 38: hardware version (uint8)
98 : // byte 39: firmware version (uint8)
99 : // byte 40: pod code
100 : // byte 41~42: error code, Bit7:hardware error, Bit13: mavlink communication freq anomaly, Bit14:BNSS unpositioned, Bit15:GCU hardware error
101 : // byte 43~46: distance from target (int32, decimeters, -1m or 0m is invalid measurement)
102 : // byte 47~50: longitude of target (int32, 1E7)
103 : // byte 51~54: latitude of target (int32, 1E7)
104 : // byte 55~58: altitude of target (int32, mm)
105 : // byte 59~60: zoom rate of RGB camera (uint16, resolution 0.1x)
106 : // byte 61~62: zoom rate of thermal camera (uint16, resolution 0.1x)
107 : // byte 63: thermal camera status, Bit0:low temp alert, Bit1:high temp alert, Bit3:spot temp measurement on, Bit4:Isotherm on, Bit5:temp alert on, Bit6:area temp on, Bit7:temp available
108 : // byte 64~65: camera status, Bit0~Bit2:pic-in-pic mode, Bit4:recording, Bit11:image auto reverse off, Bit12:OSD displays target coordinate, Bit13:OSD on, Bit14:digital zoom on, Bit15:target detection on
109 : // byte 66: timezone (int8, -12 ~ +12)
110 : // byte 67~68: reserved/unused
111 : // end of "sub data frame"
112 : // byte 69: order
113 : // byte 70 ~ S-3: execution state (variable length)
114 : // byte S-2: CRC high
115 : // byte S-1: CRC low
116 : ////////////////////////////////////////////////////////
117 :
118 : // update mount position - should be called periodically
119 0 : void AP_Mount_XFRobot::update()
120 : {
121 : // exit immediately if not initialised
122 0 : if (!_initialised) {
123 0 : return;
124 : }
125 :
126 : // reading incoming packets from gimbal
127 0 : read_incoming_packets();
128 :
129 : // check for recording request timeout
130 0 : check_recording_timeout();
131 :
132 0 : AP_Mount_Backend::update_mnt_target();
133 :
134 : // send target angles (which may be derived from other target types)
135 0 : send_target_to_gimbal();
136 : }
137 :
138 : // return true if healthy
139 0 : bool AP_Mount_XFRobot::healthy() const
140 : {
141 : // healthy if initialised and attitude from gimbal has been received within the last second
142 0 : return _initialised && (AP_HAL::millis() - attitude_latest.update_ms) < HEALTH_TIMEOUT_MS;
143 : }
144 :
145 : // take a picture. returns true on success
146 0 : bool AP_Mount_XFRobot::take_picture()
147 : {
148 : // send command to take picture
149 0 : return send_simple_command(FunctionOrder::SHUTTER, 0x01);
150 : }
151 :
152 : //set camera lens
153 0 : bool AP_Mount_XFRobot::set_lens(uint8_t lens)
154 : {
155 : // lens to camera type mapping table
156 : static const CameraType cam_type_table[] {
157 : CameraType::MAIN_ZOOM_SUB_THERMAL,
158 : CameraType::MAIN_THERMAL_SUB_ZOOM,
159 : CameraType::MAIN_PIP_ZOOM_SUB_THERMAL,
160 : };
161 :
162 : // sanity check lens values
163 0 : if (lens >= ARRAY_SIZE(cam_type_table)) {
164 0 : return false;
165 : }
166 :
167 : // map lens to camera type and send command
168 0 : return send_simple_command(FunctionOrder::PIC_IN_PIC, (uint8_t)cam_type_table[lens]);
169 : }
170 :
171 : // start or stop video recording. returns true on success
172 : // set start_recording = true to start record, false to stop recording
173 0 : bool AP_Mount_XFRobot::record_video(bool start_recording)
174 : {
175 : // reject request if waiting for reply on earlier request
176 0 : if (recording.request_ms > 0) {
177 0 : return false;
178 : }
179 :
180 : // send command to start/stop recording video
181 0 : if (send_simple_command(FunctionOrder::RECORD_VIDEO, uint8_t(start_recording))) {
182 : // record request to start or stop recording
183 0 : recording.request_ms = AP_HAL::millis();
184 0 : recording.request_start = start_recording;
185 0 : return true;
186 : }
187 0 : return false;
188 : }
189 :
190 : // set zoom specified as a rate or percentage
191 0 : bool AP_Mount_XFRobot::set_zoom(ZoomType zoom_type, float zoom_value)
192 : {
193 : // zoom rate
194 0 : if (zoom_type == ZoomType::RATE) {
195 0 : FunctionOrder zoom_fn = FunctionOrder::ZOOM_STOP;
196 0 : if (zoom_value < 0) {
197 0 : zoom_fn = FunctionOrder::ZOOM_OUT;
198 0 : } else if (zoom_value > 0) {
199 0 : zoom_fn = FunctionOrder::ZOOM_IN;
200 : }
201 0 : return send_simple_command(zoom_fn, 0x01);
202 : }
203 :
204 : // unsupported zoom type
205 0 : return false;
206 : }
207 :
208 : // set focus specified as rate, percentage or auto
209 : // focus in = -1, focus hold = 0, focus out = 1
210 0 : SetFocusResult AP_Mount_XFRobot::set_focus(FocusType focus_type, float focus_value)
211 : {
212 0 : if (focus_type == FocusType::AUTO) {
213 : // send auto focus command
214 0 : return send_simple_command(FunctionOrder::FOCUS, 0x01) ? SetFocusResult::ACCEPTED : SetFocusResult::FAILED;
215 : }
216 :
217 : // unsupported focus type
218 0 : return SetFocusResult::INVALID_PARAMETERS;
219 : }
220 :
221 : // get attitude as a quaternion. returns true on success
222 0 : bool AP_Mount_XFRobot::get_attitude_quaternion(Quaternion& att_quat)
223 : {
224 : // gimbal does not provide attitude so simply return targets
225 0 : att_quat.from_euler(radians(attitude_latest.roll_ef_deg), radians(attitude_latest.pitch_ef_deg), radians(attitude_latest.yaw_bf_deg));
226 0 : return true;
227 : }
228 :
229 : // reading incoming packets from gimbal
230 0 : void AP_Mount_XFRobot::read_incoming_packets()
231 : {
232 : // check for bytes on the serial port
233 0 : int16_t nbytes = MIN(_uart->available(), 1024U);
234 0 : if (nbytes <= 0 ) {
235 0 : return;
236 : }
237 :
238 : // flag to allow cases below to reset parser state
239 0 : bool reset_parser = false;
240 :
241 : // process bytes received
242 0 : for (int16_t i = 0; i < nbytes; i++) {
243 : uint8_t b;
244 0 : if (!_uart->read(b)) {
245 0 : continue;
246 : }
247 :
248 : // add latest byte to buffer
249 0 : msg_buff.bytes[msg_buff_len++] = b;
250 :
251 : // protect against overly long messages
252 0 : if (msg_buff_len > AP_MOUNT_XFROBOT_RECV_LENGTH_MAX) {
253 0 : reset_parser = true;
254 : }
255 :
256 : // process byte depending upon current state
257 0 : switch (parser.state) {
258 0 : case ParseState::WAITING_FOR_HEADER1:
259 0 : if (b == RECV_HEADER1) {
260 0 : parser.state = ParseState::WAITING_FOR_HEADER2;
261 : }
262 0 : break;
263 0 : case ParseState::WAITING_FOR_HEADER2:
264 0 : if (b == RECV_HEADER2) {
265 0 : parser.state = ParseState::WAITING_FOR_LENGTH_LOW;
266 : } else {
267 0 : reset_parser = true;
268 : }
269 0 : break;
270 0 : case ParseState::WAITING_FOR_LENGTH_LOW:
271 0 : parser.len_expected = b;
272 0 : parser.state = ParseState::WAITING_FOR_LENGTH_HIGH;
273 0 : break;
274 0 : case ParseState::WAITING_FOR_LENGTH_HIGH:
275 0 : parser.len_expected |= ((uint16_t)b << 8);
276 0 : if (parser.len_expected < RECV_LENGTH_MIN || parser.len_expected > AP_MOUNT_XFROBOT_RECV_LENGTH_MAX) {
277 0 : reset_parser = true;
278 : } else {
279 0 : parser.state = ParseState::WAITING_FOR_DATA;
280 : }
281 0 : break;
282 0 : case ParseState::WAITING_FOR_DATA:
283 : // check if we have received all data bytes. the final two bytes are for the CRC
284 0 : if (msg_buff_len >= parser.len_expected - 2) {
285 0 : parser.state = ParseState::WAITING_FOR_CRC_HIGH;
286 : }
287 0 : break;
288 0 : case ParseState::WAITING_FOR_CRC_HIGH:
289 0 : parser.crc = (uint16_t)b << 8;
290 0 : parser.state = ParseState::WAITING_FOR_CRC_LOW;
291 0 : break;
292 0 : case ParseState::WAITING_FOR_CRC_LOW:
293 0 : parser.crc |= b;
294 0 : const uint16_t expected_crc = crc16_ccitt(msg_buff.bytes, msg_buff_len - 2, 0);
295 0 : if (expected_crc == parser.crc) {
296 : // successfully received a message, do something with it
297 0 : process_packet();
298 : }
299 0 : reset_parser = true;
300 0 : break;
301 : }
302 :
303 : // handle reset of parser
304 0 : if (reset_parser) {
305 0 : msg_buff_len = 0;
306 0 : parser.state = ParseState::WAITING_FOR_HEADER1;
307 0 : reset_parser = false;
308 : }
309 : }
310 : }
311 :
312 : // process successfully decoded packets held in the msg_buff structure
313 0 : void AP_Mount_XFRobot::process_packet()
314 : {
315 : // sanity check size of packet received
316 0 : if (msg_buff_len < sizeof(GCUSimpleReply)) {
317 0 : return;
318 : }
319 :
320 : // extract latest attitude
321 0 : attitude_latest = {
322 0 : .roll_ef_deg = msg_buff.simple_reply.main.roll_abs_cd * 0.01f,
323 0 : .pitch_ef_deg = msg_buff.simple_reply.main.pitch_abs_cd * 0.01f,
324 0 : .yaw_bf_deg = msg_buff.simple_reply.main.angle_z * 0.01f,
325 0 : .update_ms = AP_HAL::millis()
326 : };
327 :
328 : // display hardware and firmware version
329 0 : if (!got_firmware_version) {
330 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s hw:%.1f fw:%.1f", send_text_prefix, double(msg_buff.simple_reply.main.hardware_version * 0.1), double(msg_buff.simple_reply.main.firmware_version * 0.1));
331 0 : got_firmware_version = true;
332 : }
333 :
334 : // detect failure to take picture
335 0 : if (msg_buff.simple_reply.main.order == FunctionOrder::SHUTTER && msg_buff.simple_reply.param.execution_state == 0x01) {
336 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s failed to take picture", send_text_prefix);
337 : }
338 :
339 : // check for failure to start/stop recording
340 0 : if (msg_buff.simple_reply.main.order == FunctionOrder::RECORD_VIDEO) {
341 0 : if (msg_buff.simple_reply.param.execution_state == 0x0) {
342 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording %s", send_text_prefix, recording.request_start ? "ON" : "OFF");
343 : } else {
344 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s failed to %s recording", send_text_prefix, recording.request_start ? "start" : "stop");
345 : }
346 0 : recording.request_ms = 0;
347 : }
348 : }
349 :
350 : // send_target_angles
351 0 : void AP_Mount_XFRobot::send_target_angles(const MountAngleTarget& angle_target_rad)
352 : {
353 : // exit immediately if not initialised or not enough space to send packet
354 0 : if (!_initialised || _uart->txspace() < sizeof(SetAttitudePacket)) {
355 0 : return;
356 : }
357 :
358 : // prepare packet to send to gimbal
359 0 : SetAttitudePacket set_attitude_packet {};
360 :
361 : // bytes 0~1: headers
362 0 : set_attitude_packet.content.main.header1 = SEND_HEADER1;
363 0 : set_attitude_packet.content.main.header2 = SEND_HEADER2;
364 :
365 : // byte 2~3: length (uint16)
366 0 : set_attitude_packet.content.main.length = htole16(sizeof(SetAttitudePacket));
367 :
368 : // byte 4: protocol version (0x02)
369 0 : set_attitude_packet.content.main.version = PROTOCOL_VERSION;
370 :
371 : // byte 5~6: roll control value (int16, -18000 ~ +18000)
372 : // byte 7~8: pitch control value (int16, -18000 ~ +18000)
373 : // byte 9~10: yaw control value (int16, -18000 ~ +18000)
374 0 : set_attitude_packet.content.main.roll_control = htole16(constrain_int16(degrees(angle_target_rad.roll) * 100, -18000, 18000));
375 0 : set_attitude_packet.content.main.pitch_control = htole16(constrain_int16(degrees(angle_target_rad.pitch) * 100, -9000, 9000));
376 0 : set_attitude_packet.content.main.yaw_control = htole16(constrain_int16(degrees(angle_target_rad.get_bf_yaw()) * 100, -18000, 18000));
377 :
378 : // byte 11: status, Bit0:INS valid, Bit2:control values valid
379 0 : const uint8_t status_ins_bit = AP::ahrs().have_inertial_nav() ? (1 << 0) : 0;
380 0 : set_attitude_packet.content.main.status = status_ins_bit | (1 << 2);
381 :
382 : // byte 12~13: absolute roll angle of vehicle (int16, -18000 ~ +18000)
383 : // byte 14~15: absolute pitch angle of vehicle (int16, -9000 ~ +9000)
384 : // byte 16~17: absolute yaw angle of vehicle (uint16, 0 ~ 36000)
385 0 : set_attitude_packet.content.main.roll_abs = htole16(constrain_int16(AP::ahrs().get_roll_deg() * 100, -18000, 18000));
386 0 : set_attitude_packet.content.main.pitch_abs = htole16(constrain_int16(AP::ahrs().get_pitch_deg() * 100, -9000, 9000));
387 0 : set_attitude_packet.content.main.yaw_abs = htole16(constrain_int16(degrees(wrap_PI(AP::ahrs().get_yaw_rad())) * 100, -18000, 18000));
388 :
389 : // byte 18~19: North acceleration of vehicle (int16, cm/s/s)
390 : // byte 20~21: East acceleration of vehicle (int16, cm/s/s)
391 : // byte 22~23: Upward acceleration of vehicle (int16, cm/s/s)
392 0 : const Vector3f &accel_ef = AP::ahrs().get_accel_ef();
393 0 : set_attitude_packet.content.main.accel_north = htole16(constrain_int16(accel_ef.x * 100, -INT16_MAX, INT16_MAX));
394 0 : set_attitude_packet.content.main.accel_east = htole16(constrain_int16(accel_ef.y * 100, -INT16_MAX, INT16_MAX));
395 0 : set_attitude_packet.content.main.accel_up = htole16(constrain_int16(-(accel_ef.z + GRAVITY_MSS) * 100, -INT16_MAX, INT16_MAX));
396 :
397 : // byte 24~25: North speed of vehicle (int16, decimeter/s)
398 : // byte 26~27: East speed of vehicle (int16, decimeter/s)
399 : // byte 28~29: Upward speed of vehicle (int16, decimeter/s)
400 : // ToDo: check scale (cm/s or decimeter/s)
401 0 : Vector3f velocity_ef;
402 0 : if (AP::ahrs().have_inertial_nav() && AP::ahrs().get_velocity_NED(velocity_ef)) {
403 0 : set_attitude_packet.content.main.vel_north = htole16(constrain_int16(velocity_ef.x * 10, -INT16_MAX, INT16_MAX));
404 0 : set_attitude_packet.content.main.vel_east = htole16(constrain_int16(velocity_ef.y * 10, -INT16_MAX, INT16_MAX));
405 0 : set_attitude_packet.content.main.vel_up = htole16(constrain_int16(-velocity_ef.z * 10, -INT16_MAX, INT16_MAX));
406 : }
407 :
408 : // byte 30: request code of sub frame, header of requested sub data frame from GCU (aka camera)
409 0 : set_attitude_packet.content.main.request_code = 0x01;
410 :
411 : // byte 31~36: reserved/unused
412 :
413 : // below is "sub data frame"
414 : // byte 37: sub header (0x01)
415 0 : set_attitude_packet.content.main.sub_header = 0x01;
416 :
417 : // byte 38~41: longitude of vehicle (int32, 1E7)
418 : // byte 42~45: latitude of vehicle (int32, 1E7)
419 : // byte 46~49: altitude of vehicle (int32, mm)
420 0 : Location veh_loc;
421 0 : if (AP::ahrs().get_location(veh_loc)) {
422 : // longitude
423 0 : set_attitude_packet.content.main.longitude = htole32(veh_loc.lng);
424 :
425 : // latitude
426 0 : set_attitude_packet.content.main.latitude = htole32(veh_loc.lat);
427 :
428 : // get absolute altitude and convert from cm to mm
429 : int32_t alt_amsl_cm;
430 0 : if (veh_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
431 0 : const int32_t alt_amsl_mm = alt_amsl_cm * 10;
432 0 : set_attitude_packet.content.main.alt_amsl = htole32(alt_amsl_mm);
433 : }
434 : }
435 :
436 : // byte 50: number of satellites
437 0 : set_attitude_packet.content.main.gps_num_sats = AP::gps().num_sats();
438 :
439 : // byte 51~54: GNSS milliseconds (uint32)
440 0 : const uint32_t gps_time_week_ms = AP::gps().time_week_ms();
441 0 : set_attitude_packet.content.main.gps_week_ms = htole32(gps_time_week_ms);
442 :
443 : // byte 55~56: GNSS week number (uint16)
444 0 : const uint16_t gps_time_week = AP::gps().time_week();
445 0 : set_attitude_packet.content.main.gps_week = htole16(gps_time_week);
446 :
447 : // byte 57~60: relative altitude (int32, mm, can be zero if unavailable)
448 : int32_t rel_alt_cm;
449 0 : if (veh_loc.initialised() && veh_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, rel_alt_cm)) {
450 0 : const int32_t rel_alt_mm = rel_alt_cm * 10;
451 0 : set_attitude_packet.content.main.alt_rel = htole32(rel_alt_mm);
452 : }
453 :
454 : // byte 61~68: reserved/unused
455 :
456 : // byte 69: order (uint8)
457 : // 0x00: Null (sent to ensure next command is not ignored)
458 : // 0x01: Calibration
459 : // 0x03: Neutral
460 : // 0x10: Angle control
461 0 : set_attitude_packet.content.main.order = FunctionOrder::ANGLE_CONTROL;
462 :
463 : // calculate CRC
464 0 : const uint16_t crc16 = crc16_ccitt(set_attitude_packet.bytes, sizeof(SetAttitudePacket) - 2, 0);
465 0 : set_attitude_packet.content.crc.crc_high = HIGHBYTE(crc16);
466 0 : set_attitude_packet.content.crc.crc_low = LOWBYTE(crc16);
467 :
468 : // send packet to gimbal
469 0 : _uart->write(set_attitude_packet.bytes, sizeof(SetAttitudePacket));
470 :
471 : // store time of send
472 0 : last_send_ms = AP_HAL::millis();
473 : }
474 :
475 : // send simple (1byte) command to gimbal (e.g. take pic, start recording)
476 : // returns true on success, false on failure to send
477 0 : bool AP_Mount_XFRobot::send_simple_command(FunctionOrder order, uint8_t param)
478 : {
479 : // exit immediately if not healthy or command buffer is too large
480 0 : if (!healthy() || _uart->txspace() < sizeof(SimpleCommand)) {
481 0 : return false;
482 : }
483 :
484 : // prepare packet to send to gimbal
485 0 : SimpleCommand simple_command {};
486 :
487 : // bytes 0~1: headers
488 0 : simple_command.content.main.header1 = SEND_HEADER1;
489 0 : simple_command.content.main.header2 = SEND_HEADER2;
490 :
491 : // byte 2~3: length (uint16)
492 0 : simple_command.content.main.length = htole16(sizeof(SimpleCommand));
493 :
494 : // byte 4: protocol version (0x02)
495 0 : simple_command.content.main.version = PROTOCOL_VERSION;
496 :
497 : // bytes 5 ~ 29 left as zero
498 :
499 : // byte 30: request code of sub frame, header of requested sub data frame from GCU (aka camera)
500 0 : simple_command.content.main.request_code = 0x01;
501 :
502 : // bytes 31 ~ 68 left as zero
503 :
504 : // byte 69: order (uint8)
505 : // byte 70: parameter (uint8_t)
506 0 : simple_command.content.main.order = order;
507 0 : simple_command.content.param_1byte = param;
508 :
509 : // calculate CRC
510 0 : const uint16_t crc16 = crc16_ccitt(simple_command.bytes, sizeof(SimpleCommand) - 2, 0);
511 0 : simple_command.content.crc.crc_high = HIGHBYTE(crc16);
512 0 : simple_command.content.crc.crc_low = LOWBYTE(crc16);
513 :
514 : // send packet to gimbal
515 0 : _uart->write(simple_command.bytes, sizeof(SimpleCommand));
516 :
517 : // packet sent
518 0 : return true;
519 : }
520 :
521 : // check for recording timeout
522 0 : void AP_Mount_XFRobot::check_recording_timeout()
523 : {
524 : // return immediately if not starting/stopping recording
525 0 : if (recording.request_ms == 0) {
526 0 : return;
527 : }
528 :
529 : // check for timeout
530 0 : if (AP_HAL::millis() - recording.request_ms > RECORD_REQUEST_TIMEOUT_MS) {
531 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s failed to %s recording", send_text_prefix, recording.request_start ? "start" : "stop");
532 0 : recording.request_ms = 0;
533 : }
534 : }
535 :
536 : #endif // HAL_MOUNT_XFROBOT_ENABLED
|
