Line data Source code
1 : /*
2 : Topotek gimbal driver using custom serial protocol
3 :
4 : Packet format (courtesy of Topotek's SDK document)
5 :
6 : -------------------------------------------------------------------------------------------
7 : Field Index Bytes Description
8 : -------------------------------------------------------------------------------------------
9 : Frame Header 0 3 type of command
10 : Address Bit 3 2 the source address comes first, and the destination address comes last
11 : Data_Len 5 1 data length
12 : Control Bit 6 1 r -> query w -> setup and control
13 : Identification Bit 7 3 identification function
14 : Data 10 Data_Len
15 : Check Bit 2 the frame header is converted to HEX before reaching the check bit,
16 : the sum is done, and the result is converted to ASC-II. Two bytes, the high one first
17 : */
18 :
19 : #pragma once
20 :
21 : #include "AP_Mount_config.h"
22 :
23 : #if HAL_MOUNT_TOPOTEK_ENABLED
24 :
25 : #include "AP_Mount_Backend_Serial.h"
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_Common/AP_Common.h>
29 :
30 : #define AP_MOUNT_TOPOTEK_PACKETLEN_MAX 36 // maximum number of bytes in a packet sent to or received from the gimbal
31 : #define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 7 // parse the number of gimbal command types
32 :
33 : class AP_Mount_Topotek : public AP_Mount_Backend_Serial
34 : {
35 :
36 : public:
37 : // Constructor
38 : using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
39 :
40 : // Do not allow copies
41 : CLASS_NO_COPY(AP_Mount_Topotek);
42 :
43 : // update mount position - should be called periodically
44 : void update() override;
45 :
46 : // return true if healthy
47 : bool healthy() const override;
48 :
49 : // has_pan_control - returns true if this mount can control its pan (required for multicopters)
50 7 : bool has_pan_control() const override { return yaw_range_valid(); };
51 :
52 : //
53 : // camera controls for gimbals
54 : //
55 :
56 : // take a picture. returns true on success
57 : bool take_picture() override;
58 :
59 : // start or stop video recording
60 : // set start_recording = true to start record, false to stop recording
61 : bool record_video(bool start_recording) override;
62 :
63 : // set zoom specified as a rate
64 : bool set_zoom(ZoomType zoom_type, float zoom_value) override;
65 :
66 : // set focus specified as rate or auto
67 : // focus in = -1, focus hold = 0, focus out = 1
68 : SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
69 :
70 : // set tracking to none, point or rectangle (see TrackingType enum)
71 : // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
72 : // p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
73 : bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) override;
74 :
75 : // send command to gimbal to cancel tracking (if necessary)
76 : // returns true on success, false on failure to send message
77 : bool cancel_tracking();
78 :
79 : // set camera picture-in-picture mode
80 : bool set_lens(uint8_t lens) override;
81 :
82 : #if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
83 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
84 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
85 : bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
86 : #endif
87 :
88 2 : bool has_camera_information() const override { return true; }
89 : // return camera vendor name
90 2 : void get_camera_vendor_name(char *buf, uint8_t buflen) const override { strncpy(buf, "Topotek", buflen); }
91 : // return camera model name
92 2 : void get_camera_model_name(char *buf, uint8_t buflen) const override {
93 2 : if (!_got_gimbal_model_name) {
94 0 : return;
95 : }
96 2 : strncpy(buf, _model_name, buflen);
97 : }
98 : // return camera firmware version
99 2 : uint32_t get_camera_firmware_version() const override { return _firmware_ver; }
100 : // return camera capability flags
101 2 : uint32_t get_camera_cap_flags() const override {
102 : return (CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
103 : CAMERA_CAP_FLAGS_CAPTURE_IMAGE |
104 : CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM |
105 : CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS |
106 : CAMERA_CAP_FLAGS_HAS_TRACKING_POINT |
107 2 : CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE);
108 : }
109 :
110 : // send camera settings message to GCS
111 : void send_camera_settings(mavlink_channel_t chan) const override;
112 :
113 : //
114 : // rangefinder
115 : //
116 :
117 : // get rangefinder distance. Returns true on success
118 : bool get_rangefinder_distance(float& distance_m) const override;
119 :
120 : // enable/disable rangefinder. Returns true on success
121 : bool set_rangefinder_enable(bool enable) override;
122 :
123 : protected:
124 :
125 : // get attitude as a quaternion. returns true on success
126 : bool get_attitude_quaternion(Quaternion& att_quat) override;
127 :
128 : // Topotek can send either rates or angles
129 4717 : uint8_t natively_supported_mount_target_types() const override {
130 4717 : return NATIVE_ANGLES_AND_RATES_ONLY;
131 : };
132 :
133 : private:
134 :
135 : // header type (fixed or variable length)
136 : // first three bytes of packet determined by this value
137 : enum class HeaderType : uint8_t {
138 : FIXED_LEN = 0x00, // #TP will be sent
139 : VARIABLE_LEN = 0x01, // #tp will be sent
140 : };
141 :
142 : // address (2nd and 3rd bytes of packet)
143 : // first byte is always U followed by one of the other options
144 : enum class AddressByte : uint8_t {
145 : SYSTEM_AND_IMAGE = 68, // 'D'
146 : AUXILIARY_EQUIPMENT = 69, // 'E'
147 : GIMBAL = 71, // 'G'
148 : LENS = 77, // 'M'
149 : NETWORK = 80, // 'P'
150 : UART = 85, // 'U'
151 : };
152 :
153 : // control byte (read or write)
154 : // sent as 7th byte of packet
155 : enum class ControlByte : uint8_t {
156 : READ = 114, // 'r'
157 : WRITE = 119, // 'w'
158 : };
159 :
160 : // parsing state
161 : enum class ParseState : uint8_t {
162 : WAITING_FOR_HEADER1 = 0,// #
163 : WAITING_FOR_HEADER2, // T or t
164 : WAITING_FOR_HEADER3, // P or p
165 : WAITING_FOR_ADDR1, // normally U
166 : WAITING_FOR_ADDR2, // M, D, E, P, G
167 : WAITING_FOR_DATALEN,
168 : WAITING_FOR_CONTROL, // r or w
169 : WAITING_FOR_ID1, // e.g. 'G'
170 : WAITING_FOR_ID2, // e.g. 'A'
171 : WAITING_FOR_ID3, // e.g. 'C'
172 : WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A')
173 : WAITING_FOR_CRC_LOW,
174 : WAITING_FOR_CRC_HIGH,
175 : };
176 :
177 : // tracking status
178 : enum class TrackingStatus : uint8_t {
179 : STOPPED_TRACKING = 0x30, // not tracking
180 : WAITING_FOR_TRACKING = 0x31, // wait to track command status
181 : TRACKING_IN_PROGRESS = 0x32, // the status is being tracked
182 : LENS_UNSUPPORT_TRACK = 0x34, // this lens does not support tracking
183 : };
184 :
185 : // identifier bytes
186 : typedef char Identifier[3];
187 :
188 : // send text prefix string
189 : static const char* send_message_prefix;
190 :
191 : // reading incoming packets from gimbal and confirm they are of the correct format
192 : void read_incoming_packets();
193 :
194 : // request gimbal attitude
195 : void request_gimbal_attitude();
196 :
197 : // request gimbal memory card information
198 : void request_gimbal_sdcard_info();
199 :
200 : // request gimbal tracking status
201 : void request_track_status();
202 :
203 : // request gimbal version
204 : void request_gimbal_version();
205 :
206 : // request gimbal model name
207 : void request_gimbal_model_name();
208 :
209 : // send angle target in radians to gimbal
210 : void send_target_angles(const MountAngleTarget& angle_rad) override;
211 :
212 : // send rate target in rad/s to gimbal
213 : void send_target_rates(const MountRateTarget& rate_rads) override;
214 :
215 : // send time and date to gimbal
216 : bool send_time_to_gimbal();
217 :
218 : // send GPS-related information to the gimbal
219 : bool send_location_info();
220 :
221 : // attitude information analysis of gimbal
222 : void gimbal_angle_analyse();
223 :
224 : // gimbal video information analysis
225 : void gimbal_record_analyse();
226 :
227 : // information analysis of gimbal storage card
228 : void gimbal_sdcard_analyse();
229 :
230 : // gimbal tracking information analysis
231 : void gimbal_track_analyse();
232 :
233 : // gimbal basic information analysis
234 : void gimbal_version_analyse();
235 :
236 : // gimbal model name message analysis
237 : void gimbal_model_name_analyse();
238 :
239 : // gimbal distance information analysis
240 : void gimbal_dist_info_analyse();
241 :
242 : // return the address to send as the source of our packets. The
243 : // gimbal sends its replies out of the interface named here, so this
244 : // must be the interface we are actually connected to. This is one
245 : // of the few places it is legitimate to ask a port how it is
246 : // connected; the protocol itself carries that information
247 14764 : AddressByte source_address() const {
248 14764 : return _uart->is_network_port() ? AddressByte::NETWORK : AddressByte::UART;
249 : }
250 :
251 : // calculate checksum
252 : uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const;
253 :
254 : // hexadecimal to character conversion
255 : uint8_t hex2char(uint8_t data) const;
256 :
257 : // send a fixed length packet to gimbal
258 : // returns true on success, false if serial port initialization failed
259 : bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value);
260 :
261 : // send a variable length packet to gimbal
262 : // returns true on success, false if serial port initialization failed
263 : bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len);
264 :
265 : // set gimbal's lock vs follow mode
266 : // lock should be true if gimbal should maintain an earth-frame target
267 : // lock is false to follow / maintain a body-frame target
268 : bool set_gimbal_lock(bool lock);
269 :
270 : // members
271 : bool _recording; // recording status (received from gimbal)
272 : bool _is_tracking; // whether to enable the tracking state
273 : TrackingStatus _last_tracking_state = TrackingStatus::STOPPED_TRACKING; // last tracking state received from gimbal
274 : uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking
275 : bool _sdcard_status; // memory card status (received from gimbal)
276 : bool _last_lock; // last lock mode sent to gimbal
277 : bool _got_gimbal_version; // true if gimbal's version has been received
278 : bool _got_gimbal_model_name; // true if gimbal's model name has been received
279 : bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets)
280 : bool _last_focus_stop; // true if focus has been stopped (used to re-sent in order to handle lost packets)
281 : char _model_name[16]; // gimbal model name, always null-terminated
282 : uint8_t _sent_time_count; // count of current time messages sent to gimbal
283 : uint32_t _firmware_ver; // firmware version
284 : Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
285 : uint32_t _last_current_angle_ms; // system time (in milliseconds) that angle information received from the gimbal
286 : uint32_t _last_req_current_info_ms; // system time that this driver last requested current gimbal infomation
287 : uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps)
288 : uint8_t _stop_order_count; // number of stop commands sent since target rates became zero
289 : float _measure_dist_m = -1.0f; // latest rangefinder distance (in meters)
290 : uint8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc
291 : uint8_t _msg_buff_len; // number of bytes in the msg buffer
292 : struct {
293 : ParseState state; // parser state
294 : uint8_t data_len; // expected number of data bytes
295 : } _parser;
296 :
297 : // mapping from received message key to member function pointer to consume the message
298 : typedef struct {
299 : uint8_t uart_cmd_key[4]; // gimbal message key;
300 : void (AP_Mount_Topotek::*func)(void); // member function to consume messager
301 : } UartCmdFunctionHandler;
302 :
303 : // stores command ID and corresponding member functions that are compared with the command received by the gimbal
304 : UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = {
305 : {{"GIA"}, &AP_Mount_Topotek::gimbal_angle_analyse},
306 : {{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse},
307 : {{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse},
308 : {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse},
309 : {{"TRC"}, &AP_Mount_Topotek::gimbal_track_analyse},
310 : {{"VSN"}, &AP_Mount_Topotek::gimbal_version_analyse},
311 : {{"PA2"}, &AP_Mount_Topotek::gimbal_model_name_analyse}
312 : };
313 : };
314 :
315 : #endif // HAL_MOUNT_TOPOTEK_ENABLED
|
