Line data Source code
1 : /************************************************************
2 : * AP_mount -- library to control a 2 or 3 axis mount. *
3 : * *
4 : * Author: Joe Holdsworth; *
5 : * Ritchie Wilson; *
6 : * Amilcar Lucas; *
7 : * Gregory Fletcher; *
8 : * heavily modified by Randy Mackay *
9 : * *
10 : * Purpose: Move a 2 or 3 axis mount attached to vehicle, *
11 : * Used for mount to track targets or stabilise *
12 : * camera plus other modes. *
13 : * *
14 : * Usage: Use in main code to control mounts attached to *
15 : * vehicle. *
16 : * *
17 : * Comments: All angles in degrees, distances in meters *
18 : * unless otherwise stated. *
19 : ************************************************************/
20 : #pragma once
21 :
22 : #include "AP_Mount_config.h"
23 :
24 : #if HAL_MOUNT_ENABLED
25 :
26 : #include <GCS_MAVLink/GCS_config.h>
27 : #include <AP_HAL/AP_HAL_Boards.h>
28 : #include <AP_Math/AP_Math.h>
29 : #include <AP_Common/AP_Common.h>
30 : #include <AP_Common/Location.h>
31 : #include <GCS_MAVLink/GCS_MAVLink.h>
32 : #include <AP_Camera/AP_Camera_shareddefs.h>
33 : #include "AP_Mount_Params.h"
34 :
35 : // maximum number of mounts
36 : #define AP_MOUNT_MAX_INSTANCES 2
37 :
38 : // declare backend classes
39 : class AP_Mount_Backend;
40 : class AP_Mount_Servo;
41 : class AP_Mount_SoloGimbal;
42 : class AP_Mount_Alexmos;
43 : class AP_Mount_SToRM32;
44 : class AP_Mount_SToRM32_serial;
45 : class AP_Mount_MAVLink;
46 : class AP_Mount_Siyi;
47 : class AP_Mount_Scripting;
48 : class AP_Mount_Xacti;
49 : class AP_Mount_Viewpro;
50 : class AP_Mount_Topotek;
51 : class AP_Mount_CADDX;
52 : class AP_Mount_XFRobot;
53 :
54 : /*
55 : This is a workaround to allow the MAVLink backend access to the
56 : SmallEKF. It would be nice to find a neater solution to this
57 : */
58 :
59 : class AP_Mount
60 : {
61 : // declare backends as friends
62 : friend class AP_Mount_Backend;
63 : friend class AP_Mount_Servo;
64 : friend class AP_Mount_SoloGimbal;
65 : friend class AP_Mount_Alexmos;
66 : friend class AP_Mount_SToRM32;
67 : friend class AP_Mount_SToRM32_serial;
68 : friend class AP_Mount_MAVLink;
69 : friend class AP_Mount_Siyi;
70 : friend class AP_Mount_Scripting;
71 : friend class AP_Mount_Xacti;
72 : friend class AP_Mount_Viewpro;
73 : friend class AP_Mount_Topotek;
74 : friend class AP_Mount_CADDX;
75 :
76 : public:
77 : AP_Mount();
78 :
79 : /* Do not allow copies */
80 : CLASS_NO_COPY(AP_Mount);
81 :
82 : // get singleton instance
83 1279020 : static AP_Mount *get_singleton() {
84 1279020 : return _singleton;
85 : }
86 :
87 : // Enums
88 : enum class Type {
89 : None = 0, /// no mount
90 : #if HAL_MOUNT_SERVO_ENABLED
91 : Servo = 1, /// servo controlled mount
92 : #endif
93 : #if HAL_SOLO_GIMBAL_ENABLED
94 : SoloGimbal = 2, /// Solo's gimbal
95 : #endif
96 : #if HAL_MOUNT_ALEXMOS_ENABLED
97 : Alexmos = 3, /// Alexmos mount
98 : #endif
99 : #if HAL_MOUNT_STORM32MAVLINK_ENABLED
100 : SToRM32 = 4, /// SToRM32 mount using MAVLink protocol
101 : #endif
102 : #if HAL_MOUNT_STORM32SERIAL_ENABLED
103 : SToRM32_serial = 5, /// SToRM32 mount using custom serial protocol
104 : #endif
105 : #if HAL_MOUNT_MAVLINK_ENABLED
106 : MAVLink = 6, /// MAVLink v2 Gimbal protocol
107 : #endif
108 : #if HAL_MOUNT_SERVO_ENABLED
109 : BrushlessPWM = 7, /// Brushless (stabilized) gimbal using PWM protocol
110 : #endif
111 : #if HAL_MOUNT_SIYI_ENABLED
112 : Siyi = 8, /// Siyi gimbal using custom serial protocol
113 : #endif
114 : #if HAL_MOUNT_SCRIPTING_ENABLED
115 : Scripting = 9, /// Scripting gimbal driver
116 : #endif
117 : #if HAL_MOUNT_XACTI_ENABLED
118 : Xacti = 10, /// Xacti DroneCAN gimbal driver
119 : #endif
120 : #if HAL_MOUNT_VIEWPRO_ENABLED
121 : Viewpro = 11, /// Viewpro gimbal using a custom serial protocol
122 : #endif
123 : #if HAL_MOUNT_TOPOTEK_ENABLED
124 : Topotek = 12, /// Topotek gimbal using a custom serial protocol
125 : #endif
126 : #if HAL_MOUNT_CADDX_ENABLED
127 : CADDX = 13, /// CADDX gimbal using a custom serial protocol
128 : #endif
129 : #if HAL_MOUNT_XFROBOT_ENABLED
130 : XFRobot = 14, /// XFRobot gimbal using a custom serial protocol
131 : #endif
132 : };
133 :
134 : // init - detect and initialise all mounts
135 : __INITFUNC__ void init();
136 :
137 : // update - give mount opportunity to update servos. should be called at 10hz or higher
138 : void update();
139 :
140 : // used for gimbals that need to read INS data at full rate
141 : void update_fast();
142 :
143 : // return primary instance ID
144 0 : uint8_t get_primary_instance() const { return _primary; }
145 :
146 : // get_mount_type - returns the type of mount
147 8 : Type get_mount_type() const { return get_mount_type(_primary); }
148 : Type get_mount_type(uint8_t instance) const;
149 :
150 : // has_pan_control - returns true if the mount has yaw control (required for copters)
151 42 : bool has_pan_control() const { return has_pan_control(_primary); }
152 : bool has_pan_control(uint8_t instance) const;
153 :
154 : // get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point)
155 0 : enum MAV_MOUNT_MODE get_mode() const { return get_mode(_primary); }
156 : enum MAV_MOUNT_MODE get_mode(uint8_t instance) const;
157 :
158 : // set_mode - sets mount's mode
159 : // returns true if mode is successfully set
160 83 : void set_mode(enum MAV_MOUNT_MODE mode) { return set_mode(_primary, mode); }
161 : void set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode);
162 :
163 : // set_mode_to_default - restores the mode to it's default mode held in the MNTx_DEFLT_MODE parameter
164 : // this operation requires 60us on a Pixhawk/PX4
165 246 : void set_mode_to_default() { set_mode_to_default(_primary); }
166 : void set_mode_to_default(uint8_t instance);
167 :
168 : // set pitch_lock used in RC_TARGETING mode. If true, the gimbal's pitch target is maintained in earth-frame (horizon does not move up and down) rather than body frame
169 0 : void set_pitch_lock(bool pitch_lock) { set_pitch_lock(_primary, pitch_lock); }
170 : void set_pitch_lock(uint8_t instance, bool pitch_lock);
171 :
172 : // set roll_lock used in RC_TARGETING mode. If true, the gimbal's roll target is maintained in earth-frame (horizon does not rotate) rather than body frame
173 0 : void set_roll_lock(bool roll_lock) { set_roll_lock(_primary, roll_lock); }
174 : void set_roll_lock(uint8_t instance, bool roll_lock);
175 :
176 : // set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
177 : // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
178 0 : void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); }
179 : void set_yaw_lock(uint8_t instance, bool yaw_lock);
180 :
181 : #if AP_MOUNT_POI_LOCK_ENABLED
182 : // controls POI lock which locks gimbal to GPS point currently in view and switches to GPS Targeting mode, suspends tracking poi or clears it
183 4 : void set_poi_lock() { set_poi_lock(_primary); }
184 : void set_poi_lock(uint8_t instance);
185 2 : void clear_poi_lock() { clear_poi_lock(_primary); }
186 : void clear_poi_lock(uint8_t instance);
187 7 : void suspend_poi_lock() { suspend_poi_lock(_primary); }
188 : void suspend_poi_lock(uint8_t instance);
189 : #endif
190 :
191 : // set angle target in degrees
192 : // roll and pitch are in earth-frame
193 : // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
194 83 : void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) { set_angle_target(_primary, roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); }
195 : void set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);
196 :
197 : // sets rate target in deg/s
198 : // yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle)
199 : void set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_lock) { set_rate_target(_primary, roll_degs, pitch_degs, yaw_degs, yaw_lock); }
200 : void set_rate_target(uint8_t instance, float roll_degs, float pitch_degs, float yaw_degs, bool yaw_lock);
201 :
202 : // set_roi_target - sets target location that mount should attempt to point towards
203 34 : void set_roi_target(const Location &target_loc) { set_roi_target(_primary,target_loc); }
204 : void set_roi_target(uint8_t instance, const Location &target_loc);
205 :
206 : // clear_roi_target - clears target location that mount should attempt to point towards
207 18 : void clear_roi_target() { clear_roi_target(_primary); }
208 : void clear_roi_target(uint8_t instance);
209 :
210 : // point at system ID sysid
211 9 : void set_target_sysid(uint8_t sysid) { set_target_sysid(_primary, sysid); }
212 : void set_target_sysid(uint8_t instance, uint8_t sysid);
213 :
214 : // handling of set_roi_sysid message
215 : MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet);
216 :
217 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
218 : // point at next waypoint, with attitude offsets:
219 3 : void set_roi_target_wpnext_offset(const Vector3f &rpy) { set_roi_target_wpnext_offset(_primary, rpy); }
220 : void set_roi_target_wpnext_offset(uint8_t instance, const Vector3f &rpy);
221 : #endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
222 :
223 : // mavlink message handling:
224 : MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
225 : void handle_param_value(const mavlink_message_t &msg);
226 : void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
227 :
228 : // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
229 : void send_gimbal_device_attitude_status(mavlink_channel_t chan);
230 :
231 : // send a GIMBAL_MANAGER_INFORMATION message to GCS
232 : void send_gimbal_manager_information(mavlink_channel_t chan);
233 :
234 : // send a GIMBAL_MANAGER_STATUS message to GCS
235 : void send_gimbal_manager_status(mavlink_channel_t chan);
236 :
237 : #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
238 : // get poi information. Returns true on success and fills in gimbal attitude, location and poi location
239 : bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const;
240 : #endif
241 :
242 : // get attitude as a quaternion. returns true on success.
243 : // att_quat will be an earth-frame quaternion rotated such that
244 : // yaw is in body-frame.
245 : bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat);
246 :
247 : // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
248 : // returns true on success
249 : bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);
250 :
251 : // run pre-arm check. returns false on failure and fills in failure_msg
252 : // any failure_msg returned will not include a prefix
253 : bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
254 :
255 : #if AP_SCRIPTING_ENABLED
256 : // get target rate in deg/sec. returns true on success
257 : bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
258 :
259 : // get target angle in deg. returns true on success
260 : bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
261 :
262 : // get mount target location. returns true on success
263 : bool get_location_target(uint8_t instance, Location& target_loc);
264 : #endif
265 :
266 : // accessors for scripting backends and logging
267 : void set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg);
268 :
269 : #if AP_SCRIPTING_ENABLED
270 : // used by a script sending messages to a physical device to
271 : // indicate what targets can be either natively sent to the device
272 : // or internally handled by the script
273 : void set_natively_supported_mount_target_types(uint8_t instance, uint8_t types_mask);
274 : #endif // AP_SCRIPTING_ENABLED
275 :
276 : // write mount log packet for all backends
277 : void write_log();
278 :
279 : // write mount log packet for a single backend (called by camera library)
280 : void write_log(uint8_t instance, uint64_t timestamp_us);
281 :
282 : //
283 : // camera controls for gimbals that include a camera
284 : //
285 :
286 : // take a picture
287 : bool take_picture(uint8_t instance);
288 :
289 : // start or stop video recording
290 : // set start_recording = true to start record, false to stop recording
291 : bool record_video(uint8_t instance, bool start_recording);
292 :
293 : // set zoom specified as a rate or percentage
294 : bool set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value);
295 :
296 : // set focus specified as rate, percentage or auto
297 : // focus in = -1, focus hold = 0, focus out = 1
298 : SetFocusResult set_focus(uint8_t instance, FocusType focus_type, float focus_value);
299 :
300 : // set tracking to none, point or rectangle (see TrackingType enum)
301 : // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
302 : // p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
303 : bool set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2);
304 :
305 : // set camera lens as a value from 0 to 5
306 : bool set_lens(uint8_t instance, uint8_t lens);
307 :
308 : #if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
309 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
310 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
311 : bool set_camera_source(uint8_t instance, uint8_t primary_source, uint8_t secondary_source);
312 : #endif
313 :
314 : // send camera information message to GCS
315 : void send_camera_information(uint8_t instance, mavlink_channel_t chan) const;
316 :
317 : // send camera settings message to GCS
318 : void send_camera_settings(uint8_t instance, mavlink_channel_t chan) const;
319 :
320 : // send camera capture status message to GCS
321 : void send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const;
322 :
323 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
324 : // send camera thermal range message to GCS
325 : void send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan) const;
326 : #endif
327 :
328 : // change camera settings not normally used by autopilot
329 : // setting values from AP_Camera::Setting enum
330 : bool change_setting(uint8_t instance, CameraSetting setting, float value);
331 :
332 : //
333 : // rangefinder
334 : //
335 :
336 : // get rangefinder distance. Returns true on success
337 : bool get_rangefinder_distance(uint8_t instance, float& distance_m) const;
338 :
339 : // enable/disable rangefinder. Returns true on success
340 : bool set_rangefinder_enable(uint8_t instance, bool enable);
341 :
342 : // parameter var table
343 : static const struct AP_Param::GroupInfo var_info[];
344 :
345 : protected:
346 :
347 : static AP_Mount *_singleton;
348 :
349 : // parameters for backends
350 : AP_Mount_Params _params[AP_MOUNT_MAX_INSTANCES];
351 :
352 : // front end members
353 : uint8_t _num_instances; // number of mounts instantiated
354 : uint8_t _primary; // primary mount
355 : AP_Mount_Backend *_backends[AP_MOUNT_MAX_INSTANCES]; // pointers to instantiated mounts
356 :
357 : private:
358 : // Check if instance backend is ok
359 : AP_Mount_Backend *get_primary() const;
360 : AP_Mount_Backend *get_instance(uint8_t instance) const;
361 : AP_Mount_Backend *mount_device_from_mavlink_gimbal_id(uint8_t gimbal_device_id) const;
362 :
363 : void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg);
364 :
365 : MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet);
366 : MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet);
367 : MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet);
368 : MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
369 : void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg);
370 : void handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg);
371 : void handle_global_position_int(const mavlink_message_t &msg);
372 : void handle_gimbal_device_information(const mavlink_message_t &msg);
373 : void handle_gimbal_device_attitude_status(const mavlink_message_t &msg);
374 : };
375 :
376 : namespace AP {
377 : AP_Mount *mount();
378 : };
379 :
380 : #endif // HAL_MOUNT_ENABLED
|
