mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-21 20:53:52 +08:00
Peter Barker
f769ef7d17
this removes the dual-use of the _alt_type parameter. This parameter was changing both which input we accepted from the other vehicle (eg. GLOBAL_POSITION_INT.alt vs GLOBAL_POSITION_INT.relative_alt, but it was also specifying the return frame for the Location object returned from various methods. This was somewhat confusing and unneccesary.
264 lines
13 KiB
C++
264 lines
13 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/>.
|
||
*/
|
||
#pragma once
|
||
#include "AP_Follow_config.h"
|
||
|
||
#if AP_FOLLOW_ENABLED
|
||
|
||
#include <AP_Common/AP_Common.h>
|
||
#include <AP_Common/Location.h>
|
||
#include <AP_HAL/AP_HAL.h>
|
||
#include <AP_Param/AP_Param.h>
|
||
#include <AP_Math/AP_Math.h>
|
||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||
#include <AC_PID/AC_P.h>
|
||
#include <AP_RTC/JitterCorrection.h>
|
||
|
||
//==============================================================================
|
||
// AP_Follow Class
|
||
// Target Following Logic for ArduPilot Vehicles
|
||
//==============================================================================
|
||
|
||
class AP_Follow
|
||
{
|
||
|
||
public:
|
||
//==========================================================================
|
||
// Public Enums
|
||
//==========================================================================
|
||
|
||
// enum for FOLLOW_OPTIONS parameter
|
||
enum class Option {
|
||
MOUNT_FOLLOW_ON_ENTER = 1
|
||
};
|
||
|
||
// enum for YAW_BEHAVE parameter
|
||
enum YawBehave {
|
||
YAW_BEHAVE_NONE = 0,
|
||
YAW_BEHAVE_FACE_LEAD_VEHICLE = 1,
|
||
YAW_BEHAVE_SAME_AS_LEAD_VEHICLE = 2,
|
||
YAW_BEHAVE_DIR_OF_FLIGHT = 3
|
||
};
|
||
|
||
//==========================================================================
|
||
// Constructor and Singleton Access
|
||
//==========================================================================
|
||
|
||
// constructor
|
||
AP_Follow();
|
||
|
||
// enable as singleton
|
||
static AP_Follow *get_singleton(void) {
|
||
return _singleton;
|
||
}
|
||
|
||
// returns true if library is enabled
|
||
bool enabled() const { return _enabled; }
|
||
|
||
// set which target to follow
|
||
void set_target_sysid(uint8_t sysid) { _sysid.set(sysid); }
|
||
|
||
// Resets the follow mode offsets to zero if they were automatically initialized. Should be called when exiting Follow mode.
|
||
void clear_offsets_if_required();
|
||
|
||
//==========================================================================
|
||
// Target Estimation and Tracking Methods
|
||
//==========================================================================
|
||
|
||
// Returns true if following is enabled and a recent target location update has been received.
|
||
bool have_target() const;
|
||
|
||
// Projects the target’s position, velocity, and heading forward using the latest updates, smoothing with input shaping if necessary
|
||
void update_estimates();
|
||
|
||
// Retrieves the estimated target position, velocity, and acceleration in the NED frame relative to the origin (units: meters and meters/second).
|
||
bool get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss) const;
|
||
|
||
// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured positional offsets.
|
||
bool get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss) const;
|
||
|
||
// Retrieves the estimated target heading and heading rate in radians.
|
||
bool get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads) const;
|
||
|
||
//==========================================================================
|
||
// Global Location and Velocity Retrieval (LUA Bindings)
|
||
//==========================================================================
|
||
|
||
// Retrieves the estimated global location and velocity of the target
|
||
bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned);
|
||
|
||
// Retrieves the estimated global location and velocity of the target, including configured positional offsets (for LUA bindings).
|
||
bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned);
|
||
|
||
// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
|
||
bool get_target_heading_deg(float &heading);
|
||
|
||
// Retrieves the estimated target heading rate in degrees per second.
|
||
bool get_target_heading_rate_degs(float &_target_heading_rate_degs);
|
||
|
||
// Retrieves the distance vector to the target, the distance vector including configured offsets, and the target’s velocity in the NED frame (units: meters).
|
||
bool get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned);
|
||
|
||
//==========================================================================
|
||
// Accessor Methods
|
||
//==========================================================================
|
||
|
||
// get target sysid
|
||
uint8_t get_target_sysid() const { return _sysid.get(); }
|
||
|
||
// get position controller. this controller is not used within this library but it is convenient to hold it here
|
||
const AC_P& get_pos_p() const { return _p_pos; }
|
||
|
||
// get user defined yaw behaviour
|
||
YawBehave get_yaw_behave() const { return (YawBehave)_yaw_behave.get(); }
|
||
|
||
// initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required
|
||
void init_offsets_if_required();
|
||
|
||
//==========================================================================
|
||
// MAVLink Message Handling
|
||
//==========================================================================
|
||
|
||
// parse mavlink messages which may hold target's position, velocity and attitude
|
||
void handle_msg(const mavlink_message_t &msg);
|
||
|
||
//==========================================================================
|
||
// GCS Reporting Methods
|
||
//==========================================================================
|
||
|
||
// get horizontal distance to target (including offset) in meters (for reporting purposes)
|
||
float get_distance_to_target_m() const { return _dist_to_target_m; }
|
||
|
||
// get bearing to target (including offset) in degrees (for reporting purposes)
|
||
float get_bearing_to_target_deg() const { return _bearing_to_target_deg; }
|
||
|
||
// get system time of last position update
|
||
// LUA bindings
|
||
uint32_t get_last_update_ms() const { return _last_location_update_ms; }
|
||
|
||
// returns true if a follow option enabled
|
||
bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; }
|
||
|
||
//==========================================================================
|
||
// Parameter Group Info
|
||
//==========================================================================
|
||
|
||
// parameter list
|
||
static const struct AP_Param::GroupInfo var_info[];
|
||
|
||
private:
|
||
//==========================================================================
|
||
// Private Helper Functions and Singleton
|
||
//==========================================================================
|
||
|
||
// Singleton instance of the AP_Follow class.
|
||
static AP_Follow *_singleton;
|
||
|
||
// returns true if we should extract information from msg
|
||
bool should_handle_message(const mavlink_message_t &msg) const;
|
||
|
||
// Checks whether the current estimate should be reset based on position and velocity errors.
|
||
bool estimate_error_too_large() const;
|
||
|
||
// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
|
||
float calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const;
|
||
|
||
// Rotates a 3D vector clockwise by the specified angle in degrees.
|
||
Vector3f rotate_vector(const Vector3f &vec, float angle_deg) const;
|
||
|
||
// Resets the recorded distance and bearing to the target to zero.
|
||
void clear_dist_and_bearing_to_target();
|
||
void update_dist_and_bearing_to_target();
|
||
|
||
// handle various mavlink messages supplying position:
|
||
bool handle_global_position_int_message(const mavlink_message_t &msg);
|
||
bool handle_follow_target_message(const mavlink_message_t &msg);
|
||
|
||
// write out an onboard-log message to help diagnose follow problems:
|
||
void Log_Write_FOLL();
|
||
|
||
//==========================================================================
|
||
// Parameters
|
||
//==========================================================================
|
||
|
||
AP_Int8 _enabled; // 1 = Follow mode is enabled; 0 = disabled
|
||
AP_Int16 _sysid; // MAVLink system ID of the target (0 = auto-select first sender)
|
||
AP_Float _dist_max_m; // Maximum allowed distance to target in meters; if exceeded, estimation is rejected
|
||
AP_Int8 _offset_type; // Offset frame type: 0 = NED, 1 = relative to lead vehicle heading
|
||
AP_Vector3f _offset_m; // Offset from lead vehicle (meters), in NED or FRD frame depending on _offset_type
|
||
AP_Int8 _yaw_behave; // Yaw behavior mode (see YawBehave enum)
|
||
AP_Int8 _alt_type; // Altitude reference: 0 = absolute, 1 = relative to home
|
||
AC_P _p_pos; // Position error P-controller for optional altitude following
|
||
AP_Int16 _options; // Bitmask of follow behavior options (e.g., mount follow, etc.)
|
||
|
||
AP_Float _accel_max_ne_mss; // Max horizontal acceleration for kinematic shaping (m/s²)
|
||
AP_Float _jerk_max_ne_msss; // Max horizontal jerk for kinematic shaping (m/s³)
|
||
AP_Float _accel_max_d_mss; // Max vertical acceleration for kinematic shaping (m/s²)
|
||
AP_Float _jerk_max_d_msss; // Max vertical jerk for kinematic shaping (m/s³)
|
||
AP_Float _accel_max_h_degss; // Max angular acceleration for heading shaping (deg/s²)
|
||
AP_Float _jerk_max_h_degsss; // Max angular jerk for heading shaping (deg/s³)
|
||
|
||
//==========================================================================
|
||
// Internal State Variables
|
||
//==========================================================================
|
||
|
||
uint32_t _last_location_update_ms; // Time of last target position update (ms)
|
||
uint32_t _last_estimation_update_ms; // Time of last estimate update (ms)
|
||
|
||
Vector3p _target_pos_ned_m; // Latest received target position (NED frame, meters)
|
||
Vector3f _target_vel_ned_ms; // Latest received target velocity (NED frame, m/s)
|
||
Vector3f _target_accel_ned_mss; // Latest received target acceleration (NED frame, m/s²)
|
||
float _target_heading_deg; // Latest received target heading (degrees, 0 = North)
|
||
float _target_heading_rate_degs; // Latest received target yaw rate (deg/s)
|
||
|
||
bool _estimate_valid; // True if internal estimate is valid and in sync with updates
|
||
Vector3p _estimate_pos_ned_m; // Estimated target position (NED frame, meters)
|
||
Vector3f _estimate_vel_ned_ms; // Estimated target velocity (NED frame, m/s)
|
||
Vector3f _estimate_accel_ned_mss; // Estimated target acceleration (NED frame, m/s²)
|
||
float _estimate_heading_rad; // Estimated heading (radians, range -PI to PI)
|
||
float _estimate_heading_rate_rads; // Estimated heading rate (rad/s)
|
||
float _estimate_heading_accel_radss; // Estimated heading acceleration (rad/s²)
|
||
|
||
Vector3p _ofs_estimate_pos_ned_m; // Estimated position with offsets applied (NED frame)
|
||
Vector3f _ofs_estimate_vel_ned_ms; // Estimated velocity with offsets applied (NED frame)
|
||
Vector3f _ofs_estimate_accel_ned_mss; // Estimated acceleration with offsets applied (NED frame)
|
||
|
||
bool _automatic_sysid; // True if target sysid was automatically selected
|
||
int16_t _sysid_used; // Currently active sysid used for updates
|
||
float _dist_to_target_m; // Horizontal distance to target, for reporting (meters)
|
||
float _bearing_to_target_deg; // Bearing to target from vehicle (degrees, 0 = North)
|
||
bool _offsets_were_zero; // True if initial offset was zero before being initialized
|
||
bool _using_follow_target; // True if FOLLOW_TARGET messages are being used instead of GLOBAL_POSITION_INT
|
||
|
||
HAL_Semaphore _follow_sem; // semaphore for multi-thread use of update_estimates and LUA calls
|
||
|
||
//==========================================================================
|
||
// Utilities
|
||
//==========================================================================
|
||
|
||
// Jitter correction helper for smoothing offboard timestamps.
|
||
JitterCorrection _jitter{500};
|
||
};
|
||
|
||
//==============================================================================
|
||
// AP_Follow Accessor
|
||
//==============================================================================
|
||
|
||
namespace AP {
|
||
AP_Follow &follow();
|
||
};
|
||
|
||
#endif
|