Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_ENABLED
4 :
5 : #include "AP_Mount_Backend.h"
6 :
7 : #include <AP_AHRS/AP_AHRS.h>
8 : #include <GCS_MAVLink/GCS.h>
9 : #include <AP_Logger/AP_Logger.h>
10 : #include <AP_Terrain/AP_Terrain.h>
11 : #include <AP_Vehicle/AP_Vehicle.h>
12 :
13 : extern const AP_HAL::HAL& hal;
14 :
15 : #define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate
16 : #define AP_MOUNT_POI_REQUEST_TIMEOUT_MS 30000 // POI calculations continue to be updated for this many seconds after last request
17 : #define AP_MOUNT_POI_RESULT_TIMEOUT_MS 3000 // POI calculations valid for 3 seconds
18 : #define AP_MOUNT_POI_DIST_M_MAX 10000 // POI calculations limit of 10,000m (10km)
19 : #define AP_MOUNT_SYSID_TIMEOUT_MS 3000 // sysid target location considered stale if not updated within this many ms (matches AP_Follow's own default FOLL_TIMEOUT)
20 :
21 : // Default init function for every mount
22 112 : void AP_Mount_Backend::init()
23 : {
24 : // setting default target sysid from parameters
25 112 : _target_sysid = _params.sysid_default.get();
26 :
27 : #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
28 : // create a calculation thread for poi.
29 112 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Mount_Backend::calculate_poi, void),
30 : "mount_calc_poi",
31 : 8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
32 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mount: failed to start POI thread");
33 : }
34 : #endif
35 112 : }
36 :
37 : // set device id of this instance, for MNTx_DEVID parameter
38 0 : void AP_Mount_Backend::set_dev_id(uint32_t id)
39 : {
40 0 : _params.dev_id.set_and_save(int32_t(id));
41 0 : }
42 :
43 : // base implementation should be called from derived classes for common functionality
44 399951 : void AP_Mount_Backend::update()
45 : {
46 : // move mount to a "retracted position" into the fuselage or out of it
47 399951 : const bool mount_open = (_mode == MAV_MOUNT_MODE_RETRACT);
48 399951 : SRV_Channels::move_servo(_open_idx, mount_open, 0, 1);
49 :
50 : #if AP_MOUNT_POI_LOCK_ENABLED
51 399951 : update_poi_lock_target();
52 : #endif // AP_MOUNT_POI_LOCK_ENABLED
53 :
54 : // location exists for mode
55 399951 : Location current_loc;
56 399951 : switch (_mode) {
57 372469 : case MAV_MOUNT_MODE_RETRACT:
58 : case MAV_MOUNT_MODE_NEUTRAL:
59 : case MAV_MOUNT_MODE_MAVLINK_TARGETING:
60 : case MAV_MOUNT_MODE_RC_TARGETING:
61 : case MAV_MOUNT_MODE_ENUM_END:
62 372469 : break;
63 27482 : case MAV_MOUNT_MODE_GPS_POINT:
64 : case MAV_MOUNT_MODE_SYSID_TARGET:
65 : case MAV_MOUNT_MODE_HOME_LOCATION:
66 : case MAV_MOUNT_MODE_WPNEXT_OFFSET:
67 27482 : if (!AP::ahrs().get_location(current_loc)) {
68 0 : send_warning_to_GCS("not targeting, no location");
69 : }
70 : }
71 399951 : }
72 :
73 :
74 : // return true if this mount accepts roll targets
75 0 : bool AP_Mount_Backend::has_roll_control() const
76 : {
77 0 : return (_params.roll_angle_min < _params.roll_angle_max);
78 : }
79 :
80 : // return true if this mount accepts pitch targets
81 0 : bool AP_Mount_Backend::has_pitch_control() const
82 : {
83 0 : return (_params.pitch_angle_min < _params.pitch_angle_max);
84 : }
85 :
86 842 : bool AP_Mount_Backend::valid_mode(MAV_MOUNT_MODE mode) const
87 : {
88 842 : switch (mode) {
89 842 : case MAV_MOUNT_MODE_RETRACT...MAV_MOUNT_MODE_WPNEXT_OFFSET:
90 842 : return true;
91 0 : case MAV_MOUNT_MODE_ENUM_END:
92 0 : return false;
93 : }
94 0 : return false;
95 : }
96 :
97 842 : bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
98 : {
99 842 : if (!valid_mode(mode)) {
100 0 : return false;
101 : }
102 842 : _mode = mode;
103 842 : return true;
104 : }
105 :
106 : // called when mount mode is RC-targetting, updates the mnt_target object from RC inputs:
107 336293 : void AP_Mount_Backend::update_mnt_target_from_rc_target()
108 : {
109 336293 : if (rc().in_rc_failsafe()) {
110 794 : if (option_set(Options::NEUTRAL_ON_RC_FS)) {
111 43 : mnt_target.angle_rad.set(_params.neutral_angles.get() * DEG_TO_RAD, false);
112 43 : mnt_target.target_type = MountTargetType::ANGLE;
113 802 : return;
114 : }
115 : // zero rates when in failsafe
116 751 : if (mnt_target.target_type == MountTargetType::RATE) {
117 : // note that we do not change the frame here; if a gimbal
118 : // is tracking in earth-frame it will continue to do so.
119 438 : mnt_target.rate_rads.roll = 0;
120 438 : mnt_target.rate_rads.pitch = 0;
121 438 : mnt_target.rate_rads.yaw = 0;
122 : }
123 751 : return;
124 : }
125 :
126 335499 : if (!rc().has_valid_input()) {
127 8 : return;
128 : }
129 :
130 : // get RC input from pilot
131 : float roll_in, pitch_in, yaw_in;
132 335491 : get_rc_input(roll_in, pitch_in, yaw_in);
133 :
134 : // frame locks
135 335491 : bool FPV_option = option_set(Options::FPV_LOCK); //FPV_LOCK forces bodyframe on all axes in RC targeting mode
136 335491 : mnt_target.angle_rad.yaw_is_ef = FPV_option ? false : _yaw_lock;
137 335491 : mnt_target.angle_rad.roll_is_ef = FPV_option ? false : _roll_lock;
138 335491 : mnt_target.angle_rad.pitch_is_ef = FPV_option ? false : _pitch_lock;
139 335491 : mnt_target.rate_rads.yaw_is_ef = FPV_option ? false : _yaw_lock;
140 335491 : mnt_target.rate_rads.roll_is_ef = FPV_option ? false : _roll_lock;
141 335491 : mnt_target.rate_rads.pitch_is_ef = FPV_option ? false : _pitch_lock;
142 :
143 : // if RC_RATE is zero, targets are angle
144 335491 : if (_params.rc_rate_max <= 0) {
145 61095 : mnt_target.target_type = MountTargetType::ANGLE;
146 :
147 : // roll angle
148 61095 : mnt_target.angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min));
149 :
150 : // pitch angle
151 61095 : mnt_target.angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min));
152 :
153 : // yaw angle
154 61095 : mnt_target.angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min));
155 :
156 : // if in yaw ef lock, we use the captured and adjusted yaw_lock_heading rad to
157 : // adjust the yaw so that any RC yaw changes are reflected in locked heading
158 61095 : if (mnt_target.angle_rad.yaw_is_ef) {
159 0 : mnt_target.angle_rad.yaw = wrap_PI(mnt_target.angle_rad.yaw + _yaw_lock_heading_rad);
160 : }
161 : } else {
162 : // calculate rate targets
163 274396 : mnt_target.target_type = MountTargetType::RATE;
164 274396 : const float rc_rate_max_rads = radians(_params.rc_rate_max.get());
165 274396 : mnt_target.rate_rads.roll = roll_in * rc_rate_max_rads;
166 274396 : mnt_target.rate_rads.pitch = pitch_in * rc_rate_max_rads;
167 274396 : mnt_target.rate_rads.yaw = yaw_in * rc_rate_max_rads;
168 : }
169 : }
170 :
171 : // called for stabilized mounts which use roll and pitch angle targets that are earth frame
172 : // to remove vehicle lean angle if pitch or roll is not locked, ie convert to actual body frame
173 268925 : void AP_Mount_Backend::adjust_mnt_target_if_RP_locked()
174 : {
175 : // retrieve lean angles from ahrs
176 268925 : const AP_AHRS &ahrs = AP::ahrs();
177 268925 : Vector2f ahrs_angle_rad = {ahrs.get_roll_rad(), ahrs.get_pitch_rad()};
178 :
179 : // rotate ahrs roll and pitch angles to gimbal yaw
180 268925 : if (has_pan_control()) {
181 0 : const float yaw_bf_rad = constrain_float(mnt_target.angle_rad.get_bf_yaw(), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
182 0 : ahrs_angle_rad.rotate(yaw_bf_rad);
183 : }
184 :
185 : // remove roll and pitch lean angle to correct to body frame
186 268925 : if (!mnt_target.angle_rad.roll_is_ef){
187 0 : mnt_target.angle_rad.roll += ahrs_angle_rad.x;
188 : }
189 268925 : if (!mnt_target.angle_rad.pitch_is_ef){
190 0 : mnt_target.angle_rad.pitch += ahrs_angle_rad.y;
191 : }
192 268925 : }
193 :
194 : // set angle target in degrees
195 : // roll and pitch are in earth-frame
196 : // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
197 100 : void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
198 : {
199 : // enforce angle limits
200 100 : roll_deg = constrain_float(roll_deg, _params.roll_angle_min, _params.roll_angle_max);
201 100 : pitch_deg = constrain_float(pitch_deg, _params.pitch_angle_min, _params.pitch_angle_max);
202 100 : if (!yaw_is_earth_frame) {
203 : // only limit yaw if in body-frame. earth-frame yaw limiting is backend specific
204 : // custom wrap code (instead of wrap_180) to better handle yaw of <= -180
205 100 : if (yaw_deg > 180) {
206 0 : yaw_deg -= 360;
207 : }
208 100 : yaw_deg = constrain_float(yaw_deg, _params.yaw_angle_min, _params.yaw_angle_max);
209 : }
210 :
211 : // set angle targets
212 100 : mnt_target.target_type = MountTargetType::ANGLE;
213 100 : mnt_target.angle_rad.roll = radians(roll_deg);
214 100 : mnt_target.angle_rad.pitch = radians(pitch_deg);
215 100 : mnt_target.angle_rad.yaw = radians(yaw_deg);
216 100 : mnt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame;
217 :
218 : // set the mode to mavlink targeting
219 100 : set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
220 :
221 : // optionally set RC_TARGETING yaw lock state
222 100 : if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
223 0 : set_yaw_lock(yaw_is_earth_frame);
224 : }
225 100 : }
226 :
227 : // sets rate target in deg/s
228 : // yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle)
229 1 : void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_is_earth_frame)
230 : {
231 : // set rate targets
232 1 : mnt_target.target_type = MountTargetType::RATE;
233 1 : mnt_target.rate_rads.roll = radians(roll_degs);
234 1 : mnt_target.rate_rads.pitch = radians(pitch_degs);
235 1 : mnt_target.rate_rads.yaw = radians(yaw_degs);
236 1 : mnt_target.rate_rads.yaw_is_ef = yaw_is_earth_frame;
237 1 : mnt_target.last_rate_request_ms = AP_HAL::millis();
238 :
239 : // set the mode to mavlink targeting
240 1 : set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
241 :
242 : // optionally set RC_TARGETING yaw lock state
243 1 : if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
244 0 : set_yaw_lock(yaw_is_earth_frame);
245 : }
246 1 : }
247 :
248 : // set_roi_target - sets target location that mount should attempt to point towards
249 33 : void AP_Mount_Backend::set_roi_target(const Location &target_loc)
250 : {
251 : // set the target gps location
252 33 : _roi_target = target_loc;
253 :
254 : // set the mode to GPS tracking mode
255 33 : set_mode(MAV_MOUNT_MODE_GPS_POINT);
256 :
257 : // optionally set RC_TARGETING yaw lock state
258 33 : if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
259 0 : set_yaw_lock(true);
260 : }
261 33 : }
262 :
263 : #if AP_MOUNT_POI_LOCK_ENABLED
264 : // set poi_lock - switch to GPS Targeting mode using current gimbal view's GPS point or save poi location as target
265 4 : void AP_Mount_Backend::set_poi_lock()
266 : {
267 4 : saved_mount_mode = get_mode(); //save current mount mode for the suspend_poi_lock
268 4 : if (!roi_is_set()) {
269 2 : mnt_target.poi_start_ms = AP_HAL::millis();
270 2 : mnt_target.pointing_at_poi_at_home_alt = false;
271 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking r=%.1f p=%.1f y=%.1f", degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(mnt_target.angle_rad.yaw));
272 : } else { // there is a poi target, just turn POI tracking back on
273 2 : set_mode(MAV_MOUNT_MODE_GPS_POINT);
274 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking");
275 2 : mnt_target.poi_start_ms = 0;
276 : }
277 4 : }
278 :
279 : // clear poi_lock - clear POI location and revert to default mode
280 0 : void AP_Mount_Backend::clear_poi_lock()
281 : {
282 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Cleared");
283 0 : clear_roi_target();
284 0 : }
285 :
286 : // suspend_poi_lock - revert to saved targeting mode, if it exists and POI target exists, otherwise do nothing
287 6 : void AP_Mount_Backend::suspend_poi_lock()
288 : {
289 6 : if (roi_is_set() && saved_mount_mode != MAV_MOUNT_MODE_ENUM_END) {
290 4 : set_mode(saved_mount_mode); // set back to mode before GPS_POINT if its been set by switch going HIGH
291 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Revert mode,target saved");;
292 : }
293 6 : }
294 :
295 : // update_poi_lock_target - tries to obtain POI location and start tracking,caller only needs to set poi_start_ms to current time to execute this
296 399951 : void AP_Mount_Backend::update_poi_lock_target()
297 : {
298 399951 : if (mnt_target.poi_start_ms == 0) {
299 399949 : return;
300 : }
301 :
302 3 : Location target_location;
303 :
304 3 : if (!mnt_target.pointing_at_poi_at_home_alt) {
305 2 : if (calculate_poi_at_home_alt(target_location)) {
306 2 : set_roi_target(target_location);
307 : }
308 : // attempt the home-alt POI only once per switch engagement; retrying would repeat warnings at the update rate
309 2 : mnt_target.pointing_at_poi_at_home_alt = true;
310 : }
311 :
312 : #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
313 : // POI calculation is running and but will silently give up after 3 seconds normally if it does not succeed
314 : // try to resolve a AuxFunc POI command to a lat/lng/alt using get_poi
315 : // set up variables for get_poi call
316 3 : Quaternion quat;
317 3 : Location vehicle_location;
318 : // clear the terrain_available flag
319 3 : bool terrain_available = false;
320 : #if AP_TERRAIN_AVAILABLE
321 3 : AP_Terrain *terrain = AP::terrain();
322 3 : if (terrain->enabled()) {
323 2 : terrain_available = true;
324 : }
325 : #endif // AP_TERRAIN_AVAILABLE
326 : // if terrain is not compiled in or not enabled then use home alt intersecting POI
327 :
328 3 : if (!terrain_available) {
329 1 : mnt_target.poi_start_ms = 0;
330 1 : return;
331 : }
332 : // otherwise,if terrain intersecting poi available, use it and start tracking
333 : // otherwise stop calcuation in thread, and attempt to get POI at home alt
334 : // if that fails, give warning
335 2 : if (get_poi(_instance, quat, vehicle_location, target_location)) {
336 1 : set_roi_target(target_location);
337 1 : mnt_target.poi_start_ms = 0;
338 1 : } else if (AP_HAL::millis() - mnt_target.poi_start_ms > 5000) {
339 : //stop terrain-based POI calculation
340 0 : mnt_target.poi_start_ms = 0;
341 : }
342 : #else
343 : // terrain-based POI is not available so the home-alt POI stands
344 : mnt_target.poi_start_ms = 0;
345 : #endif // AP_MOUNT_POI_TO_LATLONALT_ENABLED
346 : }
347 :
348 : #endif // AP_MOUNT_POI_LOCK_ENABLED
349 :
350 : // set yaw lock - sets the _yaw_lock variable and captures current earth frame heading of mount for targeting in RC Targeting mode
351 0 : void AP_Mount_Backend::set_yaw_lock(bool yaw_lock)
352 : {
353 : // if yaw not locked already, capture mount's earth frame heading for later possible use
354 0 : if (!_yaw_lock) {
355 : float roll_in, pitch_in, yaw_in;
356 0 : get_rc_input(roll_in, pitch_in, yaw_in);
357 : //adjust current ef mount heading by current RC yaw angle input and store for later use
358 0 : Quaternion att_quat_bf_rad;
359 0 : if (get_attitude_quaternion(att_quat_bf_rad)) {
360 0 : const float euler_yaw_bf_rad = att_quat_bf_rad.get_euler_yaw();
361 0 : const float euler_yaw_ef_rad = wrap_PI(euler_yaw_bf_rad + AP::ahrs().get_yaw_rad());
362 0 : _yaw_lock_heading_rad = wrap_PI(euler_yaw_ef_rad - radians(wrap_180((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)));
363 : }
364 : }
365 0 : _yaw_lock = yaw_lock;
366 0 : }
367 :
368 :
369 : // clear_roi_target - clears target location that mount should attempt to point towards
370 12 : void AP_Mount_Backend::clear_roi_target()
371 : {
372 : // clear the target GPS location
373 12 : _roi_target.zero();
374 :
375 : // reset the mode if in GPS tracking mode
376 12 : if (get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
377 4 : MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get();
378 4 : set_mode(default_mode);
379 : }
380 :
381 12 : clear_roi_pending = true;
382 12 : }
383 :
384 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
385 : // set_roi_target_wpnext_offset - point towards next waypoint with a
386 : // given attitude offset
387 3 : void AP_Mount_Backend::set_roi_target_wpnext_offset(const Vector3f &rpy)
388 : {
389 : // set the target gps location
390 3 : _roi_wpnext_rpy = rpy;
391 :
392 : // set the mode to GPS tracking mode
393 3 : set_mode(MAV_MOUNT_MODE_WPNEXT_OFFSET);
394 3 : }
395 : #endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
396 :
397 : // set_sys_target - sets system that mount should attempt to point towards
398 9 : void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
399 : {
400 9 : if (sysid != _target_sysid) {
401 : // forget the previous target's location so it isn't briefly
402 : // reported as the new target's (still-fresh) position
403 5 : _target_sysid_location.zero();
404 5 : _target_sysid_update_ms = 0;
405 : }
406 9 : _target_sysid = sysid;
407 :
408 : // set the mode to sysid tracking mode
409 9 : set_mode(MAV_MOUNT_MODE_SYSID_TARGET);
410 :
411 : // optionally set RC_TARGETING yaw lock state
412 9 : if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
413 0 : set_yaw_lock(true);
414 : }
415 9 : }
416 :
417 : #if HAL_GCS_ENABLED
418 : // send a CAMERA_INFORMATION message to GCS
419 5 : void AP_Mount_Backend::send_camera_information(mavlink_channel_t chan) const
420 : {
421 5 : if (!has_camera_information()) {
422 0 : return;
423 : }
424 :
425 5 : uint8_t vendor_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN+1] {};
426 5 : get_camera_vendor_name((char*)vendor_name, ARRAY_SIZE(vendor_name)-1);
427 :
428 5 : uint8_t model_name[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN+1] {};
429 5 : get_camera_model_name((char*)model_name, ARRAY_SIZE(model_name)-1);
430 :
431 5 : const char cam_definition_uri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] {};
432 :
433 5 : mavlink_msg_camera_information_send(
434 : chan,
435 : AP_HAL::millis(), // time_boot_ms
436 : vendor_name, // vendor_name uint8_t[32]
437 : model_name, // model_name uint8_t[32]
438 5 : get_camera_firmware_version(), // firmware_version uint32_t
439 5 : get_camera_focal_length_mm(), // focal_length float (mm)
440 : NaNf, // sensor_size_h float (mm)
441 : NaNf, // sensor_size_v float (mm)
442 : 0, // resolution_h uint16_t (pix)
443 : 0, // resolution_v uint16_t (pix)
444 5 : get_camera_lens_id(), // lens_id uint8_t
445 5 : get_camera_cap_flags(), // flags uint32_t (CAMERA_CAP_FLAGS)
446 : 0, // cam_definition_version uint16_t
447 : cam_definition_uri, // cam_definition_uri char[140]
448 5 : _instance + 1); // gimbal_device_id uint8_t
449 : }
450 :
451 : // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
452 65396 : void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan)
453 : {
454 65396 : if (suppress_heartbeat()) {
455 : // block heartbeat from transmitting to the GCS
456 1023 : GCS_MAVLINK::disable_channel_routing(chan);
457 : }
458 :
459 65396 : Quaternion att_quat;
460 65396 : if (!get_attitude_quaternion(att_quat)) {
461 114 : return;
462 : }
463 65282 : Vector3f ang_velocity { nanf(""), nanf(""), nanf("") };
464 65282 : IGNORE_RETURN(get_angular_velocity(ang_velocity));
465 :
466 : // construct quaternion array
467 65282 : const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4};
468 :
469 65282 : mavlink_msg_gimbal_device_attitude_status_send(chan,
470 : 0, // target system
471 : 0, // target component
472 : AP_HAL::millis(), // autopilot system time
473 65282 : get_gimbal_device_flags(),
474 : quat_array, // attitude expressed as quaternion
475 : ang_velocity.x, // roll axis angular velocity (NaN for unknown)
476 : ang_velocity.y, // pitch axis angular velocity (NaN for unknown)
477 : ang_velocity.z, // yaw axis angular velocity (NaN for unknown)
478 : 0, // failure flags (not supported)
479 65282 : std::numeric_limits<double>::quiet_NaN(), // delta_yaw (NaN for unknonw)
480 65282 : std::numeric_limits<double>::quiet_NaN(), // delta_yaw_velocity (NaN for unknonw)
481 65282 : _instance + 1); // gimbal_device_id
482 : }
483 : #endif
484 :
485 : // return gimbal manager capability flags used by GIMBAL_MANAGER_INFORMATION message
486 0 : uint32_t AP_Mount_Backend::get_gimbal_manager_capability_flags() const
487 : {
488 0 : uint32_t cap_flags = GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT |
489 : GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
490 : GIMBAL_MANAGER_CAP_FLAGS_HAS_RC_INPUTS |
491 : GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL |
492 : GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
493 :
494 : // roll control
495 0 : if (has_roll_control()) {
496 0 : cap_flags |= GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS |
497 : GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW |
498 : GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK;
499 : }
500 :
501 : // pitch control
502 0 : if (has_pitch_control()) {
503 0 : cap_flags |= GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
504 : GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW |
505 : GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK;
506 : }
507 :
508 : // yaw control
509 0 : if (has_pan_control()) {
510 0 : cap_flags |= GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
511 : GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW |
512 : GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK;
513 : }
514 :
515 0 : return cap_flags;
516 : }
517 :
518 : // send a GIMBAL_MANAGER_INFORMATION message to GCS
519 0 : void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
520 : {
521 0 : mavlink_msg_gimbal_manager_information_send(chan,
522 : AP_HAL::millis(), // autopilot system time
523 0 : get_gimbal_manager_capability_flags(), // bitmap of gimbal manager capability flags
524 0 : _instance + 1, // gimbal device id
525 0 : radians(_params.roll_angle_min), // roll_min in radians
526 0 : radians(_params.roll_angle_max), // roll_max in radians
527 0 : radians(_params.pitch_angle_min), // pitch_min in radians
528 0 : radians(_params.pitch_angle_max), // pitch_max in radians
529 0 : radians(_params.yaw_angle_min), // yaw_min in radians
530 0 : radians(_params.yaw_angle_max)); // yaw_max in radians
531 0 : }
532 :
533 : // send a GIMBAL_MANAGER_STATUS message to GCS
534 25 : void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan)
535 : {
536 25 : uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
537 :
538 25 : if (_yaw_lock) {
539 0 : flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK;
540 : }
541 :
542 25 : mavlink_msg_gimbal_manager_status_send(chan,
543 : AP_HAL::millis(), // autopilot system time
544 : flags, // bitmap of gimbal manager flags
545 25 : _instance + 1, // gimbal device id
546 25 : mavlink_control_id.sysid, // primary control system id
547 25 : mavlink_control_id.compid, // primary control component id
548 : 0, // secondary control system id
549 : 0); // secondary control component id
550 25 : }
551 :
552 : // handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success
553 36 : MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_command_int_t &packet)
554 : {
555 36 : const MAV_MOUNT_MODE new_mode = (MAV_MOUNT_MODE)packet.z;
556 :
557 : // interpret message fields based on mode
558 36 : switch (new_mode) {
559 24 : case MAV_MOUNT_MODE_RETRACT:
560 : case MAV_MOUNT_MODE_NEUTRAL:
561 : case MAV_MOUNT_MODE_RC_TARGETING:
562 : case MAV_MOUNT_MODE_HOME_LOCATION:
563 : // simply set mode
564 24 : set_mode(new_mode);
565 24 : return MAV_RESULT_ACCEPTED;
566 :
567 8 : case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
568 : // set target angles (in degrees) from mavlink message
569 8 : const float pitch_deg = packet.param1; // param1: pitch (earth-frame, degrees)
570 8 : const float roll_deg = packet.param2; // param2: roll (earth-frame, degrees)
571 8 : const float yaw_deg = packet.param3; // param3: yaw (body-frame, degrees)
572 :
573 : // warn if angles are invalid to catch angles sent in centi-degrees
574 8 : if ((fabsf(pitch_deg) > 90) || (fabsf(roll_deg) > 180) || (fabsf(yaw_deg) > 360)) {
575 0 : send_warning_to_GCS("invalid angle targets");
576 0 : return MAV_RESULT_FAILED;
577 : }
578 :
579 8 : set_angle_target(packet.param2, packet.param1, packet.param3, false);
580 8 : return MAV_RESULT_ACCEPTED;
581 : }
582 :
583 2 : case MAV_MOUNT_MODE_GPS_POINT: {
584 : // set lat, lon, alt position targets from mavlink message
585 :
586 : // warn if lat, lon appear to be in param1,2 instead of param x,y as this indicates
587 : // sender is relying on a bug in AP-4.2's (and earlier) handling of MAV_CMD_DO_MOUNT_CONTROL
588 2 : if (!is_zero(packet.param1) && !is_zero(packet.param2) && packet.x == 0 && packet.y == 0) {
589 0 : send_warning_to_GCS("GPS_POINT target invalid");
590 0 : return MAV_RESULT_FAILED;
591 : }
592 :
593 : // param4: altitude in meters
594 : // x: latitude in degrees * 1E7
595 : // y: longitude in degrees * 1E7
596 : const Location target_location {
597 2 : packet.x, // latitude in degrees * 1E7
598 2 : packet.y, // longitude in degrees * 1E7
599 2 : (int32_t)packet.param4 * 100, // alt converted from meters to cm
600 : Location::AltFrame::ABOVE_HOME
601 2 : };
602 2 : set_roi_target(target_location);
603 2 : return MAV_RESULT_ACCEPTED;
604 : }
605 :
606 2 : default:
607 : // invalid mode
608 2 : return MAV_RESULT_FAILED;
609 : }
610 : }
611 :
612 : // handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
613 : // requires original message in order to extract caller's sysid and compid
614 8 : MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
615 : {
616 : // sanity check param1 and param2 values
617 8 : if ((packet.param1 < -3) || (packet.param1 > UINT8_MAX) || (packet.param2 < -3) || (packet.param2 > UINT8_MAX)) {
618 0 : return MAV_RESULT_FAILED;
619 : }
620 :
621 : // backup the current values so we can detect a change
622 8 : mavlink_control_id_t prev_control_id = mavlink_control_id;
623 :
624 : // convert negative packet1 and packet2 values
625 8 : int16_t new_sysid = packet.param1;
626 8 : switch (new_sysid) {
627 2 : case -1:
628 : // leave unchanged
629 2 : break;
630 0 : case -2:
631 : // set itself in control
632 0 : mavlink_control_id.sysid = msg.sysid;
633 0 : mavlink_control_id.compid = msg.compid;
634 0 : break;
635 2 : case -3:
636 : // remove control if currently in control
637 2 : if ((mavlink_control_id.sysid == msg.sysid) && (mavlink_control_id.compid == msg.compid)) {
638 2 : mavlink_control_id.sysid = 0;
639 2 : mavlink_control_id.compid = 0;
640 : }
641 2 : break;
642 4 : default:
643 4 : mavlink_control_id.sysid = packet.param1;
644 4 : mavlink_control_id.compid = packet.param2;
645 4 : break;
646 : }
647 :
648 : // send gimbal_manager_status if control has changed
649 8 : if (prev_control_id != mavlink_control_id) {
650 6 : GCS_SEND_MESSAGE(MSG_GIMBAL_MANAGER_STATUS);
651 : }
652 :
653 8 : return MAV_RESULT_ACCEPTED;
654 : }
655 :
656 : // handle a GLOBAL_POSITION_INT message
657 12 : bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet)
658 : {
659 12 : if (_target_sysid != msg_sysid) {
660 0 : return false;
661 : }
662 :
663 12 : _target_sysid_location.lat = packet.lat;
664 12 : _target_sysid_location.lng = packet.lon;
665 : // global_position_int.alt is *UP*, so is location.
666 12 : _target_sysid_location.set_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE);
667 12 : _target_sysid_update_ms = AP_HAL::millis();
668 :
669 12 : return true;
670 : }
671 :
672 : #if HAL_LOGGING_ENABLED
673 : // write mount log packet
674 77800 : void AP_Mount_Backend::write_log(uint64_t timestamp_us)
675 : {
676 : // return immediately if no yaw estimate
677 77800 : float ahrs_yaw = AP::ahrs().get_yaw_rad();
678 77800 : if (isnan(ahrs_yaw)) {
679 0 : return;
680 : }
681 :
682 77800 : const auto nanf = AP_Logger::quiet_nanf();
683 :
684 : // get_attitude_quaternion and convert to Euler angles
685 77800 : float roll = nanf;
686 77800 : float pitch = nanf;
687 77800 : float yaw_bf = nanf;
688 77800 : float yaw_ef = nanf;
689 77800 : if (_frontend.get_attitude_euler(_instance, roll, pitch, yaw_bf)) {
690 77573 : yaw_ef = wrap_180(yaw_bf + degrees(ahrs_yaw));
691 : }
692 :
693 : // get mount's target (desired) angles and convert yaw to earth frame
694 77800 : float target_roll = nanf;
695 77800 : float target_pitch = nanf;
696 77800 : float target_yaw = nanf;
697 77800 : bool target_yaw_is_ef = false;
698 77800 : if (mnt_target.target_type == MountTargetType::ANGLE) {
699 19018 : target_roll = degrees(mnt_target.angle_rad.roll);
700 19018 : target_pitch = degrees(mnt_target.angle_rad.pitch);
701 19018 : target_yaw = degrees(mnt_target.angle_rad.yaw);
702 19018 : target_yaw_is_ef = mnt_target.angle_rad.yaw_is_ef;
703 : }
704 :
705 : // get rangefinder distance
706 77800 : float rangefinder_dist = nanf;
707 77800 : IGNORE_RETURN(get_rangefinder_distance(rangefinder_dist));
708 :
709 77800 : const struct log_Mount pkt {
710 : LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(LOG_MOUNT_MSG)),
711 77800 : time_us : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(),
712 77800 : instance : _instance,
713 77800 : mode : static_cast<uint8_t>(get_mode()),
714 : desired_roll : target_roll,
715 : actual_roll : roll,
716 : desired_pitch : target_pitch,
717 : actual_pitch : pitch,
718 77800 : desired_yaw_bf: target_yaw_is_ef ? nanf : target_yaw,
719 : actual_yaw_bf : yaw_bf,
720 77800 : desired_yaw_ef: target_yaw_is_ef ? target_yaw : nanf,
721 : actual_yaw_ef : yaw_ef,
722 : rangefinder_dist : rangefinder_dist,
723 155600 : };
724 77800 : AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
725 : }
726 : #endif
727 :
728 : #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
729 : // get poi information. Returns true on success and fills in gimbal attitude, location and poi location
730 2 : bool AP_Mount_Backend::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc)
731 : {
732 2 : WITH_SEMAPHORE(poi_calculation.sem);
733 :
734 : // record time of request
735 2 : const uint32_t now_ms = AP_HAL::millis();
736 2 : poi_calculation.poi_request_ms = now_ms;
737 :
738 : // check if poi calculated recently
739 2 : if (now_ms - poi_calculation.poi_update_ms > AP_MOUNT_POI_RESULT_TIMEOUT_MS) {
740 1 : return false;
741 : }
742 :
743 : // check attitude is valid
744 1 : if (poi_calculation.att_quat.is_nan()) {
745 0 : return false;
746 : }
747 :
748 1 : quat = poi_calculation.att_quat;
749 1 : loc = poi_calculation.loc;
750 1 : poi_loc = poi_calculation.poi_loc;
751 1 : return true;
752 2 : }
753 :
754 : // calculate the Location that the gimbal is pointing at
755 81737 : void AP_Mount_Backend::calculate_poi()
756 : {
757 : while (true) {
758 : // run this loop at 10hz
759 81737 : hal.scheduler->delay(100);
760 :
761 : // calculate poi if requested within last 30 seconds
762 : {
763 81625 : WITH_SEMAPHORE(poi_calculation.sem);
764 82924 : if ((poi_calculation.poi_request_ms == 0) ||
765 1299 : (AP_HAL::millis() - poi_calculation.poi_request_ms > AP_MOUNT_POI_REQUEST_TIMEOUT_MS)) {
766 81326 : continue;
767 : }
768 81625 : }
769 :
770 : // get the current location of vehicle
771 299 : const AP_AHRS &ahrs = AP::ahrs();
772 299 : Location curr_loc;
773 299 : if (!ahrs.get_location(curr_loc)) {
774 0 : continue;
775 : }
776 :
777 : // change vehicle alt to AMSL
778 299 : curr_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
779 :
780 : // project forward from vehicle looking for terrain
781 : // start testing at vehicle's location
782 299 : Location test_loc = curr_loc;
783 299 : Location prev_test_loc = curr_loc;
784 :
785 : // get terrain altitude (AMSL) at test_loc
786 299 : auto terrain = AP_Terrain::get_singleton();
787 : float terrain_amsl_m;
788 299 : if ((terrain == nullptr) || !terrain->height_amsl(test_loc, terrain_amsl_m, true)) {
789 0 : continue;
790 : }
791 :
792 : // retrieve gimbal attitude
793 299 : Quaternion quat;
794 299 : if (!get_attitude_quaternion(quat)) {
795 : // gimbal attitude unavailable
796 0 : continue;
797 : }
798 :
799 : // iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
800 299 : const float dist_increment_m = MAX(terrain->get_grid_spacing(), 10);
801 299 : const float mount_pitch_deg = degrees(quat.get_euler_pitch());
802 299 : const float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + ahrs.get_yaw_deg());
803 299 : float total_dist_m = 0;
804 299 : bool get_terrain_alt_success = true;
805 299 : float prev_terrain_amsl_m = terrain_amsl_m;
806 658 : while (total_dist_m < AP_MOUNT_POI_DIST_M_MAX && (test_loc.alt * 0.01) > terrain_amsl_m) {
807 359 : total_dist_m += dist_increment_m;
808 :
809 : // backup previous test location and terrain amsl
810 359 : prev_test_loc = test_loc;
811 359 : prev_terrain_amsl_m = terrain_amsl_m;
812 :
813 : // move test location forward
814 359 : test_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m);
815 :
816 : // get terrain's alt-above-sea-level (at test_loc)
817 : // fail if terrain alt cannot be retrieved
818 359 : if (!terrain->height_amsl(test_loc, terrain_amsl_m, true) || std::isnan(terrain_amsl_m)) {
819 0 : get_terrain_alt_success = false;
820 0 : continue;
821 : }
822 : }
823 :
824 : // if a fail occurred above when getting terrain alt then restart calculations from the beginning
825 299 : if (!get_terrain_alt_success) {
826 0 : continue;
827 : }
828 :
829 299 : if (total_dist_m >= AP_MOUNT_POI_DIST_M_MAX) {
830 : // unable to find terrain within dist_max
831 0 : continue;
832 : }
833 :
834 : // test location has dropped below terrain
835 : // interpolate along line between prev_test_loc and test_loc
836 299 : float dist_interp_m = linear_interpolate(0, dist_increment_m, 0, prev_test_loc.alt * 0.01 - prev_terrain_amsl_m, test_loc.alt * 0.01 - terrain_amsl_m);
837 : {
838 299 : WITH_SEMAPHORE(poi_calculation.sem);
839 299 : poi_calculation.poi_loc = prev_test_loc;
840 299 : poi_calculation.poi_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m);
841 299 : poi_calculation.att_quat = {quat[0], quat[1], quat[2], quat[3]};
842 299 : poi_calculation.loc = curr_loc;
843 299 : poi_calculation.poi_update_ms = AP_HAL::millis();
844 299 : }
845 81625 : }
846 : }
847 : #endif // AP_MOUNT_POI_TO_LATLONALT_ENABLED
848 :
849 : #if AP_MOUNT_POI_LOCK_ENABLED
850 : // calculate location gimbal is pointing, at HOME altitude. Used if Terrain is not avaialble
851 2 : bool AP_Mount_Backend::calculate_poi_at_home_alt(Location &target_location)
852 : {
853 2 : AP_AHRS &ahrs = AP::ahrs();
854 :
855 : // current location
856 2 : Location cur_loc;
857 2 : if (!ahrs.get_location(cur_loc)) {
858 0 : return false;
859 : }
860 :
861 : // home location/alt
862 2 : const Location &home = ahrs.get_home();
863 2 : const float cur_alt_m = cur_loc.alt * 0.01f; // cm -> m
864 2 : const float home_alt_m = home.alt * 0.01f; // cm -> m
865 :
866 : // Plane at HOME altitude in local NED (origin at vehicle):
867 : // down_of_home_plane = cur_alt - home_alt (NED down positive)
868 : // Above home => target_down_m > 0 (home plane is below us)
869 : // Below home => target_down_m < 0 (home plane is above us)
870 2 : const float target_down_m = (cur_alt_m - home_alt_m);
871 :
872 : // mount attitude quaternion
873 2 : Quaternion quat;
874 2 : if (!get_attitude_quaternion(quat)) {
875 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Failure to mount angles");
876 0 : return false;
877 : }
878 :
879 : // Extract mount euler from quat (AP convention: quat yaw is body-frame; add vehicle yaw for earth-frame yaw)
880 : float m_roll_rad;
881 : float m_pitch_rad;
882 : float m_yaw_body_rad;
883 2 : quat.to_euler(m_roll_rad, m_pitch_rad, m_yaw_body_rad);
884 :
885 2 : const float body_yaw_earth_rad = ahrs.get_yaw_rad();
886 2 : const float m_yaw_earth_rad = wrap_PI(m_yaw_body_rad + body_yaw_earth_rad);
887 :
888 : // LOS in earth NED directly from yaw_earth + pitch (avoids quaternion frame ambiguity)
889 : // NED: x=north, y=east, z=down
890 2 : Vector3f los_ned;
891 2 : const float cp = cosf(m_pitch_rad);
892 2 : const float sp = sinf(m_pitch_rad);
893 :
894 2 : los_ned.x = cp * cosf(m_yaw_earth_rad);
895 2 : los_ned.y = cp * sinf(m_yaw_earth_rad);
896 2 : los_ned.z = -sp; // negative pitch (down) => positive z (down)
897 :
898 : //just a safety check, should NEVER occur
899 2 : if (los_ned.length() < 1.0e-6f) {
900 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: return on bad los");
901 0 : return false;
902 : }
903 2 : los_ned.normalize();
904 :
905 : // Policy : if below home alt, you can be looking up, but not if above home alt
906 : // Require LOS to be at least MIN_DOWN_DEG below the horizon if above home alt.
907 : // This eliminates looking-up and near-parallel cases without needing a separate los.z ~= 0 check.
908 2 : const float MIN_DOWN_DEG = 1.0f; // tune
909 2 : const float min_los_z = sinf(radians(MIN_DOWN_DEG)); // ~= 0.01745
910 2 : if (los_ned.z < min_los_z && target_down_m > 0.0f) {
911 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
912 : "POI: Mount pitch too elevated (los.z=%.4f need >= %.4f; pitch=%.2f deg)",
913 : los_ned.z, min_los_z, degrees(m_pitch_rad));
914 0 : return false;
915 : }
916 :
917 : // Use real intersection if above home; mirror if below home:
918 : // Mirror rule: if we are below home by |target_down|, pretend we are above home by the same amount
919 : // for the purpose of selecting a forward point in the home-alt plane.
920 2 : const bool used_mirror = (target_down_m < 0.0f);
921 2 : const float effective_down_m = used_mirror ? -target_down_m : target_down_m;
922 :
923 : // guaranteed positive with min_los_z check
924 2 : const float t_m = effective_down_m / los_ned.z;
925 :
926 2 : const float north_m = los_ned.x * t_m;
927 2 : const float east_m = los_ned.y * t_m;
928 :
929 : // Horizontal distance from current location (meters)
930 2 : const float horiz_dist_m = sqrtf(north_m * north_m + east_m * east_m);
931 :
932 : // Reject targets beyond 5 km (also naturally rejects near-horizon geometry that slips through)
933 2 : if (horiz_dist_m > 5000.0f) {
934 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
935 : "POI: distance > 5km: %.1f m", horiz_dist_m);
936 0 : return false;
937 : }
938 :
939 : // Build target location at intersection point of home alt plane
940 2 : target_location = cur_loc;
941 2 : target_location.offset(north_m, east_m);
942 2 : target_location.alt = home.alt;
943 2 : return true;
944 : }
945 : #endif // AP_MOUNT_POI_LOCK_ENABLED
946 :
947 : // change to RC_TARGETING mode if rc inputs have changed by more than the dead zone
948 : // should be called on every update
949 381755 : void AP_Mount_Backend::set_rctargeting_on_rcinput_change()
950 : {
951 : // exit immediately if no RC input
952 381755 : if (!rc().has_valid_input()) {
953 802 : return;
954 : }
955 :
956 380953 : const RC_Channel *roll_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_ROLL : RC_Channel::AUX_FUNC::MOUNT2_ROLL);
957 380953 : const RC_Channel *pitch_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_PITCH : RC_Channel::AUX_FUNC::MOUNT2_PITCH);
958 380953 : const RC_Channel *yaw_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_YAW : RC_Channel::AUX_FUNC::MOUNT2_YAW);
959 :
960 : // get rc input
961 380953 : const int16_t roll_in = (roll_ch == nullptr) ? 0 : roll_ch->get_radio_in();
962 380953 : const int16_t pitch_in = (pitch_ch == nullptr) ? 0 : pitch_ch->get_radio_in();
963 380953 : const int16_t yaw_in = (yaw_ch == nullptr) ? 0 : yaw_ch->get_radio_in();
964 :
965 380953 : if (!last_rc_input.initialised) {
966 : // The first time through, initial RC inputs should be set, but not used
967 110 : last_rc_input.initialised = true;
968 110 : last_rc_input.roll_in = roll_in;
969 110 : last_rc_input.pitch_in = pitch_in;
970 110 : last_rc_input.yaw_in = yaw_in;
971 : }
972 : // if not in RC_TARGETING or RETRACT modes then check for RC change
973 380953 : if (get_mode() != MAV_MOUNT_MODE_RC_TARGETING && get_mode() != MAV_MOUNT_MODE_RETRACT) {
974 : // get dead zones
975 42475 : const int16_t roll_dz = (roll_ch == nullptr) ? 10 : MAX(roll_ch->get_dead_zone(), 10);
976 42475 : const int16_t pitch_dz = (pitch_ch == nullptr) ? 10 : MAX(pitch_ch->get_dead_zone(), 10);
977 42475 : const int16_t yaw_dz = (yaw_ch == nullptr) ? 10 : MAX(yaw_ch->get_dead_zone(), 10);
978 :
979 : // check if RC input has changed by more than the dead zone
980 42475 : if ((abs(last_rc_input.roll_in - roll_in) > roll_dz) ||
981 42475 : (abs(last_rc_input.pitch_in - pitch_in) > pitch_dz) ||
982 42465 : (abs(last_rc_input.yaw_in - yaw_in) > yaw_dz)) {
983 10 : set_mode(MAV_MOUNT_MODE_RC_TARGETING);
984 : }
985 : }
986 :
987 : // if NOW in RC_TARGETING or RETRACT mode then store last RC input (mode might have changed)
988 380953 : if (get_mode() == MAV_MOUNT_MODE_RC_TARGETING || get_mode() == MAV_MOUNT_MODE_RETRACT) {
989 338488 : last_rc_input.roll_in = roll_in;
990 338488 : last_rc_input.pitch_in = pitch_in;
991 338488 : last_rc_input.yaw_in = yaw_in;
992 : }
993 : }
994 :
995 : // get pilot input (in the range -1 to +1) received through RC
996 335491 : void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const
997 : {
998 335491 : const RC_Channel *roll_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_ROLL : RC_Channel::AUX_FUNC::MOUNT2_ROLL);
999 335491 : const RC_Channel *pitch_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_PITCH : RC_Channel::AUX_FUNC::MOUNT2_PITCH);
1000 335491 : const RC_Channel *yaw_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_YAW : RC_Channel::AUX_FUNC::MOUNT2_YAW);
1001 :
1002 335491 : roll_in = 0;
1003 335491 : if ((roll_ch != nullptr) && (roll_ch->get_radio_in() > 0)) {
1004 4483 : roll_in = roll_ch->norm_input_dz();
1005 : }
1006 :
1007 335491 : pitch_in = 0;
1008 335491 : if ((pitch_ch != nullptr) && (pitch_ch->get_radio_in() > 0)) {
1009 316203 : pitch_in = pitch_ch->norm_input_dz();
1010 : }
1011 :
1012 335491 : yaw_in = 0;
1013 335491 : if ((yaw_ch != nullptr) && (yaw_ch->get_radio_in() > 0)) {
1014 277671 : yaw_in = yaw_ch->norm_input_dz();
1015 : }
1016 335491 : }
1017 :
1018 : // get angle targets (in radians) to a Location
1019 : // returns true on success, false on failure
1020 17194 : bool AP_Mount_Backend::get_angle_target_to_location(const Location &loc, MountAngleTarget& angle_rad) const
1021 : {
1022 : // exit immediately if vehicle's location is unavailable
1023 17194 : Location current_loc;
1024 17194 : if (!AP::ahrs().get_location(current_loc)) {
1025 0 : return false;
1026 : }
1027 :
1028 : // exit immediate if location is invalid
1029 17194 : if (!loc.initialised()) {
1030 0 : return false;
1031 : }
1032 :
1033 17194 : const float GPS_vector_x = Location::diff_longitude(loc.lng, current_loc.lng)*cosf(radians((current_loc.lat + loc.lat) * 0.00000005f)) * 0.01113195f;
1034 17194 : const float GPS_vector_y = (loc.lat - current_loc.lat) * 0.01113195f;
1035 17194 : int32_t target_alt_cm = 0;
1036 17194 : if (!loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) {
1037 0 : return false;
1038 : }
1039 17194 : int32_t current_alt_cm = 0;
1040 17194 : if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, current_alt_cm)) {
1041 0 : return false;
1042 : }
1043 17194 : float GPS_vector_z = target_alt_cm - current_alt_cm;
1044 17194 : float target_distance = 100.0f*norm(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
1045 :
1046 : // calculate roll, pitch, yaw angles
1047 17194 : angle_rad.roll = 0;
1048 17194 : angle_rad.pitch = atan2f(GPS_vector_z, target_distance);
1049 17194 : angle_rad.yaw = atan2f(GPS_vector_x, GPS_vector_y);
1050 17194 : angle_rad.yaw_is_ef = true;
1051 17194 : angle_rad.pitch_is_ef = true;
1052 17194 : angle_rad.roll_is_ef = true;
1053 :
1054 17194 : return true;
1055 : }
1056 :
1057 : // get angle targets (in radians) to ROI location
1058 : // returns true on success, false on failure
1059 6582 : bool AP_Mount_Backend::get_angle_target_to_roi(MountAngleTarget& angle_rad) const
1060 : {
1061 6582 : if (!_roi_target.initialised()) {
1062 0 : return false;
1063 : }
1064 6582 : return get_angle_target_to_location(_roi_target, angle_rad);
1065 : }
1066 :
1067 : // return body-frame yaw angle from a mount target
1068 365487 : float AP_Mount_Backend::MountAngleTarget::get_bf_yaw() const
1069 : {
1070 365487 : if (yaw_is_ef) {
1071 : // convert to body-frame
1072 19947 : return wrap_PI(yaw - AP::ahrs().get_yaw_rad());
1073 : }
1074 :
1075 : // target is already body-frame
1076 345540 : return yaw;
1077 : }
1078 :
1079 : // return earth-frame yaw angle from a mount target
1080 0 : float AP_Mount_Backend::MountAngleTarget::get_ef_yaw() const
1081 : {
1082 0 : if (yaw_is_ef) {
1083 : // target is already earth-frame
1084 0 : return yaw;
1085 : }
1086 :
1087 : // convert to earth-frame
1088 0 : return wrap_PI(yaw + AP::ahrs().get_yaw_rad());
1089 : }
1090 :
1091 : // sets roll, pitch, yaw and yaw_is_ef
1092 18023 : void AP_Mount_Backend::MountAngleTarget::set(const Vector3f& rpy, bool yaw_is_ef_in)
1093 : {
1094 18023 : roll = rpy.x;
1095 18023 : pitch = rpy.y;
1096 18023 : yaw = rpy.z;
1097 18023 : yaw_is_ef = yaw_is_ef_in;
1098 18023 : }
1099 :
1100 : // update angle targets using a given rate target
1101 : // the resulting angle_rad yaw frame will match the rate_rad yaw frame
1102 : // assumes a 50hz update rate
1103 274956 : void AP_Mount_Backend::update_angle_target_from_rate(const MountRateTarget& rate_rad, MountAngleTarget& angle_rad) const
1104 : {
1105 : // update roll and pitch angles and apply limits
1106 274956 : angle_rad.roll = constrain_float(angle_rad.roll + rate_rad.roll * AP_MOUNT_UPDATE_DT, radians(_params.roll_angle_min), radians(_params.roll_angle_max));
1107 274956 : angle_rad.pitch = constrain_float(angle_rad.pitch + rate_rad.pitch * AP_MOUNT_UPDATE_DT, radians(_params.pitch_angle_min), radians(_params.pitch_angle_max));
1108 :
1109 : // ensure angle yaw frames matches rate yaw frame
1110 274956 : if (angle_rad.yaw_is_ef != rate_rad.yaw_is_ef) {
1111 0 : if (rate_rad.yaw_is_ef) {
1112 0 : angle_rad.yaw = angle_rad.get_ef_yaw();
1113 : } else {
1114 0 : angle_rad.yaw = angle_rad.get_bf_yaw();
1115 : }
1116 0 : angle_rad.yaw_is_ef = rate_rad.yaw_is_ef;
1117 : }
1118 :
1119 : // update yaw angle target
1120 274956 : angle_rad.yaw = angle_rad.yaw + rate_rad.yaw * AP_MOUNT_UPDATE_DT;
1121 274956 : if (angle_rad.yaw_is_ef) {
1122 : // if earth-frame yaw wraps between += 180 degrees
1123 0 : angle_rad.yaw = wrap_PI(angle_rad.yaw);
1124 : } else {
1125 : // if body-frame constrain yaw to body-frame limits
1126 274956 : angle_rad.yaw = constrain_float(angle_rad.yaw, radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
1127 : }
1128 274956 : }
1129 :
1130 : // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
1131 65282 : uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
1132 : {
1133 : // get yaw lock state by mode
1134 65282 : bool yaw_lock_state = false;
1135 65282 : switch (_mode) {
1136 3518 : case MAV_MOUNT_MODE_RETRACT:
1137 : case MAV_MOUNT_MODE_NEUTRAL:
1138 : case MAV_MOUNT_MODE_WPNEXT_OFFSET:
1139 : // these modes always use body-frame yaw (aka follow)
1140 3518 : yaw_lock_state = false;
1141 3518 : break;
1142 431 : case MAV_MOUNT_MODE_MAVLINK_TARGETING:
1143 431 : switch (mnt_target.target_type) {
1144 12 : case MountTargetType::RATE:
1145 12 : yaw_lock_state = mnt_target.rate_rads.yaw_is_ef;
1146 12 : break;
1147 419 : case MountTargetType::ANGLE:
1148 419 : yaw_lock_state = mnt_target.angle_rad.yaw_is_ef;
1149 419 : break;
1150 0 : case MountTargetType::RETRACTED:
1151 : case MountTargetType::NEUTRAL:
1152 0 : yaw_lock_state = false; // not locked onto the scenery
1153 0 : break;
1154 0 : case MountTargetType::LOCATION:
1155 0 : yaw_lock_state = true;
1156 0 : break;
1157 : }
1158 431 : break;
1159 59251 : case MAV_MOUNT_MODE_RC_TARGETING:
1160 59251 : yaw_lock_state = _yaw_lock;
1161 59251 : break;
1162 2082 : case MAV_MOUNT_MODE_GPS_POINT:
1163 : case MAV_MOUNT_MODE_SYSID_TARGET:
1164 : case MAV_MOUNT_MODE_HOME_LOCATION:
1165 : // these modes always use earth-frame yaw (aka lock)
1166 2082 : yaw_lock_state = true;
1167 2082 : break;
1168 0 : case MAV_MOUNT_MODE_ENUM_END:
1169 : // unsupported
1170 0 : yaw_lock_state = false;
1171 0 : break;
1172 : }
1173 :
1174 130564 : const uint16_t flags = (mnt_target.target_type == MountTargetType::RETRACTED ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
1175 65282 : (mnt_target.target_type == MountTargetType::NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
1176 : GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
1177 65282 : GIMBAL_DEVICE_FLAGS_PITCH_LOCK| // pitch angle is always earth-frame, yaw_angle is always body-frame
1178 65282 : GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // yaw angle is always in vehicle-frame
1179 : (yaw_lock_state ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
1180 65282 : return flags;
1181 : }
1182 :
1183 : // get angle targets (in radians) to home location
1184 : // returns true on success, false on failure
1185 5597 : bool AP_Mount_Backend::get_angle_target_to_home(MountAngleTarget& angle_rad) const
1186 : {
1187 : // exit immediately if home is not set
1188 5597 : if (!AP::ahrs().home_is_set()) {
1189 0 : return false;
1190 : }
1191 5597 : return get_angle_target_to_location(AP::ahrs().get_home(), angle_rad);
1192 : }
1193 :
1194 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
1195 : // get angle targets (in radians) to next waypoint, with offsets previously supplied
1196 : // returns true on success, false on failure
1197 6657 : bool AP_Mount_Backend::get_angle_target_to_wpnext_offset(MountAngleTarget& angle_rad) const
1198 : {
1199 6657 : Location wp_loc;
1200 :
1201 6657 : if (!AP::vehicle()->get_wp_location(wp_loc)) {
1202 2633 : return false;
1203 : }
1204 :
1205 4024 : if (!get_angle_target_to_location(wp_loc, angle_rad)) {
1206 0 : return false;
1207 : }
1208 :
1209 : // now add the offsets, wrapping as required:
1210 4024 : angle_rad.roll += radians(_roi_wpnext_rpy[0]);
1211 4024 : angle_rad.pitch += radians(_roi_wpnext_rpy[1]);
1212 4024 : angle_rad.yaw += radians(_roi_wpnext_rpy[2]);
1213 4024 : angle_rad.roll = wrap_PI(angle_rad.roll);
1214 4024 : angle_rad.pitch = wrap_PI(angle_rad.pitch);
1215 4024 : if (angle_rad.yaw_is_ef) {
1216 4024 : angle_rad.yaw = wrap_2PI(angle_rad.yaw);
1217 : } else {
1218 0 : angle_rad.yaw = wrap_PI(angle_rad.yaw);
1219 : }
1220 :
1221 4024 : return true;
1222 : }
1223 : #endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
1224 :
1225 : // get angle targets (in radians) to a vehicle with sysid of _target_sysid
1226 : // returns true on success, false on failure
1227 4870 : bool AP_Mount_Backend::get_angle_target_to_sysid(MountAngleTarget& angle_rad) const
1228 : {
1229 : // exit immediately if sysid is not set or no location available
1230 4870 : if (!_target_sysid_location.initialised()) {
1231 121 : return false;
1232 : }
1233 4749 : if (!_target_sysid) {
1234 0 : return false;
1235 : }
1236 : // exit if we haven't heard from the target recently, to avoid snapping to a stale location
1237 4749 : if (AP_HAL::millis() - _target_sysid_update_ms > AP_MOUNT_SYSID_TIMEOUT_MS) {
1238 3758 : return false;
1239 : }
1240 991 : return get_angle_target_to_location(_target_sysid_location, angle_rad);
1241 : }
1242 :
1243 : // updates the mount target by calling the _update_mount_target and then adjusting to remove lean angles if roll and/or pitch angles locked:
1244 : // this effectively translates earth frame targets in those axes to body frame to allow using the RP lock Aux Func and FPV_LOCK mount option
1245 : // on stabilized gimbals that dont have that capability in their backends via mount commands
1246 371526 : void AP_Mount_Backend::update_mnt_target()
1247 : {
1248 371526 : _update_mnt_target(); //does most of the work below
1249 : // now adjust mnt target if the gimbal requires removing vehicle lean angles from target to obtain body frame roll and pitch locks
1250 371526 : if (apply_bf_roll_pitch_adjustments_in_rc_targeting()) {
1251 0 : adjust_mnt_target_if_RP_locked();
1252 : }
1253 371526 : }
1254 :
1255 : // method for the mount backends to update mnt_target based on
1256 : // the mount mode. Methods in here may be overridden by the derived
1257 : // class to customise behaviour
1258 371526 : void AP_Mount_Backend::_update_mnt_target()
1259 : {
1260 : // change to RC_TARGETING mode if RC input has changed
1261 371526 : set_rctargeting_on_rcinput_change();
1262 :
1263 371526 : switch (get_mode()) {
1264 2997 : case MAV_MOUNT_MODE_RETRACT:
1265 : // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
1266 2997 : mnt_target.target_type = MountTargetType::RETRACTED;
1267 2997 : return;
1268 :
1269 9479 : case MAV_MOUNT_MODE_NEUTRAL:
1270 : // move mount to a neutral position, typically pointing forward
1271 9479 : mnt_target.target_type = MountTargetType::NEUTRAL;
1272 9479 : return;
1273 :
1274 3696 : case MAV_MOUNT_MODE_MAVLINK_TARGETING:
1275 : // point to the angles given by a mavlink message
1276 : // mavlink targets are stored while handling the incoming message
1277 :
1278 : // set target rate to zero if we have not received rate
1279 : // command for a while
1280 3816 : if ((mnt_target.target_type == MountTargetType::RATE) &&
1281 120 : (AP_HAL::millis() - mnt_target.last_rate_request_ms > 3000)) {
1282 0 : mnt_target.rate_rads.roll = 0;
1283 0 : mnt_target.rate_rads.pitch = 0;
1284 0 : mnt_target.rate_rads.yaw = 0;
1285 : }
1286 3696 : return;
1287 :
1288 334197 : case MAV_MOUNT_MODE_RC_TARGETING:
1289 : // RC radio manual angle control, but with stabilization from the AHRS
1290 334197 : update_mnt_target_from_rc_target();
1291 334197 : return;
1292 :
1293 5982 : case MAV_MOUNT_MODE_GPS_POINT:
1294 : // point mount to a GPS point given by the mission planner
1295 5982 : mnt_target.target_type = MountTargetType::LOCATION;
1296 5982 : return;
1297 :
1298 5597 : case MAV_MOUNT_MODE_HOME_LOCATION:
1299 : // point mount to Home location
1300 5597 : if (get_angle_target_to_home(mnt_target.angle_rad)) {
1301 5597 : mnt_target.target_type = MountTargetType::ANGLE;
1302 : }
1303 5597 : return;
1304 :
1305 2921 : case MAV_MOUNT_MODE_SYSID_TARGET:
1306 : // point mount to another vehicle
1307 2921 : if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
1308 591 : mnt_target.target_type = MountTargetType::ANGLE;
1309 : }
1310 2921 : return;
1311 6657 : case MAV_MOUNT_MODE_WPNEXT_OFFSET:
1312 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
1313 6657 : if (get_angle_target_to_wpnext_offset(mnt_target.angle_rad)) {
1314 4024 : mnt_target.target_type = MountTargetType::ANGLE;
1315 : }
1316 : #endif
1317 6657 : return;
1318 0 : case MAV_MOUNT_MODE_ENUM_END:
1319 0 : break;
1320 : }
1321 : // we do not know this mode so raise internal error
1322 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1323 : }
1324 :
1325 371526 : void AP_Mount_Backend::send_target_to_gimbal()
1326 : {
1327 : // process any pending clear-roi-target
1328 : // it is assumed that we have already zeroed _roi_target
1329 371526 : if (clear_roi_pending && natively_supports(MountTargetType::LOCATION)) {
1330 0 : clear_roi_pending = false;
1331 0 : send_target_location(_roi_target);
1332 : }
1333 :
1334 : // the easy case, where the gimbal natively supports the MntTargetType:
1335 371526 : if (natively_supports(mnt_target.target_type)) {
1336 77992 : switch (mnt_target.target_type) {
1337 77992 : case MountTargetType::ANGLE:
1338 77992 : send_target_angles(mnt_target.angle_rad);
1339 77992 : return;
1340 0 : case MountTargetType::RATE:
1341 0 : send_target_rates(mnt_target.rate_rads);
1342 0 : return;
1343 0 : case MountTargetType::RETRACTED:
1344 0 : send_target_retracted();
1345 0 : return;
1346 0 : case MountTargetType::NEUTRAL:
1347 0 : send_target_neutral();
1348 0 : return;
1349 0 : case MountTargetType::LOCATION:
1350 0 : send_target_location(_roi_target);
1351 0 : return;
1352 : }
1353 0 : return; // should not reach this as all cases return
1354 : }
1355 :
1356 : // the more difficult case where we need to convert to something
1357 : // the gimbal understands:
1358 293534 : switch (mnt_target.target_type) {
1359 0 : case MountTargetType::ANGLE:
1360 : // we don't know how to convert ANGLE to anything else. Note
1361 : // that the Siyi backend *does* convert angles to rates, so we
1362 : // could potentially swipe code into here.
1363 0 : break;
1364 274956 : case MountTargetType::RATE:
1365 274956 : if (natively_supports(MountTargetType::ANGLE)) {
1366 : // we integrate the rates into the angle:
1367 274956 : update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
1368 274956 : send_target_angles(mnt_target.angle_rad);
1369 274956 : return;
1370 : }
1371 0 : break;
1372 2997 : case MountTargetType::RETRACTED:
1373 2997 : if (natively_supports(MountTargetType::ANGLE)) {
1374 : // just use the parameter values
1375 : // we update mnt_target for reporting purposes
1376 2997 : const Vector3f &angle_bf_target = _params.retract_angles.get();
1377 2997 : mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
1378 2997 : send_target_angles(mnt_target.angle_rad);
1379 2997 : return;
1380 : }
1381 0 : break;
1382 9479 : case MountTargetType::NEUTRAL:
1383 9479 : if (natively_supports(MountTargetType::ANGLE)) {
1384 : // just use the parameter values
1385 : // we update mnt_target for reporting purposes
1386 9479 : const Vector3f &angle_bf_target = _params.neutral_angles.get();
1387 9479 : mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
1388 9479 : send_target_angles(mnt_target.angle_rad);
1389 9479 : return;
1390 : }
1391 0 : break;
1392 6102 : case MountTargetType::LOCATION:
1393 6102 : if (natively_supports(MountTargetType::ANGLE)) {
1394 6102 : if (get_angle_target_to_roi(mnt_target.angle_rad)) {
1395 6102 : send_target_angles(mnt_target.angle_rad);
1396 : }
1397 6102 : return;
1398 : }
1399 0 : break;
1400 : }
1401 :
1402 0 : send_warning_to_GCS("Failed to convert mount target to command gimbal");
1403 : }
1404 :
1405 :
1406 : #if AP_SCRIPTING_ENABLED
1407 : // get target rate in deg/sec. returns true on success
1408 0 : bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
1409 : {
1410 0 : if (mnt_target.target_type == MountTargetType::RATE) {
1411 0 : roll_degs = degrees(mnt_target.rate_rads.roll);
1412 0 : pitch_degs = degrees(mnt_target.rate_rads.pitch);
1413 0 : yaw_degs = degrees(mnt_target.rate_rads.yaw);
1414 0 : yaw_is_earth_frame = mnt_target.rate_rads.yaw_is_ef;
1415 0 : return true;
1416 : }
1417 0 : return false;
1418 : }
1419 :
1420 : // get target angle in deg. returns true on success
1421 0 : bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
1422 : {
1423 0 : if (mnt_target.target_type == MountTargetType::ANGLE) {
1424 0 : roll_deg = degrees(mnt_target.angle_rad.roll);
1425 0 : pitch_deg = degrees(mnt_target.angle_rad.pitch);
1426 0 : yaw_deg = degrees(mnt_target.angle_rad.yaw);
1427 0 : yaw_is_earth_frame = mnt_target.angle_rad.yaw_is_ef;
1428 0 : return true;
1429 : }
1430 0 : return false;
1431 : }
1432 :
1433 : // return target location if available
1434 : // returns true if a target location is available and fills in target_loc argument
1435 67 : bool AP_Mount_Backend::get_location_target(Location &_target_loc)
1436 : {
1437 67 : switch (get_mode()) {
1438 1 : case MAV_MOUNT_MODE_GPS_POINT:
1439 1 : _target_loc = _roi_target;
1440 1 : return _roi_target.initialised();
1441 :
1442 29 : case MAV_MOUNT_MODE_HOME_LOCATION:
1443 29 : if (AP::ahrs().home_is_set()) {
1444 29 : _target_loc = AP::ahrs().get_home();
1445 29 : return true;
1446 : }
1447 0 : break;
1448 :
1449 1 : case MAV_MOUNT_MODE_SYSID_TARGET:
1450 1 : _target_loc = _target_sysid_location;
1451 1 : return _target_sysid_location.initialised();
1452 :
1453 36 : case MAV_MOUNT_MODE_WPNEXT_OFFSET:
1454 : case MAV_MOUNT_MODE_RETRACT:
1455 : case MAV_MOUNT_MODE_NEUTRAL:
1456 : case MAV_MOUNT_MODE_MAVLINK_TARGETING:
1457 : case MAV_MOUNT_MODE_RC_TARGETING:
1458 : case MAV_MOUNT_MODE_ENUM_END:
1459 36 : break;
1460 : }
1461 36 : return false;
1462 : }
1463 : #endif
1464 :
1465 : // sent warning to GCS. Warnings are throttled to at most once every 30 seconds
1466 0 : void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str)
1467 : {
1468 0 : uint32_t now_ms = AP_HAL::millis();
1469 0 : if (now_ms - _last_warning_ms < 30000) {
1470 0 : return;
1471 : }
1472 :
1473 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Mount: %s", warning_str);
1474 0 : _last_warning_ms = now_ms;
1475 : }
1476 :
1477 : #endif // HAL_MOUNT_ENABLED
|
