Line data Source code
1 : /*
2 : XFRobot camera gimbal using serial protocol backend class
3 : */
4 : #pragma once
5 :
6 : #include "AP_Mount_config.h"
7 :
8 : #if HAL_MOUNT_XFROBOT_ENABLED
9 :
10 : #include "AP_Mount_Backend_Serial.h"
11 : #include <AP_Math/quaternion.h>
12 :
13 : #define AP_MOUNT_XFROBOT_RECV_LENGTH_MAX 80 // maximum number of bytes that will be received from the gimbal
14 :
15 : class AP_Mount_XFRobot : public AP_Mount_Backend_Serial
16 : {
17 :
18 : public:
19 : // Constructor
20 : using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
21 :
22 : // update mount position - should be called periodically
23 : void update() override;
24 :
25 : // return true if healthy
26 : bool healthy() const override;
27 :
28 : // has_pan_control - returns true if this mount can control its pan (required for multicopters)
29 0 : bool has_pan_control() const override { return yaw_range_valid(); };
30 :
31 : // has_roll_control - returns true if this mount can control its roll
32 0 : bool has_roll_control() const override { return roll_range_valid(); };
33 :
34 : // has_pitch_control - returns true if this mount can control its tilt
35 0 : bool has_pitch_control() const override { return pitch_range_valid(); };
36 :
37 : //
38 : // camera controls for gimbals that include a camera
39 : //
40 :
41 : // take a picture. returns true on success
42 : bool take_picture() override;
43 :
44 : // start or stop video recording. returns true on success
45 : // set start_recording = true to start record, false to stop recording
46 : bool record_video(bool start_recording) override;
47 :
48 : // set zoom specified as a rate or percentage
49 : bool set_zoom(ZoomType zoom_type, float zoom_value) override;
50 :
51 : // set camera lens to cycle through pic-in-pic modes
52 : bool set_lens(uint8_t lens) override;
53 :
54 : // set focus specified as rate, percentage or auto
55 : // focus in = -1, focus hold = 0, focus out = 1
56 : SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
57 :
58 : protected:
59 :
60 : // get attitude as a quaternion. returns true on success
61 : bool get_attitude_quaternion(Quaternion& att_quat) override;
62 :
63 : private:
64 :
65 : // send text prefix string to reduce flash cost
66 : static const char* send_text_prefix;
67 :
68 : // camera and gimbal functions (aka order)
69 : enum class FunctionOrder : uint8_t {
70 : NONE = 0x00, // null command
71 : CALIBRATION = 0x01, // calibration
72 : ISOTHERM = 0x02, // isotherm mode
73 : NEUTRAL = 0x03, // neutral mode
74 : OSD_COORDINATE = 0x06, // OSD coordinate control (vehicle or target)
75 : IMAGE_AUTO_REVERSE = 0x07, // auto reverse on/off
76 : TIME_ZONE = 0x08, // set timezone
77 : ANGLE_CONTROL = 0x10, // angle control
78 : HEAD_LOCK = 0x11, // head lock mode
79 : HEAD_FOLLOW = 0x12, // head follow mode
80 : ORTHOVIEW = 0x13, // orthoview mode
81 : EULER_ANGLE_CONTROL = 0x14, // euler angle control
82 : GAZE_GEO_COORDINATES = 0x15,// gaze mode with geo coordinates
83 : GAZE_GEO_TARGET_LOCK = 0x16,// gaze mode with geo target lock
84 : TRACK = 0x17, // track mode
85 : CLICK_TO_AIM = 0x1A, // click to aim
86 : FPV = 0x1C, // first person view
87 : SHUTTER = 0x20, // take picture
88 : RECORD_VIDEO = 0x21, // record video
89 : ZOOM_IN = 0x22, // zoom in continuously
90 : ZOOM_OUT = 0x23, // zoom ou continuously
91 : ZOOM_STOP = 0x24, // stop zooming
92 : ZOOM_RATE = 0x25, // zoom at specfiied rate
93 : FOCUS = 0x26, // auto focus
94 : PALETTE = 0x2A, // set palette
95 : NIGHT_VISION = 0x2B, // set night vision on/off
96 : AREA_TEMP_MEASURE = 0x30, // set area temperature measurement on/off
97 : AREA_TEMP_ALERT = 0x31, // set area temperature alert on/off
98 : SPOT_TEMP_MEASURE = 0x33, // set spot temperature measurement on/off
99 : OSD = 0x73, // show/hide osd
100 : PIC_IN_PIC = 0x74, // show/hide picture in picture
101 : TARGET_DETECTION = 0x75, // set target detection on/off
102 : DIGITAL_ZOOM = 0x76, // set digital zoom on/off
103 : LIGHTING = 0x80, // set lighting intensity (0~255)
104 : RANGING = 0x81, // set ranging on/off
105 : };
106 :
107 : // parsing state
108 : enum class ParseState : uint8_t {
109 : WAITING_FOR_HEADER1,
110 : WAITING_FOR_HEADER2,
111 : WAITING_FOR_LENGTH_LOW,
112 : WAITING_FOR_LENGTH_HIGH,
113 : WAITING_FOR_DATA,
114 : WAITING_FOR_CRC_HIGH,
115 : WAITING_FOR_CRC_LOW
116 : };
117 :
118 : // camera image types
119 : enum class CameraType : uint8_t {
120 : MAIN_PIP_ZOOM_SUB_THERMAL = 1,
121 : MAIN_THERMAL_SUB_ZOOM = 2,
122 : MAIN_PIP_THERMAL_SUB_ZOOM = 3,
123 : MAIN_ZOOM_SUB_THERMAL = 4
124 : };
125 :
126 : // reading incoming packets from gimbal
127 : void read_incoming_packets();
128 :
129 : // process successfully decoded packets held in the _msg_buff structure
130 : void process_packet();
131 :
132 : // XFRobot can only send angles
133 0 : uint8_t natively_supported_mount_target_types() const override {
134 0 : return NATIVE_ANGLES_ONLY;
135 : };
136 :
137 : // send_target_angles
138 : void send_target_angles(const MountAngleTarget& angle_target_rad) override;
139 :
140 : // send simple (1byte) command to gimbal (e.g. take pic, start recording)
141 : // returns true on success, false on failure to send
142 : bool send_simple_command(FunctionOrder order, uint8_t param);
143 :
144 : // check for recording timeout
145 : void check_recording_timeout();
146 :
147 : // internal variables
148 : uint32_t last_send_ms; // system time of last do_mount_control sent to gimbal
149 :
150 : //
151 : // packet structures
152 : //
153 :
154 : // SendPacket main and sub frames for sending to the camera gimbal
155 : struct PACKED SendPacketMainAndSubFrame {
156 : uint8_t header1; // byte0, header1 (0x8A)
157 : uint8_t header2; // byte1, header2 (0x5E)
158 : uint16_t length; // byte2~3, length
159 : uint8_t version; // byte 4: version (always 0x02)
160 : int16_t roll_control; // byte 5~6: roll control value (int16, -18000 ~ +18000)
161 : int16_t pitch_control; // byte 7~8: pitch control value (int16, -18000 ~ +18000)
162 : int16_t yaw_control; // byte 9~10: yaw control value (int16, -18000 ~ +18000)
163 : uint8_t status; // byte 11: status, Bit0:INS valid, Bit2:control values valid
164 : int16_t roll_abs; // byte 12~13: absolute roll angle of vehicle (int16, -18000 ~ +18000)
165 : int16_t pitch_abs; // byte 14~15: absolute pitch angle of vehicle (int16, -9000 ~ +9000)
166 : int16_t yaw_abs; // byte 16~17: absolute yaw angle of vehicle (uint16, 0 ~ 36000)
167 : int16_t accel_north; // byte 18~19: North acceleration of vehicle (int16, cm/s/s)
168 : int16_t accel_east; // byte 20~21: East acceleration of vehicle (int16, cm/s/s)
169 : int16_t accel_up; // byte 22~23: Upward acceleration of vehicle (int16, cm/s/s)
170 : int16_t vel_north; // byte 24~25: North speed of vehicle (int16, decimeter/s)
171 : int16_t vel_east; // byte 26~27: East speed of vehicle (int16, decimeter/s)
172 : int16_t vel_up; // byte 28~29: Upward speed of vehicle (int16, decimeter/s)
173 : uint8_t request_code; // byte 30: request code of sub frame, header of requested sub data frame from GCU (aka camera)
174 : uint8_t reserved[6]; // byte 31~36: reserved/unused
175 : uint8_t sub_header; // byte 37: header (always 0x01)
176 : int32_t longitude; // byte 38~41: longitude of vehicle (int32, 1E7)
177 : int32_t latitude; // byte 42~45: latitude of vehicle (int32, 1E7)
178 : int32_t alt_amsl; // byte 46~49: altitude of vehicle (int32, mm)
179 : uint8_t gps_num_sats; // byte 50: number of satellites
180 : uint32_t gps_week_ms; // byte 51~54: GNSS milliseconds (uint32)
181 : uint16_t gps_week; // byte 55~56: GNSS week number (uint16)
182 : int32_t alt_rel; // byte 57~60: relative altitude (int32, mm, can be zero if unavailable)
183 : uint8_t reserved2[8]; // byte 61~68: reserved/unused
184 : FunctionOrder order; // byte 69: order
185 : };
186 :
187 : // ReplyPacket main and sub frames for receiving data from camera gimbal
188 : struct PACKED ReplyPacketMainAndSubFrame {
189 : uint8_t header1; // byte0, header1 (0x8A)
190 : uint8_t header2; // byte1, header2 (0x5E)
191 : uint16_t length; // byte2~3, length
192 : uint8_t version; // byte 4: version
193 : uint8_t mode; // byte 5: mode: 0x10:angle control, 0x11:head lock, 0x12: head follow, 0x13:orthoview, 0x14:euler angle control, 0x16:gaze, 0x17:track,0x1C:FPV
194 : uint16_t status; // 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
195 : int16_t horizontal_target; // byte 8~9: horizontal target (int16, -1000 ~ +1000, rightward is positive)
196 : int16_t vertical_target; // byte 10~11: vertical target (int16, -1000 ~ +1000, downward is positive)
197 : int16_t angle_x; // byte 12~13: x-axis angle (camera-frame pitch) of camera relative to vehicle (int16, -18000 ~ +18000)
198 : int16_t angle_y; // byte 14~15: y-axis angle (camera-frame roll) of camera relative to vehicle (int16, -18000 ~ +18000)
199 : int16_t angle_z; // byte 16~17: z-axis angle (camera-frame yaw) of camera relative to vehicle (int16, -18000 ~ +18000)
200 : int16_t roll_abs_cd; // byte 18~19: roll angle of camera (absolute) (int16, -9000 ~ +9000)
201 : int16_t pitch_abs_cd; // byte 20~21: pitch angle of camera (absolute) (int16, -18000 ~ +18000)
202 : uint16_t yaw_abs_cd; // byte 22~23: yaw angle of camera (absolute) (uint16, 0 ~ +36000)
203 : int16_t angvel_x; // byte 24~25: X-axis absolute angular velocity of camera (int16, centi-degrees/s)
204 : int16_t angvel_y; // byte 26~27: Y-axis absolute angular velocity of camera (int16, centi-degrees/s)
205 : int16_t angvel_z; // byte 28~29: Z-axis absolute angular velocity of camera (int16, centi-degrees/s)
206 : uint8_t reserved[7]; // byte 30~36: reserved/unused
207 : uint8_t sub_header; // byte 37: header (0x01)
208 : uint8_t hardware_version; // byte 38: hardware version (uint8)
209 : uint8_t firmware_version; // byte 39: firmware version (uint8)
210 : uint8_t pod_code; // byte 40: pod code
211 : uint16_t error_code; // byte 41~42: error code, Bit7:hardware error, Bit13: mavlink communication freq anomaly, Bit14:BNSS unpositioned, Bit15:GCU hardware error
212 : int32_t target_dist_dm; // byte 43~46: distance from target (int32, decimeters, -1m or 0m is invalid measurement)
213 : int32_t target_lng; // byte 47~50: longitude of target (int32, 1E7)
214 : int32_t target_lat; // byte 51~54: latitude of target (int32, 1E7)
215 : int32_t target_alt; // byte 55~58: altitude of target (int32, mm)
216 : uint16_t zoom_rate_rgb; // byte 59~60: zoom rate of RGB camera (uint16, resolution 0.1x)
217 : uint16_t zoom_rate_thermal; // byte 61~62: zoom rate of thermal camera (uint16, resolution 0.1x)
218 : uint8_t thermal_status; // 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
219 : uint16_t camera_status; // 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
220 : int8_t timezone; // byte 66: timezone (int8, -12 ~ +12)
221 : uint8_t reserved2[2]; // byte 67~68: reserved/unused
222 : FunctionOrder order; // byte 69: order
223 : };
224 :
225 : // 1byte Reply Packet. Used for most replies from GCU
226 : struct PACKED PacketReply1Byte {
227 : uint8_t execution_state; // byte 70, Success:0x00, Fail:0x01, Operating:0x02
228 : };
229 :
230 : // Packet CRC
231 : struct PACKED PacketCRC {
232 : uint8_t crc_high; // byte 70: CRC high byte
233 : uint8_t crc_low; // byte 71: CRC low byte
234 : };
235 :
236 : // structure to send set-atitude command to gimbal
237 : union SetAttitudePacket {
238 : struct PACKED {
239 : SendPacketMainAndSubFrame main;
240 : PacketCRC crc;
241 : } content;
242 : uint8_t bytes[sizeof(content)];
243 : };
244 :
245 : // structure to send a simple, 1byte command
246 : union SimpleCommand {
247 : struct PACKED {
248 : SendPacketMainAndSubFrame main;
249 : uint8_t param_1byte;
250 : PacketCRC crc;
251 : } content;
252 : uint8_t bytes[sizeof(content)];
253 : };
254 :
255 : // structure to decode incoming packets
256 : struct PACKED GCUSimpleReply {
257 : ReplyPacketMainAndSubFrame main;
258 : PacketReply1Byte param;
259 : PacketCRC crc;
260 : };
261 :
262 : // parser related variables
263 : union {
264 : GCUSimpleReply simple_reply; // simple reply
265 : uint8_t bytes[AP_MOUNT_XFROBOT_RECV_LENGTH_MAX]; // raw bytes
266 : } msg_buff;
267 : uint8_t msg_buff_len; // number of bytes held in msg_buff.bytes
268 :
269 : struct {
270 : ParseState state; // state of incoming message processing
271 : uint8_t len_expected; // expected number of data bytes we expect to receive
272 : uint16_t crc; // latest message's crc
273 : uint32_t last_received_ms; // system time of last message received and successfully parsed
274 : } parser;
275 :
276 : // latest attitude
277 : struct {
278 : float roll_ef_deg; // earth-frame roll angle in degrees
279 : float pitch_ef_deg; // earth-frame pitch angle in degrees
280 : float yaw_bf_deg; // body-frame yaw angle in degrees
281 : uint32_t update_ms; // system time of last update
282 : } attitude_latest;
283 :
284 : // local variables
285 : bool got_firmware_version = false; // true once hardware and firmware version has been received
286 : struct {
287 : bool recording; // true if currently recording video
288 : bool request_start; // true if requested start recording and request_ms > 0, false if requested stop recording
289 : uint32_t request_ms; // system time of request to start recording, 0 if not requested
290 : } recording;
291 : };
292 : #endif // HAL_MOUNT_XFROBOT_ENABLED
|
