-
Notifications
You must be signed in to change notification settings - Fork 16.8k
/
AP_Mount_Backend.h
359 lines (270 loc) · 14.8 KB
/
AP_Mount_Backend.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Mount driver backend class. Each supported mount type
needs to have an object derived from this class.
*/
#pragma once
#include "AP_Mount_config.h"
#if HAL_MOUNT_ENABLED
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_Camera/AP_Camera_shareddefs.h>
#include "AP_Mount.h"
class AP_Mount_Backend
{
public:
// Constructor
AP_Mount_Backend(class AP_Mount &frontend, class AP_Mount_Params ¶ms, uint8_t instance) :
_frontend(frontend),
_params(params),
_instance(instance)
{}
// init - performs any required initialisation for this instance
virtual void init();
// set device id of this instance, for MNTx_DEVID parameter
void set_dev_id(uint32_t id);
// update mount position - should be called periodically
virtual void update() = 0;
// used for gimbals that need to read INS data at full rate
virtual void update_fast() {}
// return true if healthy
virtual bool healthy() const { return true; }
// return true if this mount accepts roll or pitch targets
virtual bool has_roll_control() const;
virtual bool has_pitch_control() const;
// returns true if this mount can control its pan (required for multicopters)
virtual bool has_pan_control() const = 0;
// get attitude as a quaternion. returns true on success.
// att_quat will be an earth-frame quaternion rotated such that
// yaw is in body-frame.
virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0;
// get angular velocity of mount. Only available on some backends
virtual bool get_angular_velocity(Vector3f& rates) { return false; }
// returns true if mode is a valid mode, false otherwise:
bool valid_mode(MAV_MOUNT_MODE mode) const;
// get mount's mode
enum MAV_MOUNT_MODE get_mode() const { return _mode; }
// set mount's mode
bool set_mode(enum MAV_MOUNT_MODE mode);
// 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)
// If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }
// set angle target in degrees
// roll and pitch are in earth-frame
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);
// sets rate target in deg/s
// yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle)
void set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_is_earth_frame);
// set_roi_target - sets target location that mount should attempt to point towards
void set_roi_target(const Location &target_loc);
// clear_roi_target - clears target location that mount should attempt to point towards
void clear_roi_target();
// set_sys_target - sets system that mount should attempt to point towards
void set_target_sysid(uint8_t sysid);
// handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success
MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet);
// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
// requires original message in order to extract caller's sysid and compid
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
#if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED
// process MOUNT_CONFIGURE messages received from GCS. deprecated.
void handle_mount_configure(const mavlink_mount_configure_t &msg);
#endif
#if AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED
// process MOUNT_CONTROL messages received from GCS. deprecated.
void handle_mount_control(const mavlink_mount_control_t &packet);
#endif
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
// return gimbal capabilities sent to GCS in the GIMBAL_MANAGER_INFORMATION
virtual uint32_t get_gimbal_manager_capability_flags() const;
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void send_gimbal_manager_information(mavlink_channel_t chan);
// send a GIMBAL_MANAGER_STATUS message to GCS
void send_gimbal_manager_status(mavlink_channel_t chan);
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}
// handle a PARAM_VALUE message
virtual void handle_param_value(const mavlink_message_t &msg) {}
// handle a GLOBAL_POSITION_INT message
bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet);
// handle GIMBAL_DEVICE_INFORMATION message
virtual void handle_gimbal_device_information(const mavlink_message_t &msg) {}
// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {}
// get target rate in deg/sec. returns true on success
bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
// get target angle in deg. returns true on success
bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
// accessors for scripting backends
virtual bool get_location_target(Location &target_loc) { return false; }
virtual void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) {};
// write mount log packet
void write_log(uint64_t timestamp_us);
//
// camera controls for gimbals that include a camera
//
// take a picture. returns true on success
virtual bool take_picture() { return false; }
// start or stop video recording. returns true on success
// set start_recording = true to start record, false to stop recording
virtual bool record_video(bool start_recording) { return false; }
// set zoom specified as a rate or percentage
virtual bool set_zoom(ZoomType zoom_type, float zoom_value) { return false; }
// set focus specified as rate, percentage or auto
// focus in = -1, focus hold = 0, focus out = 1
virtual SetFocusResult set_focus(FocusType focus_type, float focus_value) { return SetFocusResult::UNSUPPORTED; }
// set tracking to none, point or rectangle (see TrackingType enum)
// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
// p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
virtual bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) { return false; }
// set camera lens as a value from 0 to 5
virtual bool set_lens(uint8_t lens) { return false; }
#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
virtual bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) { return false; }
#endif
// send camera information message to GCS
virtual void send_camera_information(mavlink_channel_t chan) const {}
// send camera settings message to GCS
virtual void send_camera_settings(mavlink_channel_t chan) const {}
// send camera capture status message to GCS
virtual void send_camera_capture_status(mavlink_channel_t chan) const {}
#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
// get poi information. Returns true on success and fills in gimbal attitude, location and poi location
bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc);
#endif
//
// rangefinder
//
// get rangefinder distance. Returns true on success
virtual bool get_rangefinder_distance(float& distance_m) const { return false; }
// enable/disable rangefinder. Returns true on success
virtual bool set_rangefinder_enable(bool enable) { return false; }
protected:
enum class MountTargetType {
ANGLE,
RATE,
};
// class for a single angle or rate target
class MountTarget {
public:
float roll;
float pitch;
float yaw;
bool yaw_is_ef;
// return body-frame yaw angle from a mount target (in radians)
float get_bf_yaw() const;
// return earth-frame yaw angle from a mount target (in radians)
float get_ef_yaw() const;
// set roll, pitch, yaw and yaw_is_ef from Vector3f
void set(const Vector3f& rpy, bool yaw_is_ef_in);
};
// options parameter bitmask handling
enum class Options : uint8_t {
RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode
};
bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; }
// returns true if user has configured a valid roll angle range
// allows user to disable roll even on 3-axis gimbal
bool roll_range_valid() const { return (_params.roll_angle_min < _params.roll_angle_max); }
// returns true if user has configured a valid pitch angle range
// allows user to disable pitch even on 3-axis gimbal
bool pitch_range_valid() const { return (_params.pitch_angle_min < _params.pitch_angle_max); }
// returns true if user has configured a valid yaw angle range
// allows user to disable yaw even on 3-axis gimbal
bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }
// returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
virtual bool suppress_heartbeat() const { return false; }
#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
// calculate the Location that the gimbal is pointing at
void calculate_poi();
#endif
// change to RC_TARGETTING mode if rc inputs have changed by more than the dead zone
// should be called on every update
void set_rctargeting_on_rcinput_change();
// get pilot input (in the range -1 to +1) received through RC
void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
// get angle or rate targets from pilot RC
// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s
void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const;
// get angle targets (in radians) to a Location
// returns true on success, false on failure
bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED;
// get angle targets (in radians) to ROI location
// returns true on success, false on failure
bool get_angle_target_to_roi(MountTarget& angle_rad) const WARN_IF_UNUSED;
// get angle targets (in radians) to home location
// returns true on success, false on failure
bool get_angle_target_to_home(MountTarget& angle_rad) const WARN_IF_UNUSED;
// get angle targets (in radians) to a vehicle with sysid of _target_sysid
// returns true on success, false on failure
bool get_angle_target_to_sysid(MountTarget& angle_rad) const WARN_IF_UNUSED;
// update angle targets using a given rate target
// the resulting angle_rad yaw frame will match the rate_rad yaw frame
// assumes a 50hz update rate
void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const;
// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t get_gimbal_device_flags() const;
// sent warning to GCS
void send_warning_to_GCS(const char* warning_str);
AP_Mount &_frontend; // reference to the front end which holds parameters
AP_Mount_Params &_params; // parameters for this backend
uint8_t _instance; // this instance's number
MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum)
bool _yaw_lock; // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame
// structure for MAVLink Targeting angle and rate targets
struct {
MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate)
MountTarget angle_rad; // angle target in radians
MountTarget rate_rads; // rate target in rad/s
} mnt_target;
#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
struct {
HAL_Semaphore sem; // semaphore protecting this structure
uint32_t poi_request_ms; // system time POI was last requested
uint32_t poi_update_ms; // system time POI was calculated
Location loc; // gimbal location used for poi calculation
Location poi_loc; // location of the POI
Quaternion att_quat; // attitude quaternion of the gimbal
} poi_calculation;
#endif
Location _roi_target; // roi target location
bool _roi_target_set; // true if the roi target has been set
uint8_t _target_sysid; // sysid to track
Location _target_sysid_location;// sysid target location
bool _target_sysid_location_set;// true if _target_sysid has been set
uint32_t _last_warning_ms; // system time of last warning sent to GCS
// structure holding the last RC inputs
struct {
bool initialised;
int16_t roll_in;
int16_t pitch_in;
int16_t yaw_in;
} last_rc_input;
// structure holding mavlink sysid and compid of controller of this gimbal
// see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS
struct mavlink_control_id_t {
uint8_t sysid;
uint8_t compid;
// equality operators
bool operator==(const mavlink_control_id_t &rhs) const { return (sysid == rhs.sysid && compid == rhs.compid); }
bool operator!=(const mavlink_control_id_t &rhs) const { return !(*this == rhs); }
} mavlink_control_id;
};
#endif // HAL_MOUNT_ENABLED