mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
1009 lines
40 KiB
C++
1009 lines
40 KiB
C++
/*
|
||
This program is free software: you can redistribute it and/or modify
|
||
it under the terms of the GNU General Public License as published by
|
||
the Free Software Foundation, either version 3 of the License, or
|
||
(at your option) any later version.
|
||
|
||
This program is distributed in the hope that it will be useful,
|
||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
GNU General Public License for more details.
|
||
|
||
You should have received a copy of the GNU General Public License
|
||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
//==============================================================================
|
||
// AP_Follow Library
|
||
// Target Following Logic for ArduPilot Vehicles
|
||
//==============================================================================
|
||
|
||
|
||
//==============================================================================
|
||
// Includes
|
||
//==============================================================================
|
||
|
||
#include "AP_Follow_config.h"
|
||
|
||
#if AP_FOLLOW_ENABLED
|
||
|
||
#include <AP_HAL/AP_HAL.h>
|
||
#include "AP_Follow.h"
|
||
#include <ctype.h>
|
||
#include <stdio.h>
|
||
|
||
#include <AP_AHRS/AP_AHRS.h>
|
||
#include <AP_Logger/AP_Logger.h>
|
||
#include <GCS_MAVLink/GCS.h>
|
||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||
#include <AP_Scheduler/AP_Scheduler.h>
|
||
|
||
extern const AP_HAL::HAL& hal;
|
||
|
||
|
||
//==============================================================================
|
||
// Constants and Definitions
|
||
//==============================================================================
|
||
|
||
#define AP_FOLLOW_TIMEOUT 3 // position estimate timeout in seconds
|
||
#define AP_FOLLOW_ESTIMATE_TIMEOUT_MS 10000 // discard the target estimate if we have not heard from the target in 10 seconds
|
||
|
||
#define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame
|
||
#define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading
|
||
|
||
#define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default
|
||
|
||
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||
#define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast<float>(Location::AltFrame::ABSOLUTE)
|
||
#define AP_FOLLOW_DIST_MAX_DEFAULT 0 // zero = ignored
|
||
#else
|
||
#define AP_FOLLOW_ALT_TYPE_DEFAULT static_cast<float>(Location::AltFrame::ABOVE_HOME)
|
||
#define AP_FOLLOW_DIST_MAX_DEFAULT 100
|
||
#endif
|
||
|
||
|
||
//==============================================================================
|
||
// Constructor
|
||
//==============================================================================
|
||
|
||
AP_Follow *AP_Follow::_singleton;
|
||
|
||
// table of user settable parameters
|
||
const AP_Param::GroupInfo AP_Follow::var_info[] = {
|
||
|
||
// @Param: _ENABLE
|
||
// @DisplayName: Follow enable/disable
|
||
// @Description: Enabled/disable following a target
|
||
// @Values: 0:Disabled,1:Enabled
|
||
// @User: Standard
|
||
AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_Follow, _enabled, 0, AP_PARAM_FLAG_ENABLE),
|
||
|
||
// 2 is reserved for TYPE parameter
|
||
|
||
// @Param: _SYSID
|
||
// @DisplayName: Follow target's mavlink system id
|
||
// @Description: Follow target's mavlink system id. Zero means no target has been selected and following is inactive.
|
||
// @Range: 0 255
|
||
// @User: Standard
|
||
AP_GROUPINFO("_SYSID", 3, AP_Follow, _sysid, 0),
|
||
|
||
// 4 is reserved for MARGIN parameter
|
||
|
||
// @Param: _DIST_MAX
|
||
// @DisplayName: Follow distance maximum
|
||
// @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid. If zero there is no maximum.
|
||
// @Units: m
|
||
// @Range: 0 1000
|
||
// @User: Standard
|
||
AP_GROUPINFO("_DIST_MAX", 5, AP_Follow, _dist_max_m, AP_FOLLOW_DIST_MAX_DEFAULT),
|
||
|
||
// @Param: _OFS_TYPE
|
||
// @DisplayName: Follow offset type
|
||
// @Description: Follow offset type
|
||
// @Values: 0:North-East-Down, 1:Relative to lead vehicle heading
|
||
// @User: Standard
|
||
AP_GROUPINFO("_OFS_TYPE", 6, AP_Follow, _offset_type, AP_FOLLOW_OFFSET_TYPE_NED),
|
||
|
||
// @Param: _OFS_X
|
||
// @DisplayName: Follow offsets in meters north/forward
|
||
// @Description: Follow offsets in meters north/forward. If positive, this vehicle fly ahead or north of lead vehicle. Depends on FOLL_OFS_TYPE
|
||
// @Range: -100 100
|
||
// @Units: m
|
||
// @Increment: 1
|
||
// @User: Standard
|
||
|
||
// @Param: _OFS_Y
|
||
// @DisplayName: Follow offsets in meters east/right
|
||
// @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
|
||
// @Range: -100 100
|
||
// @Units: m
|
||
// @Increment: 1
|
||
// @User: Standard
|
||
|
||
// @Param: _OFS_Z
|
||
// @DisplayName: Follow offsets in meters down
|
||
// @Description: Follow offsets in meters down. If positive, this vehicle will fly below the lead vehicle
|
||
// @Range: -100 100
|
||
// @Units: m
|
||
// @Increment: 1
|
||
// @User: Standard
|
||
AP_GROUPINFO("_OFS", 7, AP_Follow, _offset_m, 0),
|
||
|
||
#if !(APM_BUILD_TYPE(APM_BUILD_Rover))
|
||
// @Param: _YAW_BEHAVE
|
||
// @DisplayName: Follow yaw behaviour
|
||
// @Description: Follow yaw behaviour
|
||
// @Values: 0:None,1:Face Lead Vehicle,2:Same as Lead vehicle,3:Direction of Flight
|
||
// @User: Standard
|
||
AP_GROUPINFO("_YAW_BEHAVE", 8, AP_Follow, _yaw_behave, 1),
|
||
#endif
|
||
|
||
// @Param: _POS_P
|
||
// @DisplayName: Follow position error P gain
|
||
// @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
|
||
// @Range: 0.01 1.00
|
||
// @Increment: 0.01
|
||
// @User: Standard
|
||
AP_SUBGROUPINFO(_p_pos, "_POS_", 9, AP_Follow, AC_P),
|
||
|
||
#if !(APM_BUILD_TYPE(APM_BUILD_Rover))
|
||
// @Param: _ALT_TYPE
|
||
// @DisplayName: Follow altitude type
|
||
// @Description: Follow altitude type
|
||
// @Values: 0:absolute, 1:relative, 3:terrain
|
||
// @User: Standard
|
||
AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
|
||
#endif
|
||
|
||
// @Param: _OPTIONS
|
||
// @DisplayName: Follow options
|
||
// @Description: Follow options bitmask
|
||
// @Bitmask: 0:Mount follows lead vehicle on mode enter
|
||
// @User: Standard
|
||
AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
|
||
|
||
// @Param: _ACCEL_NE
|
||
// @DisplayName: Acceleration limit for the horizontal kinematic input shaping
|
||
// @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in velocity
|
||
// @Range: 0 10
|
||
// @Units: m/s/s
|
||
// @User: Advanced
|
||
AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 5.0),
|
||
|
||
// @Param: _JERK_NE
|
||
// @DisplayName: Jerk limit for the horizontal kinematic input shaping
|
||
// @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in acceleration
|
||
// @Range: 0 20
|
||
// @Units: m/s/s/s
|
||
// @User: Advanced
|
||
AP_GROUPINFO("_JERK_NE", 13, AP_Follow, _jerk_max_ne_msss, 10.0),
|
||
|
||
// @Param: _ACCEL_D
|
||
// @DisplayName: Acceleration limit for the vertical kinematic input shaping
|
||
// @Description: Acceleration limit of the vertical kinematic path generation used to determine how quickly the estimate varies in velocity
|
||
// @Range: 0 10
|
||
// @Units: m/s/s
|
||
// @User: Advanced
|
||
AP_GROUPINFO("_ACCEL_D", 14, AP_Follow, _accel_max_d_mss, 5.0),
|
||
|
||
// @Param: _JERK_D
|
||
// @DisplayName: Jerk limit for the vertical kinematic input shaping
|
||
// @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the estimate varies in acceleration
|
||
// @Range: 0 20
|
||
// @Units: m/s/s/s
|
||
// @User: Advanced
|
||
AP_GROUPINFO("_JERK_D", 15, AP_Follow, _jerk_max_d_msss, 10.0),
|
||
|
||
// @Param: _ACCEL_H
|
||
// @DisplayName: Angular acceleration limit for the heading kinematic input shaping
|
||
// @Description: Angular acceleration limit of the heading kinematic path generation used to determine how quickly the estimate varies in angular velocity
|
||
// @Range: 0 720
|
||
// @Units: deg/s/s
|
||
// @User: Advanced
|
||
AP_GROUPINFO("_ACCEL_H", 16, AP_Follow, _accel_max_h_degss, 360.0),
|
||
|
||
// @Param: _JERK_H
|
||
// @DisplayName: Angular jerk limit for the heading kinematic input shaping
|
||
// @Description: Angular jerk limit of the heading kinematic path generation used to determine how quickly the estimate varies in angular acceleration
|
||
// @Range: 0 1440
|
||
// @Units: deg/s/s/s
|
||
// @User: Advanced
|
||
AP_GROUPINFO("_JERK_H", 17, AP_Follow, _jerk_max_h_degsss, 720.0),
|
||
|
||
// @Param: _TIMEOUT
|
||
// @DisplayName: Follow timeout
|
||
// @Description: Follow position update from lead - timeout after x seconds
|
||
// @User: Standard
|
||
// @Units: s
|
||
AP_GROUPINFO("_TIMEOUT", 18, AP_Follow, _timeout, AP_FOLLOW_TIMEOUT),
|
||
|
||
AP_GROUPEND
|
||
};
|
||
|
||
// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
|
||
AP_Follow::AP_Follow() :
|
||
_p_pos(AP_FOLLOW_POS_P_DEFAULT)
|
||
{
|
||
_singleton = this;
|
||
AP_Param::setup_object_defaults(this, var_info);
|
||
}
|
||
|
||
|
||
//==============================================================================
|
||
// Target Estimation Update Functions
|
||
//==============================================================================
|
||
|
||
// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
|
||
void AP_Follow::update_estimates()
|
||
{
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
// if we do not hold fresh data from the configured target, invalidate the estimate.
|
||
// this cannot gate on have_target() because that tests _estimate_valid, which is
|
||
// cleared below, and the estimate could then never be rebuilt
|
||
if (!have_target_data()) {
|
||
clear_dist_and_bearing_to_target();
|
||
_estimate_valid = false;
|
||
return;
|
||
}
|
||
|
||
// check for timeout
|
||
if ((_last_location_update_ms == 0) || ((AP_HAL::millis() - _last_location_update_ms) > (uint32_t)(_timeout * 1000.0f))) {
|
||
clear_dist_and_bearing_to_target();
|
||
_estimate_valid = false;
|
||
return;
|
||
}
|
||
|
||
const uint32_t now = AP_HAL::millis();
|
||
|
||
// calculate time since last location update in seconds
|
||
const float dt = (now - _last_location_update_ms) * 0.001f;
|
||
|
||
// project target's position and velocity forward using simple kinematics
|
||
Vector3f delta_pos_m = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
|
||
Vector3f delta_vel_ms = _target_accel_ned_mss * dt;
|
||
float delta_heading_rad = radians(_target_heading_rate_degs) * dt;
|
||
|
||
// calculate time since last estimation update
|
||
const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
|
||
|
||
const bool valid_kinematic_params = (_accel_max_ne_mss > 0.0f) && (_jerk_max_ne_msss > 0.0f) &&
|
||
(_accel_max_d_mss > 0.0f) && (_jerk_max_d_msss > 0.0f) &&
|
||
(_accel_max_h_degss > 0.0f) && (_jerk_max_h_degsss > 0.0f);
|
||
|
||
if (_estimate_valid && valid_kinematic_params) {
|
||
// update X/Y position, velocity, acceleration with shaping
|
||
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());
|
||
|
||
// integrate the Z axis estimate forward by e_dt
|
||
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);
|
||
|
||
// apply horizontal shaping to refine estimate toward projected target state
|
||
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(),
|
||
_estimate_pos_ned_m.xy(), _estimate_vel_ned_ms.xy(), _estimate_accel_ned_mss.xy(),
|
||
0.0, _accel_max_ne_mss, _jerk_max_ne_msss, e_dt, false);
|
||
|
||
// apply vertical shaping to refine the Z estimate toward the projected
|
||
// target state (jerk-limited, tuned via FOLL_ACCEL_D / FOLL_JERK_D)
|
||
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,
|
||
_estimate_pos_ned_m.z, _estimate_vel_ned_ms.z, _estimate_accel_ned_mss.z,
|
||
0.0, 0.0, -_accel_max_d_mss, _accel_max_d_mss, _jerk_max_d_msss, e_dt, false);
|
||
|
||
// apply angular shaping for heading estimate
|
||
shape_angle_vel_accel(radians(_target_heading_deg) + delta_heading_rad, radians(_target_heading_rate_degs), 0.0,
|
||
_estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
|
||
0.0, 0.0, radians(_accel_max_h_degss),
|
||
radians(_jerk_max_h_degsss), e_dt, false);
|
||
|
||
// update heading angle separately to maintain proper wrapping [-PI, PI]
|
||
postype_t estimate_heading_rad = _estimate_heading_rad;
|
||
update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
|
||
_estimate_heading_rad = wrap_PI(float(estimate_heading_rad));
|
||
} else {
|
||
// no valid estimate yet: initialise from latest target position
|
||
_estimate_pos_ned_m = _target_pos_ned_m + delta_pos_m.topostype();
|
||
_estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel_ms;
|
||
_estimate_accel_ned_mss = _target_accel_ned_mss;
|
||
_estimate_heading_rad = radians(_target_heading_deg) + delta_heading_rad;
|
||
_estimate_heading_rate_rads = radians(_target_heading_rate_degs);
|
||
_estimate_heading_accel_radss = 0.0;
|
||
_estimate_valid = true;
|
||
}
|
||
|
||
Vector3f offset_m = _offset_m.get();
|
||
|
||
// calculate estimated position and velocity with offsets applied
|
||
if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
|
||
// offsets are in NED frame: simple addition
|
||
_ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
|
||
_ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
|
||
_ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
|
||
} else {
|
||
// offsets are in FRD frame: rotate the FRD offset into NED using the target's heading
|
||
offset_m.xy().rotate(_estimate_heading_rad);
|
||
_ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
|
||
// seed vel/accel with the target's own translational vel/accel; rotational
|
||
// contributions from the offset rotating with the target are added below
|
||
_ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
|
||
_ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
|
||
// When heading rate/accel are available (kinematic shaping active), the offset
|
||
// point is fixed in a frame rotating about the NED down-axis at the target's
|
||
// heading rate (ω) and heading accel (α), at radius r = offset_m, giving:
|
||
// v_rot = ω × r
|
||
// a_rot = α × r + ω × (ω × r) (Euler/tangential term + centripetal term)
|
||
// These are added on top of the target's translational vel/accel set above.
|
||
if (valid_kinematic_params) {
|
||
const Vector3f angular_vel{0.0f, 0.0f, _estimate_heading_rate_rads}; // ω: heading rate about NED down-axis
|
||
const Vector3f angular_accel{0.0f, 0.0f, _estimate_heading_accel_radss}; // α: heading angular acceleration
|
||
const Vector3f vel_due_to_rotation = angular_vel.cross(offset_m); // ω × r
|
||
_ofs_estimate_vel_ned_ms += vel_due_to_rotation;
|
||
_ofs_estimate_accel_ned_mss += angular_accel.cross(offset_m) + angular_vel.cross(vel_due_to_rotation); // α × r + ω × (ω × r)
|
||
}
|
||
}
|
||
|
||
// update the distance and bearing to the target
|
||
update_dist_and_bearing_to_target();
|
||
|
||
_last_estimation_update_ms = now;
|
||
|
||
// Check if the target is within the maximum distance
|
||
Vector3p current_position_ned_m;
|
||
if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
|
||
// no idea where we are; knowing where other things are won't help.
|
||
_estimate_valid = false;
|
||
return;
|
||
}
|
||
const Vector3p dist_vec_ned_m = _target_pos_ned_m - current_position_ned_m;
|
||
// If _dist_max_m is not positive, we don't check the distance
|
||
if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
|
||
// target is too far away, mark the estimate invalid
|
||
_estimate_valid = false;
|
||
}
|
||
}
|
||
|
||
|
||
//==============================================================================
|
||
// Target Information Retrieval Functions
|
||
//==============================================================================
|
||
|
||
// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
|
||
bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss) const
|
||
{
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
pos_ned_m = _estimate_pos_ned_m;
|
||
vel_ned_ms = _estimate_vel_ned_ms;
|
||
accel_ned_mss = _estimate_accel_ned_mss;
|
||
|
||
return true;
|
||
}
|
||
|
||
// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
|
||
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
|
||
{
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
|
||
vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
|
||
accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
|
||
|
||
return true;
|
||
}
|
||
|
||
// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
|
||
bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
|
||
{
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
Vector3p current_position_ned_m;
|
||
if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
|
||
return false;
|
||
}
|
||
|
||
const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m;
|
||
const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m;
|
||
dist_ned = dist_vec_ned_m.tofloat();
|
||
dist_with_offs = ofs_dist_vec.tofloat();
|
||
vel_ned = _ofs_estimate_vel_ned_ms;
|
||
|
||
return true;
|
||
}
|
||
|
||
// Retrieves the estimated target heading and heading rate in radians.
|
||
bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads) const
|
||
{
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
// return latest heading estimate
|
||
heading_rad = _estimate_heading_rad;
|
||
heading_rate_rads = _estimate_heading_rate_rads;
|
||
return true;
|
||
}
|
||
|
||
// Retrieves the target's estimated global location and estimated velocity
|
||
bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
|
||
{
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
|
||
return false;
|
||
}
|
||
vel_ned = _estimate_vel_ned_ms;
|
||
|
||
// The frame requested by FOLL_ALT_TYPE may not be the frame of location returned by ahrs.
|
||
// Make sure we give the caller the frame they have asked for.
|
||
return loc.change_alt_frame(_alt_type);
|
||
}
|
||
|
||
// Retrieves the target's estimated global location including configured offsets, and estimated velocity, for LUA bindings.
|
||
bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
|
||
{
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
|
||
return false;
|
||
}
|
||
|
||
vel_ned = _ofs_estimate_vel_ned_ms;
|
||
|
||
// give the caller the frame FOLL_ALT_TYPE asks for
|
||
return loc.change_alt_frame(_alt_type);
|
||
}
|
||
|
||
// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
|
||
bool AP_Follow::get_target_heading_deg(float &heading_deg)
|
||
{
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
// return latest heading estimate
|
||
heading_deg = degrees(_estimate_heading_rad);
|
||
return true;
|
||
}
|
||
|
||
// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
|
||
bool AP_Follow::get_target_heading_rate_degs(float &heading_rate_degs)
|
||
{
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
if (!have_target()) {
|
||
return false;
|
||
}
|
||
|
||
// return latest heading estimate
|
||
heading_rate_degs = degrees(_estimate_heading_rate_rads);
|
||
return true;
|
||
}
|
||
|
||
|
||
//==============================================================================
|
||
// MAVLink Message Handling
|
||
//==============================================================================
|
||
|
||
// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
|
||
void AP_Follow::handle_msg(const mavlink_message_t &msg)
|
||
{
|
||
// the LUA accessors read this state from the scripting thread
|
||
WITH_SEMAPHORE(_follow_sem);
|
||
|
||
// FOLL_SYSID no longer matches the system that supplied the data we hold. Forget the old
|
||
// target's message type and update time. This must run before the switch below, which
|
||
// consults _using_follow_target to decide whether GLOBAL_POSITION_INT is still wanted.
|
||
if (_sysid != _sysid_of_data) {
|
||
_using_follow_target = false;
|
||
_last_location_update_ms = 0; // 0 = nothing heard from this target
|
||
}
|
||
|
||
// Invalidate the estimate if no position update has been received within the timeout period.
|
||
if ((_last_location_update_ms == 0) ||
|
||
(AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_ESTIMATE_TIMEOUT_MS)) {
|
||
_estimate_valid = false; // mark estimate as invalid
|
||
_using_follow_target = false; // reset follow-target usage flag
|
||
}
|
||
|
||
if (!should_handle_message(msg)) {
|
||
// ignore message if filtering rules reject it (e.g., wrong sysid)
|
||
return;
|
||
}
|
||
|
||
// decode MAVLink message
|
||
bool updated = false;
|
||
|
||
switch (msg.msgid) {
|
||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
|
||
// if we are using follow_target, ignore global_position_int messages
|
||
if (_using_follow_target) {
|
||
return;
|
||
}
|
||
// handle standard global position messages
|
||
updated = handle_global_position_int_message(msg);
|
||
break;
|
||
}
|
||
case MAVLINK_MSG_ID_FOLLOW_TARGET: {
|
||
// handle follow-target specific messages
|
||
updated = handle_follow_target_message(msg);
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (updated) {
|
||
// reset the estimate on a large error or a change of source system
|
||
if (estimate_error_too_large() || (_sysid_of_data != msg.sysid)) {
|
||
_estimate_valid = false;
|
||
}
|
||
|
||
// record the system that supplied this update. this is msg.sysid rather than
|
||
// _sysid because msg.sysid is the value should_handle_message() validated, and
|
||
// _sysid can be written by the scripting thread part way through this function
|
||
_sysid_of_data = msg.sysid;
|
||
|
||
#if HAL_LOGGING_ENABLED
|
||
// log current follow diagnostic data
|
||
Log_Write_FOLL();
|
||
#endif
|
||
}
|
||
}
|
||
|
||
// Returns true if the incoming MAVLink message should be processed for target updates.
|
||
bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
|
||
{
|
||
// exit immediately if not enabled
|
||
if (!_enabled) {
|
||
return false;
|
||
}
|
||
|
||
// sysid 0 is the broadcast system and is never a valid target. without this
|
||
// a sender broadcasting as sysid 0 would compare equal to an unset _sysid
|
||
if (msg.sysid == 0) {
|
||
return false;
|
||
}
|
||
|
||
// skip our own messages
|
||
if (msg.sysid == mavlink_system.sysid) {
|
||
return false;
|
||
}
|
||
|
||
// skip message if not from our target. a _sysid of zero means no target
|
||
// has been selected, so no message is ever accepted
|
||
if (msg.sysid != _sysid) {
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// Checks whether the current estimate should be reset based on position and velocity errors.
|
||
bool AP_Follow::estimate_error_too_large() const
|
||
{
|
||
const float timeout_sec = _timeout;
|
||
|
||
// Calculate position thresholds based on maximum acceleration then deceleration for the timeout duration
|
||
const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(timeout_sec * 0.5);
|
||
const float pos_thresh_vert_m = _accel_max_d_mss.get() * sq(timeout_sec * 0.5);
|
||
|
||
// Calculate velocity thresholds using the new helper function
|
||
const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
|
||
const float vel_thresh_vert_ms = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
|
||
|
||
// Calculate current position and velocity errors
|
||
const Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
|
||
const Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
|
||
|
||
const Vector2f pos_error_xy = pos_error.xy();
|
||
const float pos_error_z = pos_error.z;
|
||
const Vector2f vel_error_xy = vel_error.xy();
|
||
const float vel_error_z = vel_error.z;
|
||
|
||
// Check horizontal and vertical separately
|
||
const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
|
||
const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
|
||
const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
|
||
const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
|
||
|
||
return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
|
||
}
|
||
|
||
// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
|
||
float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
|
||
{
|
||
// a non-positive jerk limit means "no jerk limit": acceleration can be
|
||
// applied instantly, so the profile is a pure trapezoid. Guarding here also
|
||
// avoids a divide-by-zero when the FOLL_JERK_* parameter is set to zero.
|
||
if (!is_positive(jerk_max)) {
|
||
return accel_max * timeout_sec;
|
||
}
|
||
const float t_jerk = accel_max / jerk_max;
|
||
const float t_total_jerk = 2.0f * t_jerk;
|
||
|
||
if (timeout_sec >= t_total_jerk) {
|
||
// time to ramp up, constant accel phase, and ramp down
|
||
const float t_const = timeout_sec - t_total_jerk;
|
||
const float delta_v_jerk = 0.5f * accel_max * t_jerk;
|
||
const float delta_v_const = accel_max * t_const;
|
||
return 2.0f * delta_v_jerk + delta_v_const;
|
||
} else {
|
||
// timeout too short: pure triangle profile
|
||
const float t_half = timeout_sec * 0.5f;
|
||
return jerk_max * sq(t_half);
|
||
}
|
||
}
|
||
|
||
// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
|
||
bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
|
||
{
|
||
// decode GLOBAL_POSITION_INT message into packet struct
|
||
mavlink_global_position_int_t packet;
|
||
mavlink_msg_global_position_int_decode(&msg, &packet);
|
||
|
||
// ignore message if latitude and longitude are exactly zero (invalid GPS fix)
|
||
if ((packet.lat == 0 && packet.lon == 0)) {
|
||
return false;
|
||
}
|
||
|
||
Location target_location;
|
||
target_location.lat = packet.lat;
|
||
target_location.lng = packet.lon;
|
||
|
||
switch((Location::AltFrame)_alt_type) {
|
||
case Location::AltFrame::ABSOLUTE:
|
||
target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
|
||
break;
|
||
case Location::AltFrame::ABOVE_HOME:
|
||
target_location.set_alt_cm(packet.relative_alt * 0.1, Location::AltFrame::ABOVE_HOME);
|
||
break;
|
||
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduCopter)
|
||
case Location::AltFrame::ABOVE_TERRAIN:
|
||
/// Altitude comes in as AMSL
|
||
target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
|
||
// convert the incoming altitude to terrain altitude, but fail if there is no terrain data available
|
||
if (!target_location.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
|
||
return false;
|
||
};
|
||
break;
|
||
#endif
|
||
default:
|
||
// don't process the packet if the _alt_type is invalid
|
||
return false;
|
||
}
|
||
|
||
// convert global location to local NED frame position
|
||
Vector3p target_pos_neu_m;
|
||
if (!target_location.get_vector_from_origin_NEU_m(target_pos_neu_m)) {
|
||
return false;
|
||
}
|
||
|
||
if (packet.hdg <= 36000) {
|
||
// valid heading field available (in centi-degrees)
|
||
_target_heading_deg = packet.hdg * 0.01f;
|
||
} else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
|
||
// heading missing but relative offset mode requires heading -> reject
|
||
return false;
|
||
} else {
|
||
// no heading available: set heading rate to zero
|
||
_target_heading_rate_degs = 0.0f;
|
||
}
|
||
|
||
_target_pos_ned_m.xy() = target_pos_neu_m.xy();
|
||
_target_pos_ned_m.z = -target_pos_neu_m.z;
|
||
|
||
// decode target velocity components (cm/s converted to m/s)
|
||
_target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
|
||
_target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
|
||
_target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
|
||
|
||
// target acceleration not available in GLOBAL_POSITION_INT
|
||
_target_accel_ned_mss.zero();
|
||
|
||
// apply jitter-corrected timestamp to this update
|
||
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
|
||
|
||
return true;
|
||
}
|
||
|
||
// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
|
||
bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
|
||
{
|
||
// decode FOLLOW_TARGET message into packet struct
|
||
mavlink_follow_target_t packet;
|
||
mavlink_msg_follow_target_decode(&msg, &packet);
|
||
|
||
// ignore message if latitude and longitude are exactly zero (invalid GPS fix)
|
||
if ((packet.lat == 0 && packet.lon == 0)) {
|
||
return false;
|
||
}
|
||
|
||
// require that at least position is estimated (bit 0 of est_capabilities)
|
||
if ((packet.est_capabilities & (1<<0)) == 0) {
|
||
return false;
|
||
}
|
||
|
||
// build Location object from latitude, longitude, and altitude (alt in meters)
|
||
const Location target_location {
|
||
packet.lat,
|
||
packet.lon,
|
||
int32_t(packet.alt * 100), // convert meters to centimeters
|
||
Location::AltFrame::ABSOLUTE
|
||
};
|
||
|
||
// convert global location to local NED frame position
|
||
Vector3p target_pos_neu_m;
|
||
if (!target_location.get_vector_from_origin_NEU_m(target_pos_neu_m)) {
|
||
return false;
|
||
}
|
||
|
||
// adjust Z coordinate to NED frame (NEU altitude -> NED)
|
||
Location origin;
|
||
if (!AP::ahrs().get_origin(origin)) {
|
||
return false;
|
||
}
|
||
|
||
// decode attitude if available (bit 3 of est_capabilities)
|
||
if (packet.est_capabilities & (1 << 3)) {
|
||
// reconstruct quaternion from packet
|
||
Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
|
||
float roll, pitch, yaw;
|
||
q.to_euler(roll, pitch, yaw);
|
||
|
||
// store heading in degrees, wrapped 0–360
|
||
_target_heading_deg = wrap_360(degrees(yaw));
|
||
|
||
// transform body rates (roll, pitch, yaw) to earth frame rates
|
||
Matrix3f R;
|
||
q.rotation_matrix(R);
|
||
Vector3f omega_body = Vector3f{packet.rates[0], packet.rates[1], packet.rates[2]};
|
||
Vector3f omega_world = R * omega_body; // rotate rates into earth frame
|
||
|
||
// store heading rate (yaw rate in world frame) in degrees/sec
|
||
_target_heading_rate_degs = degrees(omega_world.z);
|
||
} else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
|
||
// if attitude unavailable and using relative frame, cannot compute — abort
|
||
return false;
|
||
} else {
|
||
// otherwise, default heading rate to zero
|
||
_target_heading_rate_degs = 0.0f;
|
||
}
|
||
|
||
_target_pos_ned_m.xy() = target_pos_neu_m.xy();
|
||
_target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
|
||
|
||
// decode velocity if available (bit 1 of est_capabilities)
|
||
if (packet.est_capabilities & (1<<1)) {
|
||
_target_vel_ned_ms.x = packet.vel[0]; // velocity north
|
||
_target_vel_ned_ms.y = packet.vel[1]; // velocity east
|
||
_target_vel_ned_ms.z = packet.vel[2]; // velocity down
|
||
} else {
|
||
_target_vel_ned_ms.zero();
|
||
}
|
||
|
||
// decode acceleration if available (bit 2 of est_capabilities)
|
||
if (packet.est_capabilities & (1 << 2)) {
|
||
_target_accel_ned_mss.x = packet.acc[0]; // acceleration north
|
||
_target_accel_ned_mss.y = packet.acc[1]; // acceleration east
|
||
_target_accel_ned_mss.z = packet.acc[2]; // acceleration down
|
||
} else {
|
||
_target_accel_ned_mss.zero();
|
||
}
|
||
|
||
// apply jitter-corrected timestamp to this update
|
||
_last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
|
||
|
||
// prefer FOLLOW_TARGET over GLOBAL_POSITION_INT from now on
|
||
_using_follow_target = true;
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
//==============================================================================
|
||
// Offset Initialization and Adjustment Functions
|
||
//==============================================================================
|
||
|
||
// Initializes the positional offsets from the target vehicle if not already set.
|
||
void AP_Follow::init_offsets_if_required()
|
||
{
|
||
// return immediately if offsets have already been set
|
||
if (!_offset_m.get().is_zero()) {
|
||
return;
|
||
}
|
||
_offsets_were_zero = true;
|
||
|
||
if (!have_target()) {
|
||
return;
|
||
}
|
||
|
||
// Check if the target is within the maximum distance
|
||
Vector3p current_position_ned_m;
|
||
if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
|
||
return;
|
||
}
|
||
const Vector3f dist_vec_ned_m = (_target_pos_ned_m - current_position_ned_m).tofloat();
|
||
|
||
if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE)) {
|
||
// rotate offset into vehicle-relative frame based on heading
|
||
_offset_m.set(rotate_vector(-dist_vec_ned_m, -degrees(_estimate_heading_rad)));
|
||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Relative follow offset loaded");
|
||
} else {
|
||
// initialize offset in NED frame (no heading rotation)
|
||
_offset_m.set(-dist_vec_ned_m);
|
||
|
||
// ensure offset type is set to NED frame if initialized this way
|
||
_offset_type.set(AP_FOLLOW_OFFSET_TYPE_NED);
|
||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "N-E-D follow offset loaded");
|
||
}
|
||
}
|
||
|
||
// Rotates a 3D vector clockwise by the specified angle (degrees).
|
||
Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
|
||
{
|
||
// rotate roll, pitch input from north facing to vehicle's perspective
|
||
Vector3f ret = vec;
|
||
ret.xy().rotate(radians(angle_deg));
|
||
|
||
return ret;
|
||
}
|
||
|
||
// Resets follow mode offsets to zero if they were automatically initialized.
|
||
void AP_Follow::clear_offsets_if_required()
|
||
{
|
||
if (_offsets_were_zero) {
|
||
_offset_m.set(Vector3f());
|
||
_offsets_were_zero = false;
|
||
}
|
||
}
|
||
|
||
|
||
//==============================================================================
|
||
// Distance and Bearing Management
|
||
//==============================================================================
|
||
|
||
// Resets the recorded distance and bearing to the target to zero.
|
||
void AP_Follow::clear_dist_and_bearing_to_target()
|
||
{
|
||
_dist_to_target_m = 0.0f;
|
||
_bearing_to_target_deg = 0.0f;
|
||
}
|
||
|
||
// Updates the recorded distance and bearing to the target to zero.
|
||
void AP_Follow::update_dist_and_bearing_to_target()
|
||
{
|
||
Vector3p current_position_ned_m;
|
||
if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
|
||
// if unable to retrieve local position, clear distance/bearing info
|
||
clear_dist_and_bearing_to_target();
|
||
} else {
|
||
// get_relative_position_NED_origin() already returns metres in the NED
|
||
// frame, matching _ofs_estimate_pos_ned_m, so no conversion is required.
|
||
const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m;
|
||
|
||
// record distance and bearing to target for reporting/logging
|
||
if (ofs_dist_vec.xy().is_zero()) {
|
||
clear_dist_and_bearing_to_target();
|
||
} else {
|
||
_dist_to_target_m = ofs_dist_vec.xy().length();
|
||
_bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
//==============================================================================
|
||
// Logging
|
||
//==============================================================================
|
||
|
||
// Writes a diagnostic onboard log message containing target and vehicle tracking data for Follow mode.
|
||
#if HAL_LOGGING_ENABLED
|
||
void AP_Follow::Log_Write_FOLL()
|
||
{
|
||
// retrieve latest estimated location and velocity
|
||
Location loc_estimate{};
|
||
Vector3f vel_estimate;
|
||
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
|
||
|
||
Location target_location;
|
||
UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(target_location, _target_pos_ned_m));
|
||
|
||
// log the lead target's reported position and vehicle's estimated position
|
||
// @LoggerMessage: FOLL
|
||
// @Description: Follow library diagnostic data
|
||
// @Field: TimeUS: Time since system startup (microseconds)
|
||
// @Field: Lat: Target latitude (degrees * 1E7)
|
||
// @Field: Lon: Target longitude (degrees * 1E7)
|
||
// @Field: Alt: Target absolute altitude (centimeters)
|
||
// @Field: VelN: Target velocity, North (m/s)
|
||
// @Field: VelE: Target velocity, East (m/s)
|
||
// @Field: VelD: Target velocity, Down (m/s)
|
||
// @Field: LatE: Vehicle estimated latitude (degrees * 1E7)
|
||
// @Field: LonE: Vehicle estimated longitude (degrees * 1E7)
|
||
// @Field: AltE: Vehicle estimated altitude (centimeters)
|
||
// @Field: FrmE: Vehicle estimated altitude Frame
|
||
AP::logger().WriteStreaming("FOLL",
|
||
"TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE,FrmE", // labels
|
||
"sDUmnnnDUm-", // units
|
||
"F--B000--B-", // mults
|
||
"QLLifffLLib", // fmt
|
||
AP_HAL::micros64(),
|
||
target_location.lat,
|
||
target_location.lng,
|
||
target_location.alt,
|
||
(double)_target_vel_ned_ms.x,
|
||
(double)_target_vel_ned_ms.y,
|
||
(double)_target_vel_ned_ms.z,
|
||
loc_estimate.lat,
|
||
loc_estimate.lng,
|
||
loc_estimate.alt,
|
||
loc_estimate.get_alt_frame()
|
||
);
|
||
}
|
||
#endif // HAL_LOGGING_ENABLED
|
||
|
||
|
||
//==============================================================================
|
||
// Accessors and Helpers
|
||
//==============================================================================
|
||
|
||
// Returns true if the target data we hold is fresh and was supplied by the configured system.
|
||
// This is the gate for update_estimates(). It deliberately excludes _estimate_valid so that
|
||
// update_estimates() can rebuild the estimate after that flag has been cleared.
|
||
bool AP_Follow::have_target_data(void) const
|
||
{
|
||
if (!_enabled) {
|
||
return false;
|
||
}
|
||
|
||
// no target system has been configured
|
||
if (_sysid == 0) {
|
||
return false;
|
||
}
|
||
|
||
// we have not yet accepted an update from the configured system
|
||
if (_sysid_of_data != _sysid) {
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// Returns true if a usable estimate of the configured target is available.
|
||
// Every accessor is gated on this, but a true return does not guarantee that they succeed.
|
||
// The location getters can still fail on the AHRS origin lookup or on the FOLL_ALT_TYPE frame
|
||
// conversion, and the target state can change between this call and the accessor call, so
|
||
// callers must check the value every accessor returns.
|
||
bool AP_Follow::have_target(void) const
|
||
{
|
||
return _estimate_valid && have_target_data();
|
||
}
|
||
|
||
//==============================================================================
|
||
// AP_Follow Accessor
|
||
//==============================================================================
|
||
|
||
// Accessor for the AP_Follow singleton instance.
|
||
namespace AP {
|
||
AP_Follow &follow()
|
||
{
|
||
return *AP_Follow::get_singleton();
|
||
}
|
||
}
|
||
|
||
#endif // AP_FOLLOW_ENABLED
|