mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-07 22:32:34 +08:00
808 lines
44 KiB
C++
808 lines
44 KiB
C++
#pragma once
|
||
|
||
#include <AP_Common/AP_Common.h>
|
||
#include <AP_Param/AP_Param.h>
|
||
#include <AP_Math/AP_Math.h>
|
||
#include <AC_PID/AC_P.h> // P library
|
||
#include <AC_PID/AC_PID.h> // PID library
|
||
#include <AC_PID/AC_P_1D.h> // P library (1-axis)
|
||
#include <AC_PID/AC_P_2D.h> // P library (2-axis)
|
||
#include <AC_PID/AC_PI_2D.h> // PI library (2-axis)
|
||
#include <AC_PID/AC_PID_Basic.h> // PID library (1-axis)
|
||
#include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
|
||
#include <AP_Scripting/AP_Scripting_config.h>
|
||
#include "AC_AttitudeControl.h" // Attitude control library
|
||
|
||
#include <AP_Logger/LogStructure.h>
|
||
|
||
// position controller default definitions
|
||
#define POSCONTROL_ACCEL_NE_MSS 1.0f // default horizontal acceleration in m/s². This is overwritten by waypoint and loiter controllers
|
||
#define POSCONTROL_JERK_NE_MSSS 5.0f // default horizontal jerk m/s³
|
||
|
||
#define POSCONTROL_STOPPING_DIST_UP_MAX_M 3.0f // max stopping distance (in m) vertically while climbing
|
||
#define POSCONTROL_STOPPING_DIST_DOWN_MAX_M 2.0f // max stopping distance (in m) vertically while descending
|
||
|
||
#define POSCONTROL_SPEED_MS 5.0f // default horizontal speed in m/s
|
||
#define POSCONTROL_SPEED_DOWN_MS 1.5f // default descent rate in m/s
|
||
#define POSCONTROL_SPEED_UP_MS 2.5f // default climb rate in m/s
|
||
|
||
#define POSCONTROL_ACCEL_D_MSS 2.5f // default vertical acceleration in m/s²
|
||
#define POSCONTROL_JERK_D_MSSS 5.0f // default vertical jerk m/s³
|
||
|
||
#define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz)
|
||
|
||
#define POSCONTROL_OVERSPEED_GAIN_U 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
|
||
|
||
#define POSCONTROL_RELAX_TC 0.16f // This is used to decay the I term to 5% in half a second
|
||
|
||
class AC_PosControl
|
||
{
|
||
public:
|
||
|
||
/// Constructor
|
||
AC_PosControl(AP_AHRS_View& ahrs, const class AP_Motors& motors, AC_AttitudeControl& attitude_control);
|
||
|
||
// do not allow copying
|
||
CLASS_NO_COPY(AC_PosControl);
|
||
|
||
// Sets the timestep between controller updates (seconds).
|
||
// This should match the IMU sample time used by the controller.
|
||
void set_dt_s(float dt) { _dt_s = dt; }
|
||
|
||
// Returns the timestep used in the controller update (seconds).
|
||
float get_dt_s() const { return _dt_s; }
|
||
|
||
// Updates internal NED position and velocity estimates from AHRS.
|
||
// Falls back to vertical-only data if horizontal velocity or position is invalid or vibration forces it.
|
||
// When high_vibes is true, forces use of vertical fallback for velocity.
|
||
void update_estimates(bool high_vibes = false);
|
||
|
||
// Returns the jerk limit for horizontal path shaping in m/s³.
|
||
// Used to constrain acceleration changes in trajectory generation.
|
||
float get_shaping_jerk_NE_msss() const { return _shaping_jerk_ne_msss; }
|
||
|
||
|
||
///
|
||
/// 3D position shaper
|
||
///
|
||
|
||
// Sets a new NED position target in meters and computes a jerk-limited trajectory.
|
||
// Updates internal acceleration commands using a smooth kinematic path constrained
|
||
// by configured acceleration and jerk limits.
|
||
// The path can be offset vertically to follow the terrain by providing the current
|
||
// terrain level in the NED frame and the terrain margin. Terrain margin is used to
|
||
// constrain horizontal velocity to avoid vertical buffer violation.
|
||
void input_pos_NED_m(const Vector3p& pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m);
|
||
|
||
// Returns a scaling factor for horizontal velocity in m/s to ensure
|
||
// the vertical controller maintains a safe distance above terrain.
|
||
float terrain_scaler_D_m(float pos_terrain_d_m, float terrain_margin_m) const;
|
||
|
||
///
|
||
/// Lateral position controller
|
||
///
|
||
|
||
// Sets maximum horizontal speed (cm/s) and acceleration (cm/s²) for NE-axis shaping.
|
||
// Can be called anytime; transitions are handled smoothly.
|
||
// All arguments should be positive.
|
||
// See NE_set_max_speed_accel_m() for full details.
|
||
void NE_set_max_speed_accel_cm(float speed_cms, float accel_cmss);
|
||
|
||
// Sets maximum horizontal speed (m/s) and acceleration (m/s²) for NE-axis shaping.
|
||
// These values constrain the kinematic trajectory used by the lateral controller.
|
||
// All arguments should be positive.
|
||
void NE_set_max_speed_accel_m(float speed_ms, float accel_mss);
|
||
|
||
// Sets horizontal correction limits for velocity (cm/s) and acceleration (cm/s²).
|
||
// Should be called only during initialization to avoid control discontinuities.
|
||
// All arguments should be positive.
|
||
// See NE_set_correction_speed_accel_m() for full details.
|
||
void NE_set_correction_speed_accel_cm(float speed_cms, float accel_cmss);
|
||
|
||
// Sets horizontal correction limits for velocity (m/s) and acceleration (m/s²).
|
||
// These values constrain the PID correction path, not the desired trajectory.
|
||
// All arguments should be positive.
|
||
void NE_set_correction_speed_accel_m(float speed_ms, float accel_mss);
|
||
|
||
// Returns maximum horizontal speed in cm/s.
|
||
// See NE_get_max_speed_ms() for full details.
|
||
float NE_get_max_speed_cms() const { return NE_get_max_speed_ms() * 100.0; }
|
||
|
||
// Returns maximum horizontal speed in m/s used for shaping the trajectory.
|
||
float NE_get_max_speed_ms() const { return _vel_max_ne_ms; }
|
||
|
||
// Returns maximum horizontal acceleration in m/s² used for trajectory shaping.
|
||
float NE_get_max_accel_mss() const { return _accel_max_ne_mss; }
|
||
|
||
// Sets maximum allowed horizontal position error in meters.
|
||
// Used to constrain the output of the horizontal position P controller.
|
||
void NE_set_pos_error_max_m(float error_max_m) { _p_pos_ne_m.set_error_max(error_max_m); }
|
||
|
||
// Returns maximum allowed horizontal position error in meters.
|
||
float NE_get_pos_error_max_m() const { return _p_pos_ne_m.get_error_max(); }
|
||
|
||
// Initializes NE controller to a stationary stopping point with zero velocity and acceleration.
|
||
// Use when the expected trajectory begins at rest but the starting position is unspecified.
|
||
// The starting position can be retrieved with get_pos_target_NED_m().
|
||
void NE_init_controller_stopping_point();
|
||
|
||
// Smoothly decays NE acceleration over time to zero while maintaining current velocity and position.
|
||
// Reduces output acceleration by ~95% over 0.5 seconds to avoid abrupt transitions.
|
||
void NE_relax_velocity_controller();
|
||
|
||
// Softens NE controller for landing by reducing position error and suppressing I-term windup.
|
||
// Used to make descent behavior more stable near ground contact.
|
||
void NE_soften_for_landing();
|
||
|
||
// Fully initializes the NE controller with current position, velocity, acceleration, and attitude.
|
||
// Intended for normal startup when the full state is known.
|
||
// Private function shared by other NE initializers.
|
||
void NE_init_controller();
|
||
|
||
// Sets the desired NE-plane acceleration in m/s² using jerk-limited shaping.
|
||
// Smoothly transitions to the specified acceleration from current kinematic state.
|
||
// Constraints: max acceleration and jerk set via NE_set_max_speed_accel_m().
|
||
void input_accel_NE_m(const Vector2f& accel_ne_msss);
|
||
|
||
// Sets desired NE-plane velocity and acceleration (cm/s, cm/s²) using jerk-limited shaping.
|
||
// See input_vel_accel_NE_m() for full details.
|
||
void input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output = true);
|
||
|
||
// Sets desired NE-plane velocity and acceleration (m/s, m/s²) using jerk-limited shaping.
|
||
// Calculates target acceleration using current kinematics constrained by acceleration and jerk limits.
|
||
// If `limit_output` is true, applies limits to total command (desired + correction).
|
||
void input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true);
|
||
|
||
// Sets desired NE position, velocity, and acceleration (cm, cm/s, cm/s²) with jerk-limited shaping.
|
||
// See input_pos_vel_accel_NE_m() for full details.
|
||
void input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output = true);
|
||
|
||
// Sets desired NE position, velocity, and acceleration (m, m/s, m/s²) with jerk-limited shaping.
|
||
// Calculates acceleration trajectory based on current kinematics and constraints.
|
||
// If `limit_output` is true, limits apply to full command (desired + correction).
|
||
void input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true);
|
||
|
||
// Returns true if the NE position controller has run in the last 5 control loop cycles.
|
||
bool NE_is_active() const;
|
||
|
||
// Disables NE position correction by setting the target position to the current position.
|
||
// Useful to freeze positional control without disrupting velocity control.
|
||
void NE_stop_pos_stabilisation();
|
||
|
||
// Disables NE position and velocity correction by setting target values to current state.
|
||
// Useful to prevent further corrections and freeze motion stabilization in NE axes.
|
||
void NE_stop_vel_stabilisation();
|
||
|
||
// Applies a scalar multiplier to the NE control loop.
|
||
// Set to 0 to disable lateral control; 1 for full authority.
|
||
void NE_set_control_scale_factor(float ne_control_scale_factor) {
|
||
_ne_control_scale_factor = ne_control_scale_factor;
|
||
}
|
||
|
||
// Runs the NE-axis position controller, computing output acceleration from position and velocity errors.
|
||
// Uses P and PID controllers to generate corrections which are added to feedforward velocity/acceleration.
|
||
// Requires all desired targets to be pre-set using the input_* or set_* methods.
|
||
void NE_update_controller();
|
||
|
||
///
|
||
/// Vertical position controller
|
||
///
|
||
|
||
// Sets maximum climb/descent rate (cm/s) and vertical acceleration (cm/s²) for the U-axis.
|
||
// Descent rate may be positive or negative and is always interpreted as a descent.
|
||
// See D_set_max_speed_accel_m() for full details.
|
||
// All values must be positive.
|
||
void D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss);
|
||
|
||
// Sets maximum climb/descent rate (m/s) and vertical acceleration (m/s²) for the U-axis.
|
||
// These values are used for jerk-limited kinematic shaping of the vertical trajectory.
|
||
// All values must be positive.
|
||
void D_set_max_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss);
|
||
|
||
// Sets vertical correction velocity and acceleration limits (cm/s, cm/s²).
|
||
// Should only be called during initialization to avoid discontinuities.
|
||
// See D_set_correction_speed_accel_m() for full details.
|
||
// All values must be positive.
|
||
void D_set_correction_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss);
|
||
|
||
// Sets vertical correction velocity and acceleration limits (m/s, m/s²).
|
||
// These values constrain the correction output of the PID controller.
|
||
// All values must be positive.
|
||
void D_set_correction_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss);
|
||
|
||
// Returns maximum vertical acceleration in m/s² used for shaping the climb/descent trajectory.
|
||
float D_get_max_accel_mss() const { return _accel_max_d_mss; }
|
||
|
||
// Returns maximum allowed positive (upward) position error in meters.
|
||
float get_pos_error_up_m() const { return _p_pos_d_m.get_error_min(); }
|
||
|
||
// Returns maximum allowed negative (downward) position error in meters.
|
||
float get_pos_error_down_m() const { return _p_pos_d_m.get_error_max(); }
|
||
|
||
// Returns maximum climb rate in cm/s.
|
||
// See get_max_speed_up_ms() for full details.
|
||
float get_max_speed_up_cms() const { return get_max_speed_up_ms() * 100.0; }
|
||
|
||
// Returns maximum climb rate in m/s used for shaping the vertical trajectory.
|
||
float get_max_speed_up_ms() const { return _vel_max_up_ms; }
|
||
|
||
/// Returns maximum descent rate in m/s (zero or positive).
|
||
float get_max_speed_down_ms() const { return _vel_max_down_ms; }
|
||
|
||
// Initializes U-axis controller to current position, velocity, and acceleration, disallowing descent.
|
||
// Used for takeoff or hold scenarios where downward motion is prohibited.
|
||
void D_init_controller_no_descent();
|
||
|
||
// Initializes U-axis controller to a stationary stopping point with zero velocity and acceleration.
|
||
// Used when the trajectory starts at rest but the initial altitude is unspecified.
|
||
// The resulting position target can be retrieved with get_pos_target_NED_m().
|
||
void D_init_controller_stopping_point();
|
||
|
||
// Smoothly decays U-axis acceleration to zero over time while maintaining current vertical velocity.
|
||
// Reduces requested acceleration by ~95% every 0.5 seconds to avoid abrupt transitions.
|
||
// `throttle_setting` is used to determine whether to preserve positive acceleration in low-thrust cases.
|
||
void D_relax_controller(float throttle_setting);
|
||
|
||
// Fully initializes the U-axis controller with current position, velocity, acceleration, and attitude.
|
||
// Used during standard controller activation when full state is known.
|
||
// Private function shared by other vertical initializers.
|
||
void D_init_controller();
|
||
|
||
// Sets the desired vertical acceleration in m/s² using jerk-limited shaping.
|
||
// Smoothly transitions to the target acceleration from current kinematic state.
|
||
// Constraints: max acceleration and jerk set via D_set_max_speed_accel_m().
|
||
void input_accel_D_m(float accel_d_mss);
|
||
|
||
// Sets desired vertical velocity and acceleration (m/s, m/s²) using jerk-limited shaping.
|
||
// Calculates required acceleration using current vertical kinematics.
|
||
// If `limit_output` is true, limits apply to the combined (desired + correction) command.
|
||
void input_vel_accel_D_m(float &vel_d_ms, float accel_d_mss, bool limit_output = true);
|
||
|
||
// Generates a vertical trajectory using the given climb rate in cm/s and jerk-limited shaping.
|
||
// Adjusts the internal target altitude based on integrated climb rate.
|
||
// See D_set_pos_target_from_climb_rate_ms() for full details.
|
||
void D_set_pos_target_from_climb_rate_cms(float climb_rate_cms);
|
||
|
||
// Generates a vertical trajectory using the given climb rate in m/s and jerk-limited shaping.
|
||
// Target altitude is updated over time by integrating the climb rate.
|
||
void D_set_pos_target_from_climb_rate_ms(float climb_rate_ms, bool ignore_descent_limit = false);
|
||
|
||
// Sets vertical position, velocity, and acceleration in cm using jerk-limited shaping.
|
||
// See input_pos_vel_accel_D_m() for full details.
|
||
void input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output = true);
|
||
|
||
// Sets vertical position, velocity, and acceleration in meters using jerk-limited shaping.
|
||
// Calculates required acceleration using current state and constraints.
|
||
// If `limit_output` is true, limits are applied to combined (desired + correction) command.
|
||
void input_pos_vel_accel_D_m(float &pos_d_m, float &vel_d_ms, float accel_d_mss, bool limit_output = true);
|
||
|
||
// Sets target altitude in cm using jerk-limited shaping to gradually move to the new position.
|
||
// See D_set_alt_target_with_slew_m() for full details.
|
||
void set_alt_target_with_slew_cm(float pos_u_cm);
|
||
|
||
// Sets target altitude in meters using jerk-limited shaping.
|
||
void D_set_alt_target_with_slew_m(float pos_u_m);
|
||
|
||
// Returns true if the U-axis controller has run in the last 5 control loop cycles.
|
||
bool D_is_active() const;
|
||
|
||
// Runs the vertical (U-axis) position controller.
|
||
// Computes output acceleration based on position and velocity errors using PID correction.
|
||
// Feedforward velocity and acceleration are combined with corrections to produce a smooth vertical command.
|
||
// Desired position, velocity, and acceleration must be set before calling.
|
||
void D_update_controller();
|
||
|
||
|
||
|
||
///
|
||
/// Accessors
|
||
///
|
||
|
||
// Sets externally computed NED position, velocity, and acceleration in meters, m/s, and m/s².
|
||
// Use when path planning or shaping is done outside this controller.
|
||
void set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss);
|
||
|
||
// Sets externally computed NE position, velocity, and acceleration in meters, m/s, and m/s².
|
||
// Use when path planning or shaping is done outside this controller.
|
||
void set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss);
|
||
|
||
|
||
/// Position
|
||
|
||
// Returns the estimated position in NED frame, in meters relative to EKF origin.
|
||
const Vector3p& get_pos_estimate_NED_m() const { return _pos_estimate_ned_m; }
|
||
|
||
// Returns estimated altitude above EKF origin in meters.
|
||
float get_pos_estimate_U_m() const { return -_pos_estimate_ned_m.z; }
|
||
|
||
// Returns the target position in NED frame, in meters relative to EKF origin.
|
||
const Vector3p& get_pos_target_NED_m() const { return _pos_target_ned_m; }
|
||
|
||
// Sets the desired NE position in meters relative to EKF origin.
|
||
void set_pos_desired_NE_m(const Vector2p& pos_desired_ne_m) { _pos_desired_ned_m.xy() = pos_desired_ne_m; }
|
||
|
||
// Returns the desired position in NED frame, in meters relative to EKF origin.
|
||
const Vector3p& get_pos_desired_NED_m() const { return _pos_desired_ned_m; }
|
||
|
||
// Returns target altitude above EKF origin in centimeters.
|
||
// See get_pos_target_U_m() for full details.
|
||
float get_pos_target_U_cm() const { return get_pos_target_U_m() * 100.0; }
|
||
|
||
// Returns target altitude above EKF origin in meters.
|
||
float get_pos_target_U_m() const { return -_pos_target_ned_m.z; }
|
||
|
||
// Sets desired altitude above EKF origin in centimeters.
|
||
// See set_pos_desired_U_m() for full details.
|
||
void set_pos_desired_U_cm(float pos_desired_u_cm) { set_pos_desired_U_m(pos_desired_u_cm * 0.01); }
|
||
|
||
// Sets desired altitude above EKF origin in meters.
|
||
void set_pos_desired_U_m(float pos_desired_u_m) { _pos_desired_ned_m.z = -pos_desired_u_m; }
|
||
|
||
// Returns desired altitude above EKF origin in centimeters.
|
||
// See get_pos_desired_U_m() for full details.
|
||
float get_pos_desired_U_cm() const { return get_pos_desired_U_m() * 100.0; }
|
||
|
||
// Returns desired altitude above EKF origin in meters.
|
||
float get_pos_desired_U_m() const { return -_pos_desired_ned_m.z; }
|
||
|
||
|
||
/// Stopping Point
|
||
|
||
// Computes NE stopping point in meters based on current position, velocity, and acceleration.
|
||
void get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const;
|
||
|
||
// Computes vertical stopping point relative to EKF origin in meters, Down-positive. based on current velocity and acceleration.
|
||
void get_stopping_point_D_m(postype_t &stopping_point_d_m) const;
|
||
|
||
|
||
/// Position Error
|
||
|
||
// Returns NED position error vector in meters between current and target positions.
|
||
const Vector3f get_pos_error_NED_m() const { return Vector3f{_p_pos_ne_m.get_error().x, _p_pos_ne_m.get_error().y, _p_pos_d_m.get_error()}; }
|
||
|
||
// Returns total NE-plane position error magnitude in meters.
|
||
float get_pos_error_NE_m() const { return _p_pos_ne_m.get_error().length(); }
|
||
|
||
// Returns vertical position error (altitude) in centimeters.
|
||
// See get_pos_error_D_m() for full details.
|
||
float get_pos_error_U_cm() const { return -get_pos_error_D_m() * 100.0; }
|
||
|
||
// Returns vertical position error (altitude) in meters.
|
||
float get_pos_error_D_m() const { return _p_pos_d_m.get_error(); }
|
||
|
||
|
||
/// Velocity
|
||
|
||
// Returns current velocity estimate in NED frame in m/s.
|
||
const Vector3f& get_vel_estimate_NED_ms() const { return _vel_estimate_ned_ms; }
|
||
|
||
// Returns current velocity estimate (Up) in m/s.
|
||
float get_vel_estimate_U_ms() const { return -_vel_estimate_ned_ms.z; }
|
||
|
||
// Sets desired velocity in NED frame in cm/s.
|
||
// See set_vel_desired_NED_ms() for full details.
|
||
void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { set_vel_desired_NED_ms(Vector3f{vel_desired_neu_cms.x, vel_desired_neu_cms.y, -vel_desired_neu_cms.z} * 0.01); }
|
||
|
||
// Sets desired velocity in NED frame in m/s.
|
||
void set_vel_desired_NED_ms(const Vector3f &vel_desired_ned_ms) { _vel_desired_ned_ms = vel_desired_ned_ms; }
|
||
|
||
// Sets desired horizontal (NE) velocity in m/s.
|
||
void set_vel_desired_NE_ms(const Vector2f &vel_desired_ne_ms) { _vel_desired_ned_ms.xy() = vel_desired_ne_ms; }
|
||
|
||
// Returns desired velocity in NEU frame in cm/s.
|
||
// See get_vel_desired_NED_ms() for full details.
|
||
const Vector3f get_vel_desired_NEU_cms() const { return Vector3f{_vel_desired_ned_ms.x, _vel_desired_ned_ms.y, -_vel_desired_ned_ms.z} * 100.0; }
|
||
|
||
// Returns desired velocity in NED frame in m/s.
|
||
const Vector3f& get_vel_desired_NED_ms() const { return _vel_desired_ned_ms; }
|
||
|
||
// Returns desired velocity (Up) in m/s.
|
||
float get_vel_desired_U_ms() const { return -_vel_desired_ned_ms.z; }
|
||
|
||
// Returns velocity target in NEU frame in cm/s.
|
||
// See get_vel_target_NED_ms() for full details.
|
||
const Vector3f get_vel_target_NEU_cms() const { return Vector3f{_vel_target_ned_ms.x, _vel_target_ned_ms.y, -_vel_target_ned_ms.z} * 100.0; }
|
||
|
||
// Returns velocity target in NED frame in m/s.
|
||
const Vector3f& get_vel_target_NED_ms() const { return _vel_target_ned_ms; }
|
||
|
||
// Sets desired vertical velocity (Up) in m/s.
|
||
void set_vel_desired_D_ms(float vel_desired_d_ms) { _vel_desired_ned_ms.z = vel_desired_d_ms; }
|
||
|
||
// Returns vertical velocity target (Up) in cm/s.
|
||
// See get_vel_target_U_ms() for full details.
|
||
float get_vel_target_U_cms() const { return get_vel_target_U_ms() * 100.0; }
|
||
|
||
// Returns vertical velocity target (Up) in m/s.
|
||
float get_vel_target_U_ms() const { return -_vel_target_ned_ms.z; }
|
||
|
||
|
||
/// Acceleration
|
||
|
||
// Sets desired horizontal acceleration in m/s².
|
||
void set_accel_desired_NE_mss(const Vector2f &accel_desired_ne_mss) { _accel_desired_ned_mss.xy() = accel_desired_ne_mss; }
|
||
|
||
// Returns target NED acceleration in m/s².
|
||
const Vector3f& get_accel_target_NED_mss() const { return _accel_target_ned_mss; }
|
||
|
||
|
||
/// Terrain
|
||
|
||
// Sets both the terrain altitude and terrain target to the same value
|
||
// (altitude above EKF origin in centimeters, Up-positive).
|
||
// See set_pos_terrain_target_D_m() for full description.
|
||
void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm) { set_pos_terrain_target_D_m(-pos_terrain_target_u_cm * 0.01); }
|
||
|
||
// Sets both the terrain altitude and terrain target to the same value
|
||
// (relative to EKF origin in meters, Down-positive).
|
||
void set_pos_terrain_target_D_m(float pos_terrain_target_d_m) { _pos_terrain_target_d_m = pos_terrain_target_d_m; }
|
||
|
||
// Initializes both the terrain altitude and terrain target to the same value
|
||
// (altitude above EKF origin in centimeters, Up-positive).
|
||
// See init_pos_terrain_D_m() for full description.
|
||
void init_pos_terrain_U_cm(float pos_terrain_u_cm);
|
||
|
||
// Initializes both the terrain altitude and terrain target to the same value
|
||
// (relative to EKF origin in meters, Down-positive).
|
||
void init_pos_terrain_D_m(float pos_terrain_d_m);
|
||
|
||
// Returns the current terrain altitude (Down-positive, relative to EKF origin, in meters).
|
||
float get_pos_terrain_D_m() const { return _pos_terrain_d_m; }
|
||
|
||
|
||
/// Offset
|
||
|
||
#if AP_SCRIPTING_ENABLED
|
||
// Sets additional position, velocity, and acceleration offsets in meters (NED frame) for scripting.
|
||
// Offsets are added to the controller’s internal target.
|
||
// Used in LUA
|
||
bool set_posvelaccel_offset(const Vector3f &pos_offset_NED_m, const Vector3f &vel_offset_NED_ms, const Vector3f &accel_offset_NED_mss);
|
||
|
||
// Retrieves current scripted offsets in meters (NED frame).
|
||
// Used in LUA
|
||
|
||
bool get_posvelaccel_offset(Vector3f &pos_offset_NED_m, Vector3f &vel_offset_NED_ms, Vector3f &accel_offset_NED_mss);
|
||
|
||
// Retrieves current target velocity (NED frame, m/s) including any scripted offset.
|
||
// Used in LUA
|
||
bool get_vel_target(Vector3f &vel_target_NED_ms);
|
||
|
||
// Retrieves current target acceleration (NED frame, m/s²) including any scripted offset.
|
||
// Used in LUA
|
||
bool get_accel_target(Vector3f &accel_target_NED_mss);
|
||
#endif
|
||
|
||
// Sets NE offset targets in meters, m/s, and m/s².
|
||
void set_posvelaccel_offset_target_NE_m(const Vector2p& pos_offset_target_ne_m, const Vector2f& vel_offset_target_ne_ms, const Vector2f& accel_offset_target_ne_mss);
|
||
|
||
// Sets vertical offset targets (m, m/s, m/s²) relative to EKF origin in meters, Down-positive.
|
||
void set_posvelaccel_offset_target_D_m(float pos_offset_target_d_m, float vel_offset_target_d_ms, float accel_offset_target_d_mss);
|
||
|
||
// Returns current NED position offset in meters.
|
||
const Vector3p& get_pos_offset_NED_m() const { return _pos_offset_ned_m; }
|
||
|
||
// Returns current NED velocity offset in m/s.
|
||
const Vector3f& get_vel_offset_NED_ms() const { return _vel_offset_ned_ms; }
|
||
|
||
// Returns current NED acceleration offset in m/s².
|
||
const Vector3f& get_accel_offset_NED_mss() const { return _accel_offset_ned_mss; }
|
||
|
||
// Sets vertical position offset in meters relative to EKF origin in meters, Down-positive.
|
||
void set_pos_offset_D_m(float pos_offset_d_m) { _pos_offset_ned_m.z = pos_offset_d_m; }
|
||
|
||
// Returns vertical position offset in meters relative to EKF origin in meters, Down-positive.
|
||
float get_pos_offset_U_m() const { return -_pos_offset_ned_m.z; }
|
||
|
||
// Returns vertical velocity offset in m/s.
|
||
float get_vel_offset_D_ms() const { return _vel_offset_ned_ms.z; }
|
||
|
||
// Returns vertical acceleration offset in m/s².
|
||
float get_accel_offset_D_mss() const { return _accel_offset_ned_mss.z; }
|
||
|
||
/// Outputs
|
||
|
||
// Returns desired roll angle in radians for the attitude controller
|
||
float get_roll_rad() const { return _roll_target_rad; }
|
||
|
||
// Returns desired pitch angle in radians for the attitude controller.
|
||
float get_pitch_rad() const { return _pitch_target_rad; }
|
||
|
||
// Returns desired yaw angle in radians for the attitude controller.
|
||
float get_yaw_rad() const { return _yaw_target_rad; }
|
||
|
||
// Returns desired yaw rate in radians/second for the attitude controller.
|
||
float get_yaw_rate_rads() const { return _yaw_rate_target_rads; }
|
||
|
||
// Returns desired roll angle in centidegrees for the attitude controller.
|
||
// See get_roll_rad() for full details.
|
||
float get_roll_cd() const { return rad_to_cd(_roll_target_rad); }
|
||
|
||
// Returns desired pitch angle in centidegrees for the attitude controller.
|
||
// See get_pitch_rad() for full details.
|
||
float get_pitch_cd() const { return rad_to_cd(_pitch_target_rad); }
|
||
|
||
// Returns desired thrust direction as a unit vector in the body frame.
|
||
Vector3f get_thrust_vector() const;
|
||
|
||
// Returns bearing from current position to position target in radians.
|
||
// 0 = North, positive = clockwise.
|
||
float get_bearing_to_target_rad() const;
|
||
|
||
// Returns the maximum allowed roll/pitch angle in radians.
|
||
float get_lean_angle_max_rad() const;
|
||
|
||
// Overrides the maximum allowed roll/pitch angle in radians.
|
||
// A value of 0 reverts to using the ANGLE_MAX parameter.
|
||
void set_lean_angle_max_rad(float angle_max_rad) { _angle_max_override_rad = angle_max_rad; }
|
||
|
||
// Overrides the maximum allowed roll/pitch angle in degrees.
|
||
// See set_lean_angle_max_rad() for full details.
|
||
void set_lean_angle_max_deg(const float angle_max_deg) { set_lean_angle_max_rad(radians(angle_max_deg)); }
|
||
|
||
// Overrides the maximum allowed roll/pitch angle in centidegrees.
|
||
// See set_lean_angle_max_rad() for full details.
|
||
void set_lean_angle_max_cd(const float angle_max_cd) { set_lean_angle_max_rad(cd_to_rad(angle_max_cd)); }
|
||
|
||
/// Other
|
||
|
||
// Returns reference to the NE position P controller.
|
||
AC_P_2D& NE_get_pos_p() { return _p_pos_ne_m; }
|
||
|
||
// Returns reference to the D (vertical) position P controller.
|
||
AC_P_1D& D_get_pos_p() { return _p_pos_d_m; }
|
||
|
||
// Returns reference to the NE velocity PID controller.
|
||
AC_PID_2D& NE_get_vel_pid() { return _pid_vel_ne_m; }
|
||
|
||
// Returns reference to the D (vertical) velocity PID controller.
|
||
AC_PID_Basic& D_get_vel_pid() { return _pid_vel_d_m; }
|
||
|
||
// Returns reference to the D acceleration PID controller.
|
||
AC_PID& D_get_accel_pid() { return _pid_accel_d_m; }
|
||
|
||
// Marks that NE acceleration has been externally limited.
|
||
// Prevents I-term windup by storing the current target direction.
|
||
void NE_set_externally_limited() { _limit_vector_ned.x = _accel_target_ned_mss.x; _limit_vector_ned.y = _accel_target_ned_mss.y; }
|
||
|
||
// Converts lean angles (rad) to NED acceleration in m/s².
|
||
Vector3f lean_angles_rad_to_accel_NED_mss(const Vector3f& att_target_euler_rad) const;
|
||
|
||
// Writes position controller diagnostic logs (PSCN, PSCE, etc).
|
||
void write_log();
|
||
|
||
// Performs pre-arm checks for position control parameters and EKF readiness.
|
||
// Returns false if failure_msg is populated.
|
||
bool pre_arm_checks(const char *param_prefix,
|
||
char *failure_msg,
|
||
const uint8_t failure_msg_len);
|
||
|
||
// Enables or disables vibration compensation mode.
|
||
// When enabled, disables use of horizontal velocity estimates.
|
||
void set_vibe_comp(bool on_off) { _vibe_comp_enabled = on_off; }
|
||
|
||
// Reset handling method
|
||
enum class EKFResetMethod : uint8_t {
|
||
MoveTarget = 0, // the target position is reset so the vehicle does not physically move
|
||
MoveVehicle = 1 // the target position is smoothly transitioned so the vehicle moves to its previous position coordinates
|
||
};
|
||
void set_reset_handling_method(EKFResetMethod reset_method) { _ekf_reset_method = reset_method; }
|
||
|
||
// Returns confidence (0–1) in vertical control authority based on output usage.
|
||
// Used to assess throttle margin and PID effectiveness.
|
||
float get_vel_D_control_ratio() const { return constrain_float(_vel_d_control_ratio, 0.0f, 1.0f); }
|
||
|
||
// Returns lateral distance to closest point on active trajectory in meters.
|
||
// Used to assess horizontal deviation from path.
|
||
float crosstrack_error_m() const;
|
||
|
||
// Resets NED position controller state to prevent transients when exiting standby.
|
||
// Zeros I-terms and aligns targets to current position.
|
||
void NED_standby_reset();
|
||
|
||
// Returns measured vertical (Down) acceleration in m/s² (Earth frame, gravity-compensated).
|
||
// Positive = downward acceleration.
|
||
float get_estimated_accel_D_mss() const { return _ahrs.get_accel_ef().z + GRAVITY_MSS; }
|
||
|
||
// Returns measured vertical (Up) acceleration in m/s² (Earth frame, gravity-compensated).
|
||
// Positive = upward acceleration.
|
||
float get_estimated_accel_U_mss() const { return -get_estimated_accel_D_mss(); }
|
||
|
||
// Returns true if the requested forward pitch is limited by the configured tilt constraint.
|
||
bool get_fwd_pitch_is_limited() const;
|
||
|
||
// Sets artificial NE position disturbance in meters.
|
||
void set_disturb_pos_NE_m(const Vector2f& disturb_pos_m) { _disturb_pos_ne_m = disturb_pos_m; }
|
||
|
||
// Sets artificial NE velocity disturbance in m/s.
|
||
void set_disturb_vel_NE_ms(const Vector2f& disturb_vel_ms) { _disturb_vel_ne_ms = disturb_vel_ms; }
|
||
|
||
// Logs position controller state along the North axis to PSCN..
|
||
// Logs desired, target, and actual position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss);
|
||
|
||
// Logs position controller state along the East axis to PSCE.
|
||
// Logs desired, target, and actual values for position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSCE(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss);
|
||
|
||
// Logs position controller state along the Down (vertical) axis to PSCD.
|
||
// Logs desired, target, and actual values for position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSCD(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss);
|
||
|
||
// Logs offset tracking along the North axis to PSON.
|
||
// Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSON(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
|
||
|
||
// Logs offset tracking along the East axis to PSOE.
|
||
// Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSOE(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
|
||
|
||
// Logs offset tracking along the Down axis to PSOD.
|
||
// Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSOD(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
|
||
|
||
// Logs terrain-following offset tracking along the Down axis to PSOT.
|
||
// Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
|
||
static void Write_PSOT(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
|
||
|
||
// perform any required parameter conversions
|
||
void convert_parameters();
|
||
|
||
// Returns pointer to the global AC_PosControl singleton.
|
||
static AC_PosControl *get_singleton(void) { return _singleton; }
|
||
|
||
static const struct AP_Param::GroupInfo var_info[];
|
||
|
||
protected:
|
||
|
||
// Calculates vertical throttle using vibration-resistant feedforward estimation.
|
||
// Returns throttle output using manual feedforward gain for vibration compensation mode.
|
||
// Integrator is adjusted using velocity error when PID is being overridden.
|
||
float get_throttle_with_vibration_override();
|
||
|
||
// Converts horizontal acceleration (m/s²) to roll/pitch lean angles in radians.
|
||
void accel_NE_mss_to_lean_angles_rad(float accel_n_mss, float accel_e_mss, float& roll_target_rad, float& pitch_target_rad) const;
|
||
|
||
// Converts current target lean angles to NE acceleration in m/s².
|
||
void lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel_e_mss) const;
|
||
|
||
// Computes desired yaw and yaw rate based on the NE acceleration and velocity vectors.
|
||
// Aligns yaw with the direction of travel if speed exceeds 5% of maximum.
|
||
void calculate_yaw_and_rate_yaw();
|
||
|
||
// Computes scaling factor to increase max vertical accel/jerk if vertical speed exceeds configured limits.
|
||
float calculate_overspeed_gain();
|
||
|
||
|
||
/// Terrain Following
|
||
|
||
// Initializes terrain position, velocity, and acceleration to match the terrain target.
|
||
void init_terrain();
|
||
|
||
// Updates terrain estimate (_pos_terrain_d_m) toward target using filter time constants.
|
||
void update_terrain();
|
||
|
||
|
||
/// Offsets
|
||
|
||
// Initializes NE position/velocity/acceleration offsets to match their respective targets.
|
||
void NE_init_offsets();
|
||
|
||
// Initializes vertical (D) offsets to match their respective targets.
|
||
void D_init_offsets();
|
||
|
||
// Updates NE offsets by gradually moving them toward their targets.
|
||
void NE_update_offsets();
|
||
|
||
// Updates vertical (D) offsets by gradually moving them toward their targets.
|
||
void D_update_offsets();
|
||
|
||
// Initializes tracking of NE EKF position resets.
|
||
void NE_init_ekf_reset();
|
||
|
||
// Handles NE position reset detection and response (e.g., clearing accumulated errors).
|
||
void NE_handle_ekf_reset();
|
||
|
||
// Initializes tracking of vertical (D) EKF resets.
|
||
void D_init_ekf_reset();
|
||
|
||
// Handles the vertical (D) EKF reset detection and response.
|
||
void D_handle_ekf_reset();
|
||
|
||
// references to inertial nav and ahrs libraries
|
||
AP_AHRS_View& _ahrs;
|
||
const class AP_Motors& _motors;
|
||
AC_AttitudeControl& _attitude_control;
|
||
|
||
// parameters
|
||
AP_Float _lean_angle_max_deg; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
|
||
AP_Float _shaping_jerk_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
|
||
AP_Float _shaping_jerk_d_msss; // Jerk limit of the u kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
|
||
AC_P_2D _p_pos_ne_m; // XY axis position controller to convert target distance (m) to target velocity (m/s)
|
||
AC_P_1D _p_pos_d_m; // Z axis position controller to convert target altitude (m) to target climb rate (m/s)
|
||
AC_PID_2D _pid_vel_ne_m; // XY axis velocity controller to convert target velocity (m/s) to target acceleration (m/s²)
|
||
AC_PID_Basic _pid_vel_d_m; // Z axis velocity controller to convert target climb rate (m/s) to target acceleration (m/s²)
|
||
AC_PID _pid_accel_d_m; // Z axis acceleration controller to convert target acceleration (in units of gravity) to normalised throttle output
|
||
|
||
// internal variables
|
||
float _dt_s; // time difference (in seconds) since the last loop time
|
||
uint32_t _last_update_ne_ticks; // ticks of last NE_update_controller call
|
||
uint32_t _last_update_d_ticks; // ticks of last update_z_controller call
|
||
float _vel_max_ne_ms; // max horizontal speed in m/s used for kinematic shaping
|
||
float _vel_max_up_ms; // max climb rate in m/s used for kinematic shaping
|
||
float _vel_max_down_ms; // max descent rate in m/s used for kinematic shaping
|
||
float _accel_max_ne_mss; // max horizontal acceleration in m/s² used for kinematic shaping
|
||
float _accel_max_d_mss; // max vertical acceleration in m/s² used for kinematic shaping
|
||
float _jerk_max_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
|
||
float _jerk_max_d_msss; // Jerk limit of the z kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
|
||
float _vel_d_control_ratio = 2.0f;// confidence that we have control in the vertical axis
|
||
Vector2f _disturb_pos_ne_m; // position disturbance generated by system ID mode
|
||
Vector2f _disturb_vel_ne_ms; // velocity disturbance generated by system ID mode
|
||
float _ne_control_scale_factor = 1.0; // single loop scale factor for XY control
|
||
|
||
// output from controller
|
||
float _roll_target_rad; // desired roll angle in radians calculated by position controller
|
||
float _pitch_target_rad; // desired roll pitch in radians calculated by position controller
|
||
float _yaw_target_rad; // desired yaw in radians calculated by position controller
|
||
float _yaw_rate_target_rads; // desired yaw rate in radians per second calculated by position controller
|
||
|
||
// position controller internal variables
|
||
Vector3p _pos_estimate_ned_m; // estimated location, frame NED in m relative to the EKF origin.
|
||
Vector3p _pos_desired_ned_m; // desired location, frame NED in m relative to the EKF origin. This is equal to the _pos_target_ned_m minus offsets
|
||
Vector3p _pos_target_ned_m; // target location, frame NED in m relative to the EKF origin. This is equal to the _pos_desired_ned_m plus offsets
|
||
Vector3f _vel_estimate_ned_ms; // estimated velocity in NED m/s
|
||
Vector3f _vel_desired_ned_ms; // desired velocity in NED m/s
|
||
Vector3f _vel_target_ned_ms; // velocity target in NED m/s calculated by pos_to_rate step
|
||
Vector3f _accel_desired_ned_mss; // desired acceleration in NED m/s² (feed forward)
|
||
Vector3f _accel_target_ned_mss; // acceleration target in NED m/s²
|
||
// todo: seperate the limit vector into ne and u. ne is based on acceleration while u is set +-1 based on throttle saturation. Together they don't form a direction vector because the units are different.
|
||
Vector3f _limit_vector_ned; // the direction that the position controller is limited, zero when not limited
|
||
|
||
// terrain handling variables
|
||
float _pos_terrain_target_d_m; // position terrain target in m relative to the EKF origin in NED frame
|
||
float _pos_terrain_d_m; // position terrain in m from the EKF origin in NED frame. This terrain moves towards _pos_terrain_target_d_m
|
||
float _vel_terrain_d_ms; // velocity terrain in NED m/s calculated by pos_to_rate step. This terrain moves towards _vel_terrain_target
|
||
float _accel_terrain_d_mss; // acceleration terrain in NED m/s²
|
||
|
||
// offset handling variables
|
||
Vector3p _pos_offset_target_ned_m; // position offset target in m relative to the EKF origin in NED frame
|
||
Vector3p _pos_offset_ned_m; // position offset in m from the EKF origin in NED frame. This offset moves towards _pos_offset_target_ned_m
|
||
Vector3f _vel_offset_target_ned_ms; // velocity offset target in m/s in NED frame
|
||
Vector3f _vel_offset_ned_ms; // velocity offset in NED m/s calculated by pos_to_rate step. This offset moves towards _vel_offset_target_ned_ms
|
||
Vector3f _accel_offset_target_ned_mss; // acceleration offset target in m/s² in NED frame
|
||
Vector3f _accel_offset_ned_mss; // acceleration offset in NED m/s²
|
||
uint32_t _posvelaccel_offset_target_ne_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
|
||
uint32_t _posvelaccel_offset_target_d_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
|
||
|
||
// ekf reset handling
|
||
uint32_t _ekf_ne_reset_ms; // system time of last recorded ekf ne position reset
|
||
uint32_t _ekf_d_reset_ms; // system time of last recorded ekf altitude reset
|
||
EKFResetMethod _ekf_reset_method = EKFResetMethod::MoveTarget; // EKF reset handling method. Loiter should use MoveTarget, Auto should use MoveVehicle
|
||
|
||
// high vibration handling
|
||
bool _vibe_comp_enabled; // true when high vibration compensation is on
|
||
|
||
// angle max override, if zero then use ANGLE_MAX parameter
|
||
float _angle_max_override_rad;
|
||
|
||
// return true if on a real vehicle or SITL with lock-step scheduling
|
||
bool has_good_timing(void) const;
|
||
|
||
private:
|
||
// Internal log writer for PSCx (North, East, Down tracking).
|
||
// Reduces duplication between Write_PSCN, PSCE, and PSCD.
|
||
// Used for logging desired/target/actual position, velocity, and acceleration per axis.
|
||
static void Write_PSCx(LogMessages ID, float pos_desired_m, float pos_target_m, float pos_m,
|
||
float vel_desired_ms, float vel_target_ms, float vel_ms,
|
||
float accel_desired_mss, float accel_target_mss, float accel_mss);
|
||
|
||
// Internal log writer for PSOx (North, East, Down tracking).
|
||
// Reduces duplication between Write_PSON, PSOE, and PSOD.
|
||
// Used for logging the target/actual position, velocity, and acceleration offset per axis.
|
||
static void Write_PSOx(LogMessages id, float pos_target_offset_m, float pos_offset_m,
|
||
float vel_target_offset_ms, float vel_offset_ms,
|
||
float accel_target_offset_mss, float accel_offset_mss);
|
||
|
||
// singleton
|
||
static AC_PosControl *_singleton;
|
||
};
|