Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //==============================================================================
17 : // AP_Follow Library
18 : // Target Following Logic for ArduPilot Vehicles
19 : //==============================================================================
20 :
21 :
22 : //==============================================================================
23 : // Includes
24 : //==============================================================================
25 :
26 : #include "AP_Follow_config.h"
27 :
28 : #if AP_FOLLOW_ENABLED
29 :
30 : #include <AP_HAL/AP_HAL.h>
31 : #include "AP_Follow.h"
32 : #include <ctype.h>
33 : #include <stdio.h>
34 :
35 : #include <AP_AHRS/AP_AHRS.h>
36 : #include <AP_Logger/AP_Logger.h>
37 : #include <GCS_MAVLink/GCS.h>
38 : #include <AP_Vehicle/AP_Vehicle_Type.h>
39 : #include <AP_Scheduler/AP_Scheduler.h>
40 :
41 : extern const AP_HAL::HAL& hal;
42 :
43 :
44 : //==============================================================================
45 : // Constants and Definitions
46 : //==============================================================================
47 :
48 : #define AP_FOLLOW_TIMEOUT 3 // position estimate timeout in seconds
49 : #define AP_FOLLOW_ESTIMATE_TIMEOUT_MS 10000 // discard the target estimate if we have not heard from the target in 10 seconds
50 :
51 : #define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame
52 : #define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading
53 :
54 : #define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default
55 :
56 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
57 : #define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast<float>(Location::AltFrame::ABSOLUTE)
58 : #define AP_FOLLOW_DIST_MAX_DEFAULT 0 // zero = ignored
59 : #else
60 : #define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast<float>(Location::AltFrame::ABOVE_HOME)
61 : #define AP_FOLLOW_DIST_MAX_DEFAULT 100
62 : #endif
63 :
64 :
65 : //==============================================================================
66 : // Constructor
67 : //==============================================================================
68 :
69 : AP_Follow *AP_Follow::_singleton;
70 :
71 : // table of user settable parameters
72 : const AP_Param::GroupInfo AP_Follow::var_info[] = {
73 :
74 : // @Param: _ENABLE
75 : // @DisplayName: Follow enable/disable
76 : // @Description: Enabled/disable following a target
77 : // @Values: 0:Disabled,1:Enabled
78 : // @User: Standard
79 : AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_Follow, _enabled, 0, AP_PARAM_FLAG_ENABLE),
80 :
81 : // 2 is reserved for TYPE parameter
82 :
83 : // @Param: _SYSID
84 : // @DisplayName: Follow target's mavlink system id
85 : // @Description: Follow target's mavlink system id. Zero means no target has been selected and following is inactive.
86 : // @Range: 0 255
87 : // @User: Standard
88 : AP_GROUPINFO("_SYSID", 3, AP_Follow, _sysid, 0),
89 :
90 : // 4 is reserved for MARGIN parameter
91 :
92 : // @Param: _DIST_MAX
93 : // @DisplayName: Follow distance maximum
94 : // @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid. If zero there is no maximum.
95 : // @Units: m
96 : // @Range: 0 1000
97 : // @User: Standard
98 : AP_GROUPINFO("_DIST_MAX", 5, AP_Follow, _dist_max_m, AP_FOLLOW_DIST_MAX_DEFAULT),
99 :
100 : // @Param: _OFS_TYPE
101 : // @DisplayName: Follow offset type
102 : // @Description: Follow offset type
103 : // @Values: 0:North-East-Down, 1:Relative to lead vehicle heading
104 : // @User: Standard
105 : AP_GROUPINFO("_OFS_TYPE", 6, AP_Follow, _offset_type, AP_FOLLOW_OFFSET_TYPE_NED),
106 :
107 : // @Param: _OFS_X
108 : // @DisplayName: Follow offsets in meters north/forward
109 : // @Description: Follow offsets in meters north/forward. If positive, this vehicle fly ahead or north of lead vehicle. Depends on FOLL_OFS_TYPE
110 : // @Range: -100 100
111 : // @Units: m
112 : // @Increment: 1
113 : // @User: Standard
114 :
115 : // @Param: _OFS_Y
116 : // @DisplayName: Follow offsets in meters east/right
117 : // @Description: Follow offsets in meters east/right. If positive, this vehicle will fly to the right or east of lead vehicle. Depends on FOLL_OFS_TYPE
118 : // @Range: -100 100
119 : // @Units: m
120 : // @Increment: 1
121 : // @User: Standard
122 :
123 : // @Param: _OFS_Z
124 : // @DisplayName: Follow offsets in meters down
125 : // @Description: Follow offsets in meters down. If positive, this vehicle will fly below the lead vehicle
126 : // @Range: -100 100
127 : // @Units: m
128 : // @Increment: 1
129 : // @User: Standard
130 : AP_GROUPINFO("_OFS", 7, AP_Follow, _offset_m, 0),
131 :
132 : #if !(APM_BUILD_TYPE(APM_BUILD_Rover))
133 : // @Param: _YAW_BEHAVE
134 : // @DisplayName: Follow yaw behaviour
135 : // @Description: Follow yaw behaviour
136 : // @Values: 0:None,1:Face Lead Vehicle,2:Same as Lead vehicle,3:Direction of Flight
137 : // @User: Standard
138 : AP_GROUPINFO("_YAW_BEHAVE", 8, AP_Follow, _yaw_behave, 1),
139 : #endif
140 :
141 : // @Param: _POS_P
142 : // @DisplayName: Follow position error P gain
143 : // @Description: Follow position error P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
144 : // @Range: 0.01 1.00
145 : // @Increment: 0.01
146 : // @User: Standard
147 : AP_SUBGROUPINFO(_p_pos, "_POS_", 9, AP_Follow, AC_P),
148 :
149 : #if !(APM_BUILD_TYPE(APM_BUILD_Rover))
150 : // @Param: _ALT_TYPE
151 : // @DisplayName: Follow altitude type
152 : // @Description: Follow altitude type
153 : // @Values: 0:absolute, 1:relative, 3:terrain
154 : // @User: Standard
155 : AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
156 : #endif
157 :
158 : // @Param: _OPTIONS
159 : // @DisplayName: Follow options
160 : // @Description: Follow options bitmask
161 : // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
162 : // @User: Standard
163 : AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
164 :
165 : // @Param: _ACCEL_NE
166 : // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
167 : // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in velocity
168 : // @Range: 0 10
169 : // @Units: m/s/s
170 : // @User: Advanced
171 : AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 5.0),
172 :
173 : // @Param: _JERK_NE
174 : // @DisplayName: Jerk limit for the horizontal kinematic input shaping
175 : // @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in acceleration
176 : // @Range: 0 20
177 : // @Units: m/s/s/s
178 : // @User: Advanced
179 : AP_GROUPINFO("_JERK_NE", 13, AP_Follow, _jerk_max_ne_msss, 10.0),
180 :
181 : // @Param: _ACCEL_D
182 : // @DisplayName: Acceleration limit for the vertical kinematic input shaping
183 : // @Description: Acceleration limit of the vertical kinematic path generation used to determine how quickly the estimate varies in velocity
184 : // @Range: 0 10
185 : // @Units: m/s/s
186 : // @User: Advanced
187 : AP_GROUPINFO("_ACCEL_D", 14, AP_Follow, _accel_max_d_mss, 5.0),
188 :
189 : // @Param: _JERK_D
190 : // @DisplayName: Jerk limit for the vertical kinematic input shaping
191 : // @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the estimate varies in acceleration
192 : // @Range: 0 20
193 : // @Units: m/s/s/s
194 : // @User: Advanced
195 : AP_GROUPINFO("_JERK_D", 15, AP_Follow, _jerk_max_d_msss, 10.0),
196 :
197 : // @Param: _ACCEL_H
198 : // @DisplayName: Angular acceleration limit for the heading kinematic input shaping
199 : // @Description: Angular acceleration limit of the heading kinematic path generation used to determine how quickly the estimate varies in angular velocity
200 : // @Range: 0 720
201 : // @Units: deg/s/s
202 : // @User: Advanced
203 : AP_GROUPINFO("_ACCEL_H", 16, AP_Follow, _accel_max_h_degss, 360.0),
204 :
205 : // @Param: _JERK_H
206 : // @DisplayName: Angular jerk limit for the heading kinematic input shaping
207 : // @Description: Angular jerk limit of the heading kinematic path generation used to determine how quickly the estimate varies in angular acceleration
208 : // @Range: 0 1440
209 : // @Units: deg/s/s/s
210 : // @User: Advanced
211 : AP_GROUPINFO("_JERK_H", 17, AP_Follow, _jerk_max_h_degsss, 720.0),
212 :
213 : // @Param: _TIMEOUT
214 : // @DisplayName: Follow timeout
215 : // @Description: Follow position update from lead - timeout after x seconds
216 : // @User: Standard
217 : // @Units: s
218 : AP_GROUPINFO("_TIMEOUT", 18, AP_Follow, _timeout, AP_FOLLOW_TIMEOUT),
219 :
220 : AP_GROUPEND
221 : };
222 :
223 : // Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
224 1978 : AP_Follow::AP_Follow() :
225 1978 : _p_pos(AP_FOLLOW_POS_P_DEFAULT)
226 : {
227 1978 : _singleton = this;
228 1978 : AP_Param::setup_object_defaults(this, var_info);
229 1978 : }
230 :
231 :
232 : //==============================================================================
233 : // Target Estimation Update Functions
234 : //==============================================================================
235 :
236 : // Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
237 39419043 : void AP_Follow::update_estimates()
238 : {
239 39419043 : WITH_SEMAPHORE(_follow_sem);
240 :
241 : // if we do not hold fresh data from the configured target, invalidate the estimate.
242 : // this cannot gate on have_target() because that tests _estimate_valid, which is
243 : // cleared below, and the estimate could then never be rebuilt
244 39419043 : if (!have_target_data()) {
245 39310420 : clear_dist_and_bearing_to_target();
246 39310420 : _estimate_valid = false;
247 39310420 : return;
248 : }
249 :
250 : // check for timeout
251 108623 : if ((_last_location_update_ms == 0) || ((AP_HAL::millis() - _last_location_update_ms) > (uint32_t)(_timeout * 1000.0f))) {
252 51210 : clear_dist_and_bearing_to_target();
253 51210 : _estimate_valid = false;
254 51210 : return;
255 : }
256 :
257 57413 : const uint32_t now = AP_HAL::millis();
258 :
259 : // calculate time since last location update in seconds
260 57413 : const float dt = (now - _last_location_update_ms) * 0.001f;
261 :
262 : // project target's position and velocity forward using simple kinematics
263 57413 : Vector3f delta_pos_m = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
264 57413 : Vector3f delta_vel_ms = _target_accel_ned_mss * dt;
265 57413 : float delta_heading_rad = radians(_target_heading_rate_degs) * dt;
266 :
267 : // calculate time since last estimation update
268 57413 : const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
269 :
270 114826 : const bool valid_kinematic_params = (_accel_max_ne_mss > 0.0f) && (_jerk_max_ne_msss > 0.0f) &&
271 57413 : (_accel_max_d_mss > 0.0f) && (_jerk_max_d_msss > 0.0f) &&
272 114826 : (_accel_max_h_degss > 0.0f) && (_jerk_max_h_degsss > 0.0f);
273 :
274 57413 : if (_estimate_valid && valid_kinematic_params) {
275 : // update X/Y position, velocity, acceleration with shaping
276 57407 : update_pos_vel_accel_xy(_estimate_pos_ned_m.xy(), _estimate_vel_ned_ms.xy(), _estimate_accel_ned_mss.xy(), e_dt, Vector2f(), Vector2f(), Vector2f());
277 :
278 : // integrate the Z axis estimate forward by e_dt
279 57407 : update_pos_vel_accel(_estimate_pos_ned_m.z, _estimate_vel_ned_ms.z, _estimate_accel_ned_mss.z, e_dt, 0.0, 0.0, 0.0);
280 :
281 : // apply horizontal shaping to refine estimate toward projected target state
282 114814 : shape_pos_vel_accel_xy(_target_pos_ned_m.xy() + delta_pos_m.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel_ms.xy(), _target_accel_ned_mss.xy(),
283 57407 : _estimate_pos_ned_m.xy(), _estimate_vel_ned_ms.xy(), _estimate_accel_ned_mss.xy(),
284 : 0.0, _accel_max_ne_mss, _jerk_max_ne_msss, e_dt, false);
285 :
286 : // apply vertical shaping to refine the Z estimate toward the projected
287 : // target state (jerk-limited, tuned via FOLL_ACCEL_D / FOLL_JERK_D)
288 57407 : shape_pos_vel_accel(_target_pos_ned_m.z + delta_pos_m.z, _target_vel_ned_ms.z + delta_vel_ms.z, _target_accel_ned_mss.z,
289 57407 : _estimate_pos_ned_m.z, _estimate_vel_ned_ms.z, _estimate_accel_ned_mss.z,
290 57407 : 0.0, 0.0, -_accel_max_d_mss, _accel_max_d_mss, _jerk_max_d_msss, e_dt, false);
291 :
292 : // apply angular shaping for heading estimate
293 172221 : shape_angle_vel_accel(radians(_target_heading_deg) + delta_heading_rad, radians(_target_heading_rate_degs), 0.0,
294 57407 : _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
295 57407 : 0.0, 0.0, radians(_accel_max_h_degss),
296 57407 : radians(_jerk_max_h_degsss), e_dt, false);
297 :
298 : // update heading angle separately to maintain proper wrapping [-PI, PI]
299 57407 : postype_t estimate_heading_rad = _estimate_heading_rad;
300 57407 : update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
301 57407 : _estimate_heading_rad = wrap_PI(float(estimate_heading_rad));
302 57407 : } else {
303 : // no valid estimate yet: initialise from latest target position
304 6 : _estimate_pos_ned_m = _target_pos_ned_m + delta_pos_m.topostype();
305 6 : _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel_ms;
306 6 : _estimate_accel_ned_mss = _target_accel_ned_mss;
307 6 : _estimate_heading_rad = radians(_target_heading_deg) + delta_heading_rad;
308 6 : _estimate_heading_rate_rads = radians(_target_heading_rate_degs);
309 6 : _estimate_heading_accel_radss = 0.0;
310 6 : _estimate_valid = true;
311 : }
312 :
313 57413 : Vector3f offset_m = _offset_m.get();
314 :
315 : // calculate estimated position and velocity with offsets applied
316 57413 : if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
317 : // offsets are in NED frame: simple addition
318 39822 : _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
319 39822 : _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
320 39822 : _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
321 : } else {
322 : // offsets are in FRD frame: rotate the FRD offset into NED using the target's heading
323 17591 : offset_m.xy().rotate(_estimate_heading_rad);
324 17591 : _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
325 : // seed vel/accel with the target's own translational vel/accel; rotational
326 : // contributions from the offset rotating with the target are added below
327 17591 : _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
328 17591 : _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
329 : // When heading rate/accel are available (kinematic shaping active), the offset
330 : // point is fixed in a frame rotating about the NED down-axis at the target's
331 : // heading rate (ω) and heading accel (α), at radius r = offset_m, giving:
332 : // v_rot = ω × r
333 : // a_rot = α × r + ω × (ω × r) (Euler/tangential term + centripetal term)
334 : // These are added on top of the target's translational vel/accel set above.
335 17591 : if (valid_kinematic_params) {
336 17591 : const Vector3f angular_vel{0.0f, 0.0f, _estimate_heading_rate_rads}; // ω: heading rate about NED down-axis
337 17591 : const Vector3f angular_accel{0.0f, 0.0f, _estimate_heading_accel_radss}; // α: heading angular acceleration
338 17591 : const Vector3f vel_due_to_rotation = angular_vel.cross(offset_m); // ω × r
339 17591 : _ofs_estimate_vel_ned_ms += vel_due_to_rotation;
340 17591 : _ofs_estimate_accel_ned_mss += angular_accel.cross(offset_m) + angular_vel.cross(vel_due_to_rotation); // α × r + ω × (ω × r)
341 : }
342 : }
343 :
344 : // update the distance and bearing to the target
345 57413 : update_dist_and_bearing_to_target();
346 :
347 57413 : _last_estimation_update_ms = now;
348 :
349 : // Check if the target is within the maximum distance
350 57413 : Vector3p current_position_ned_m;
351 57413 : if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
352 : // no idea where we are; knowing where other things are won't help.
353 0 : _estimate_valid = false;
354 0 : return;
355 : }
356 57413 : const Vector3p dist_vec_ned_m = _target_pos_ned_m - current_position_ned_m;
357 : // If _dist_max_m is not positive, we don't check the distance
358 57413 : if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
359 : // target is too far away, mark the estimate invalid
360 0 : _estimate_valid = false;
361 : }
362 39419043 : }
363 :
364 :
365 : //==============================================================================
366 : // Target Information Retrieval Functions
367 : //==============================================================================
368 :
369 : // Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
370 15272 : bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss) const
371 : {
372 15272 : if (!have_target()) {
373 0 : return false;
374 : }
375 :
376 15272 : pos_ned_m = _estimate_pos_ned_m;
377 15272 : vel_ned_ms = _estimate_vel_ned_ms;
378 15272 : accel_ned_mss = _estimate_accel_ned_mss;
379 :
380 15272 : return true;
381 : }
382 :
383 : // Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
384 15437 : bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss) const
385 : {
386 15437 : if (!have_target()) {
387 165 : return false;
388 : }
389 :
390 15272 : pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
391 15272 : vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
392 15272 : accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
393 :
394 15272 : return true;
395 : }
396 :
397 : // Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
398 0 : bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
399 : {
400 0 : WITH_SEMAPHORE(_follow_sem);
401 :
402 0 : if (!have_target()) {
403 0 : return false;
404 : }
405 :
406 0 : Vector3p current_position_ned_m;
407 0 : if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
408 0 : return false;
409 : }
410 :
411 0 : const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m;
412 0 : const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m;
413 0 : dist_ned = dist_vec_ned_m.tofloat();
414 0 : dist_with_offs = ofs_dist_vec.tofloat();
415 0 : vel_ned = _ofs_estimate_vel_ned_ms;
416 :
417 0 : return true;
418 0 : }
419 :
420 : // Retrieves the estimated target heading and heading rate in radians.
421 0 : bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads) const
422 : {
423 0 : if (!have_target()) {
424 0 : return false;
425 : }
426 :
427 : // return latest heading estimate
428 0 : heading_rad = _estimate_heading_rad;
429 0 : heading_rate_rads = _estimate_heading_rate_rads;
430 0 : return true;
431 : }
432 :
433 : // Retrieves the target's estimated global location and estimated velocity
434 812 : bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
435 : {
436 812 : WITH_SEMAPHORE(_follow_sem);
437 :
438 812 : if (!have_target()) {
439 6 : return false;
440 : }
441 :
442 806 : if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
443 0 : return false;
444 : }
445 806 : vel_ned = _estimate_vel_ned_ms;
446 :
447 : // The frame requested by FOLL_ALT_TYPE may not be the frame of location returned by ahrs.
448 : // Make sure we give the caller the frame they have asked for.
449 806 : return loc.change_alt_frame(_alt_type);
450 812 : }
451 :
452 : // Retrieves the target's estimated global location including configured offsets, and estimated velocity, for LUA bindings.
453 2828 : bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
454 : {
455 2828 : WITH_SEMAPHORE(_follow_sem);
456 :
457 2828 : if (!have_target()) {
458 10 : return false;
459 : }
460 2818 : if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
461 0 : return false;
462 : }
463 :
464 2818 : vel_ned = _ofs_estimate_vel_ned_ms;
465 :
466 : // give the caller the frame FOLL_ALT_TYPE asks for
467 2818 : return loc.change_alt_frame(_alt_type);
468 2828 : }
469 :
470 : // Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
471 17899 : bool AP_Follow::get_target_heading_deg(float &heading_deg)
472 : {
473 17899 : WITH_SEMAPHORE(_follow_sem);
474 :
475 17899 : if (!have_target()) {
476 0 : return false;
477 : }
478 :
479 : // return latest heading estimate
480 17899 : heading_deg = degrees(_estimate_heading_rad);
481 17899 : return true;
482 17899 : }
483 :
484 : // Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
485 15272 : bool AP_Follow::get_target_heading_rate_degs(float &heading_rate_degs)
486 : {
487 15272 : WITH_SEMAPHORE(_follow_sem);
488 :
489 15272 : if (!have_target()) {
490 0 : return false;
491 : }
492 :
493 : // return latest heading estimate
494 15272 : heading_rate_degs = degrees(_estimate_heading_rate_rads);
495 15272 : return true;
496 15272 : }
497 :
498 :
499 : //==============================================================================
500 : // MAVLink Message Handling
501 : //==============================================================================
502 :
503 : // Handles incoming MAVLink messages to update the target's position, velocity, and heading.
504 2637438 : void AP_Follow::handle_msg(const mavlink_message_t &msg)
505 : {
506 : // the LUA accessors read this state from the scripting thread
507 2637438 : WITH_SEMAPHORE(_follow_sem);
508 :
509 : // FOLL_SYSID no longer matches the system that supplied the data we hold. Forget the old
510 : // target's message type and update time. This must run before the switch below, which
511 : // consults _using_follow_target to decide whether GLOBAL_POSITION_INT is still wanted.
512 2637438 : if (_sysid != _sysid_of_data) {
513 5697 : _using_follow_target = false;
514 5697 : _last_location_update_ms = 0; // 0 = nothing heard from this target
515 : }
516 :
517 : // Invalidate the estimate if no position update has been received within the timeout period.
518 2644083 : if ((_last_location_update_ms == 0) ||
519 6645 : (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_ESTIMATE_TIMEOUT_MS)) {
520 2631320 : _estimate_valid = false; // mark estimate as invalid
521 2631320 : _using_follow_target = false; // reset follow-target usage flag
522 : }
523 :
524 2637438 : if (!should_handle_message(msg)) {
525 : // ignore message if filtering rules reject it (e.g., wrong sysid)
526 2632005 : return;
527 : }
528 :
529 : // decode MAVLink message
530 5433 : bool updated = false;
531 :
532 5433 : switch (msg.msgid) {
533 861 : case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
534 : // if we are using follow_target, ignore global_position_int messages
535 861 : if (_using_follow_target) {
536 0 : return;
537 : }
538 : // handle standard global position messages
539 861 : updated = handle_global_position_int_message(msg);
540 861 : break;
541 : }
542 58 : case MAVLINK_MSG_ID_FOLLOW_TARGET: {
543 : // handle follow-target specific messages
544 58 : updated = handle_follow_target_message(msg);
545 58 : break;
546 : }
547 : }
548 :
549 5433 : if (updated) {
550 : // reset the estimate on a large error or a change of source system
551 812 : if (estimate_error_too_large() || (_sysid_of_data != msg.sysid)) {
552 6 : _estimate_valid = false;
553 : }
554 :
555 : // record the system that supplied this update. this is msg.sysid rather than
556 : // _sysid because msg.sysid is the value should_handle_message() validated, and
557 : // _sysid can be written by the scripting thread part way through this function
558 812 : _sysid_of_data = msg.sysid;
559 :
560 : #if HAL_LOGGING_ENABLED
561 : // log current follow diagnostic data
562 812 : Log_Write_FOLL();
563 : #endif
564 : }
565 2637438 : }
566 :
567 : // Returns true if the incoming MAVLink message should be processed for target updates.
568 2637438 : bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
569 : {
570 : // exit immediately if not enabled
571 2637438 : if (!_enabled) {
572 2618031 : return false;
573 : }
574 :
575 : // sysid 0 is the broadcast system and is never a valid target. without this
576 : // a sender broadcasting as sysid 0 would compare equal to an unset _sysid
577 19407 : if (msg.sysid == 0) {
578 0 : return false;
579 : }
580 :
581 : // skip our own messages
582 19407 : if (msg.sysid == mavlink_system.sysid) {
583 0 : return false;
584 : }
585 :
586 : // skip message if not from our target. a _sysid of zero means no target
587 : // has been selected, so no message is ever accepted
588 19407 : if (msg.sysid != _sysid) {
589 13974 : return false;
590 : }
591 :
592 5433 : return true;
593 : }
594 :
595 : // Checks whether the current estimate should be reset based on position and velocity errors.
596 812 : bool AP_Follow::estimate_error_too_large() const
597 : {
598 812 : const float timeout_sec = _timeout;
599 :
600 : // Calculate position thresholds based on maximum acceleration then deceleration for the timeout duration
601 812 : const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(timeout_sec * 0.5);
602 812 : const float pos_thresh_vert_m = _accel_max_d_mss.get() * sq(timeout_sec * 0.5);
603 :
604 : // Calculate velocity thresholds using the new helper function
605 812 : const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
606 812 : const float vel_thresh_vert_ms = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
607 :
608 : // Calculate current position and velocity errors
609 812 : const Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
610 812 : const Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
611 :
612 812 : const Vector2f pos_error_xy = pos_error.xy();
613 812 : const float pos_error_z = pos_error.z;
614 812 : const Vector2f vel_error_xy = vel_error.xy();
615 812 : const float vel_error_z = vel_error.z;
616 :
617 : // Check horizontal and vertical separately
618 812 : const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
619 812 : const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
620 812 : const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
621 812 : const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
622 :
623 812 : return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
624 : }
625 :
626 : // Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
627 1624 : float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
628 : {
629 : // a non-positive jerk limit means "no jerk limit": acceleration can be
630 : // applied instantly, so the profile is a pure trapezoid. Guarding here also
631 : // avoids a divide-by-zero when the FOLL_JERK_* parameter is set to zero.
632 1624 : if (!is_positive(jerk_max)) {
633 0 : return accel_max * timeout_sec;
634 : }
635 1624 : const float t_jerk = accel_max / jerk_max;
636 1624 : const float t_total_jerk = 2.0f * t_jerk;
637 :
638 1624 : if (timeout_sec >= t_total_jerk) {
639 : // time to ramp up, constant accel phase, and ramp down
640 1624 : const float t_const = timeout_sec - t_total_jerk;
641 1624 : const float delta_v_jerk = 0.5f * accel_max * t_jerk;
642 1624 : const float delta_v_const = accel_max * t_const;
643 1624 : return 2.0f * delta_v_jerk + delta_v_const;
644 : } else {
645 : // timeout too short: pure triangle profile
646 0 : const float t_half = timeout_sec * 0.5f;
647 0 : return jerk_max * sq(t_half);
648 : }
649 : }
650 :
651 : // Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
652 861 : bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
653 : {
654 : // decode GLOBAL_POSITION_INT message into packet struct
655 : mavlink_global_position_int_t packet;
656 861 : mavlink_msg_global_position_int_decode(&msg, &packet);
657 :
658 : // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
659 861 : if ((packet.lat == 0 && packet.lon == 0)) {
660 0 : return false;
661 : }
662 :
663 861 : Location target_location;
664 861 : target_location.lat = packet.lat;
665 861 : target_location.lng = packet.lon;
666 :
667 861 : switch((Location::AltFrame)_alt_type) {
668 767 : case Location::AltFrame::ABSOLUTE:
669 767 : target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
670 767 : break;
671 94 : case Location::AltFrame::ABOVE_HOME:
672 94 : target_location.set_alt_cm(packet.relative_alt * 0.1, Location::AltFrame::ABOVE_HOME);
673 94 : break;
674 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduCopter)
675 0 : case Location::AltFrame::ABOVE_TERRAIN:
676 : /// Altitude comes in as AMSL
677 0 : target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
678 : // convert the incoming altitude to terrain altitude, but fail if there is no terrain data available
679 0 : if (!target_location.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
680 0 : return false;
681 : };
682 0 : break;
683 : #endif
684 0 : default:
685 : // don't process the packet if the _alt_type is invalid
686 0 : return false;
687 : }
688 :
689 : // convert global location to local NED frame position
690 861 : Vector3p target_pos_neu_m;
691 861 : if (!target_location.get_vector_from_origin_NEU_m(target_pos_neu_m)) {
692 107 : return false;
693 : }
694 :
695 754 : if (packet.hdg <= 36000) {
696 : // valid heading field available (in centi-degrees)
697 754 : _target_heading_deg = packet.hdg * 0.01f;
698 0 : } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
699 : // heading missing but relative offset mode requires heading -> reject
700 0 : return false;
701 : } else {
702 : // no heading available: set heading rate to zero
703 0 : _target_heading_rate_degs = 0.0f;
704 : }
705 :
706 754 : _target_pos_ned_m.xy() = target_pos_neu_m.xy();
707 754 : _target_pos_ned_m.z = -target_pos_neu_m.z;
708 :
709 : // decode target velocity components (cm/s converted to m/s)
710 754 : _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
711 754 : _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
712 754 : _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
713 :
714 : // target acceleration not available in GLOBAL_POSITION_INT
715 754 : _target_accel_ned_mss.zero();
716 :
717 : // apply jitter-corrected timestamp to this update
718 754 : _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
719 :
720 754 : return true;
721 : }
722 :
723 : // Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
724 58 : bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
725 : {
726 : // decode FOLLOW_TARGET message into packet struct
727 : mavlink_follow_target_t packet;
728 58 : mavlink_msg_follow_target_decode(&msg, &packet);
729 :
730 : // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
731 58 : if ((packet.lat == 0 && packet.lon == 0)) {
732 0 : return false;
733 : }
734 :
735 : // require that at least position is estimated (bit 0 of est_capabilities)
736 58 : if ((packet.est_capabilities & (1<<0)) == 0) {
737 0 : return false;
738 : }
739 :
740 : // build Location object from latitude, longitude, and altitude (alt in meters)
741 : const Location target_location {
742 : packet.lat,
743 : packet.lon,
744 58 : int32_t(packet.alt * 100), // convert meters to centimeters
745 : Location::AltFrame::ABSOLUTE
746 58 : };
747 :
748 : // convert global location to local NED frame position
749 58 : Vector3p target_pos_neu_m;
750 58 : if (!target_location.get_vector_from_origin_NEU_m(target_pos_neu_m)) {
751 0 : return false;
752 : }
753 :
754 : // adjust Z coordinate to NED frame (NEU altitude -> NED)
755 58 : Location origin;
756 58 : if (!AP::ahrs().get_origin(origin)) {
757 0 : return false;
758 : }
759 :
760 : // decode attitude if available (bit 3 of est_capabilities)
761 58 : if (packet.est_capabilities & (1 << 3)) {
762 : // reconstruct quaternion from packet
763 58 : Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
764 : float roll, pitch, yaw;
765 58 : q.to_euler(roll, pitch, yaw);
766 :
767 : // store heading in degrees, wrapped 0–360
768 58 : _target_heading_deg = wrap_360(degrees(yaw));
769 :
770 : // transform body rates (roll, pitch, yaw) to earth frame rates
771 58 : Matrix3f R;
772 58 : q.rotation_matrix(R);
773 58 : Vector3f omega_body = Vector3f{packet.rates[0], packet.rates[1], packet.rates[2]};
774 58 : Vector3f omega_world = R * omega_body; // rotate rates into earth frame
775 :
776 : // store heading rate (yaw rate in world frame) in degrees/sec
777 58 : _target_heading_rate_degs = degrees(omega_world.z);
778 0 : } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
779 : // if attitude unavailable and using relative frame, cannot compute — abort
780 0 : return false;
781 : } else {
782 : // otherwise, default heading rate to zero
783 0 : _target_heading_rate_degs = 0.0f;
784 : }
785 :
786 58 : _target_pos_ned_m.xy() = target_pos_neu_m.xy();
787 58 : _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
788 :
789 : // decode velocity if available (bit 1 of est_capabilities)
790 58 : if (packet.est_capabilities & (1<<1)) {
791 58 : _target_vel_ned_ms.x = packet.vel[0]; // velocity north
792 58 : _target_vel_ned_ms.y = packet.vel[1]; // velocity east
793 58 : _target_vel_ned_ms.z = packet.vel[2]; // velocity down
794 : } else {
795 0 : _target_vel_ned_ms.zero();
796 : }
797 :
798 : // decode acceleration if available (bit 2 of est_capabilities)
799 58 : if (packet.est_capabilities & (1 << 2)) {
800 0 : _target_accel_ned_mss.x = packet.acc[0]; // acceleration north
801 0 : _target_accel_ned_mss.y = packet.acc[1]; // acceleration east
802 0 : _target_accel_ned_mss.z = packet.acc[2]; // acceleration down
803 : } else {
804 58 : _target_accel_ned_mss.zero();
805 : }
806 :
807 : // apply jitter-corrected timestamp to this update
808 58 : _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
809 :
810 : // prefer FOLLOW_TARGET over GLOBAL_POSITION_INT from now on
811 58 : _using_follow_target = true;
812 :
813 58 : return true;
814 : }
815 :
816 :
817 : //==============================================================================
818 : // Offset Initialization and Adjustment Functions
819 : //==============================================================================
820 :
821 : // Initializes the positional offsets from the target vehicle if not already set.
822 15437 : void AP_Follow::init_offsets_if_required()
823 : {
824 : // return immediately if offsets have already been set
825 15437 : if (!_offset_m.get().is_zero()) {
826 15437 : return;
827 : }
828 0 : _offsets_were_zero = true;
829 :
830 0 : if (!have_target()) {
831 0 : return;
832 : }
833 :
834 : // Check if the target is within the maximum distance
835 0 : Vector3p current_position_ned_m;
836 0 : if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
837 0 : return;
838 : }
839 0 : const Vector3f dist_vec_ned_m = (_target_pos_ned_m - current_position_ned_m).tofloat();
840 :
841 0 : if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE)) {
842 : // rotate offset into vehicle-relative frame based on heading
843 0 : _offset_m.set(rotate_vector(-dist_vec_ned_m, -degrees(_estimate_heading_rad)));
844 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Relative follow offset loaded");
845 : } else {
846 : // initialize offset in NED frame (no heading rotation)
847 0 : _offset_m.set(-dist_vec_ned_m);
848 :
849 : // ensure offset type is set to NED frame if initialized this way
850 0 : _offset_type.set(AP_FOLLOW_OFFSET_TYPE_NED);
851 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "N-E-D follow offset loaded");
852 : }
853 : }
854 :
855 : // Rotates a 3D vector clockwise by the specified angle (degrees).
856 0 : Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
857 : {
858 : // rotate roll, pitch input from north facing to vehicle's perspective
859 0 : Vector3f ret = vec;
860 0 : ret.xy().rotate(radians(angle_deg));
861 :
862 0 : return ret;
863 : }
864 :
865 : // Resets follow mode offsets to zero if they were automatically initialized.
866 7 : void AP_Follow::clear_offsets_if_required()
867 : {
868 7 : if (_offsets_were_zero) {
869 0 : _offset_m.set(Vector3f());
870 0 : _offsets_were_zero = false;
871 : }
872 7 : }
873 :
874 :
875 : //==============================================================================
876 : // Distance and Bearing Management
877 : //==============================================================================
878 :
879 : // Resets the recorded distance and bearing to the target to zero.
880 39361630 : void AP_Follow::clear_dist_and_bearing_to_target()
881 : {
882 39361630 : _dist_to_target_m = 0.0f;
883 39361630 : _bearing_to_target_deg = 0.0f;
884 39361630 : }
885 :
886 : // Updates the recorded distance and bearing to the target to zero.
887 57413 : void AP_Follow::update_dist_and_bearing_to_target()
888 : {
889 57413 : Vector3p current_position_ned_m;
890 57413 : if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
891 : // if unable to retrieve local position, clear distance/bearing info
892 0 : clear_dist_and_bearing_to_target();
893 : } else {
894 : // get_relative_position_NED_origin() already returns metres in the NED
895 : // frame, matching _ofs_estimate_pos_ned_m, so no conversion is required.
896 57413 : const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m;
897 :
898 : // record distance and bearing to target for reporting/logging
899 57413 : if (ofs_dist_vec.xy().is_zero()) {
900 0 : clear_dist_and_bearing_to_target();
901 : } else {
902 57413 : _dist_to_target_m = ofs_dist_vec.xy().length();
903 57413 : _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
904 : }
905 : }
906 57413 : }
907 :
908 :
909 : //==============================================================================
910 : // Logging
911 : //==============================================================================
912 :
913 : // Writes a diagnostic onboard log message containing target and vehicle tracking data for Follow mode.
914 : #if HAL_LOGGING_ENABLED
915 812 : void AP_Follow::Log_Write_FOLL()
916 : {
917 : // retrieve latest estimated location and velocity
918 812 : Location loc_estimate{};
919 812 : Vector3f vel_estimate;
920 812 : UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
921 :
922 812 : Location target_location;
923 812 : UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(target_location, _target_pos_ned_m));
924 :
925 : // log the lead target's reported position and vehicle's estimated position
926 : // @LoggerMessage: FOLL
927 : // @Description: Follow library diagnostic data
928 : // @Field: TimeUS: Time since system startup (microseconds)
929 : // @Field: Lat: Target latitude (degrees * 1E7)
930 : // @Field: Lon: Target longitude (degrees * 1E7)
931 : // @Field: Alt: Target absolute altitude (centimeters)
932 : // @Field: VelN: Target velocity, North (m/s)
933 : // @Field: VelE: Target velocity, East (m/s)
934 : // @Field: VelD: Target velocity, Down (m/s)
935 : // @Field: LatE: Vehicle estimated latitude (degrees * 1E7)
936 : // @Field: LonE: Vehicle estimated longitude (degrees * 1E7)
937 : // @Field: AltE: Vehicle estimated altitude (centimeters)
938 : // @Field: FrmE: Vehicle estimated altitude Frame
939 1624 : AP::logger().WriteStreaming("FOLL",
940 : "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE,FrmE", // labels
941 : "sDUmnnnDUm-", // units
942 : "F--B000--B-", // mults
943 : "QLLifffLLib", // fmt
944 : AP_HAL::micros64(),
945 : target_location.lat,
946 : target_location.lng,
947 : target_location.alt,
948 812 : (double)_target_vel_ned_ms.x,
949 812 : (double)_target_vel_ned_ms.y,
950 812 : (double)_target_vel_ned_ms.z,
951 : loc_estimate.lat,
952 : loc_estimate.lng,
953 : loc_estimate.alt,
954 : loc_estimate.get_alt_frame()
955 : );
956 812 : }
957 : #endif // HAL_LOGGING_ENABLED
958 :
959 :
960 : //==============================================================================
961 : // Accessors and Helpers
962 : //==============================================================================
963 :
964 : // Returns true if the target data we hold is fresh and was supplied by the configured system.
965 : // This is the gate for update_estimates(). It deliberately excludes _estimate_valid so that
966 : // update_estimates() can rebuild the estimate after that flag has been cleared.
967 39489009 : bool AP_Follow::have_target_data(void) const
968 : {
969 39489009 : if (!_enabled) {
970 39075835 : return false;
971 : }
972 :
973 : // no target system has been configured
974 413174 : if (_sysid == 0) {
975 215638 : return false;
976 : }
977 :
978 : // we have not yet accepted an update from the configured system
979 197536 : if (_sysid_of_data != _sysid) {
980 18947 : return false;
981 : }
982 :
983 178588 : return true;
984 : }
985 :
986 : // Returns true if a usable estimate of the configured target is available.
987 : // Every accessor is gated on this, but a true return does not guarantee that they succeed.
988 : // The location getters can still fail on the AHRS origin lookup or on the FOLL_ALT_TYPE frame
989 : // conversion, and the target state can change between this call and the accessor call, so
990 : // callers must check the value every accessor returns.
991 70523 : bool AP_Follow::have_target(void) const
992 : {
993 70523 : return _estimate_valid && have_target_data();
994 : }
995 :
996 : //==============================================================================
997 : // AP_Follow Accessor
998 : //==============================================================================
999 :
1000 : // Accessor for the AP_Follow singleton instance.
1001 : namespace AP {
1002 0 : AP_Follow &follow()
1003 : {
1004 0 : return *AP_Follow::get_singleton();
1005 : }
1006 : }
1007 :
1008 : #endif // AP_FOLLOW_ENABLED
|
