Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-02-07 05:35:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
Files
12b58b63e00d27d8ad46df79f28ec0013bcf3b4e
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp
Randy Mackay 24d05a63eb AC_PosControl: param conversion comment fix
2026-01-23 11:51:54 +11:00

1910 lines
91 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#include <AP_HAL/AP_HAL.h>
#include "AC_PosControl.h"
#include <AP_Math/AP_Math.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Motors/AP_Motors.h> // motors library
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Scheduler/AP_Scheduler.h>
extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default gains for Plane
# define POSCONTROL_D_POS_P 1.0f // vertical position controller P gain default
# define POSCONTROL_D_VEL_P 5.0f // vertical velocity controller P gain default
# define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default
# define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter
# define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
# define POSCONTROL_D_ACC_P 0.03f // vertical acceleration controller P gain default
# define POSCONTROL_D_ACC_I 0.1f // vertical acceleration controller I gain default
# define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default
# define POSCONTROL_D_ACC_IMAX 0.8f // vertical acceleration controller IMAX gain default
# define POSCONTROL_D_ACC_FILT_HZ 10.0f // vertical acceleration controller input filter default
# define POSCONTROL_D_ACC_DT 0.02f // vertical acceleration controller dt default
# define POSCONTROL_NE_POS_P 0.5f // horizontal position controller P gain default
# define POSCONTROL_NE_VEL_P 0.7f // horizontal velocity controller P gain default
# define POSCONTROL_NE_VEL_I 0.35f // horizontal velocity controller I gain default
# define POSCONTROL_NE_VEL_D 0.17f // horizontal velocity controller D gain default
# define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default
# define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter
# define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
#elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
// default gains for Sub
# define POSCONTROL_D_POS_P 3.0f // vertical position controller P gain default
# define POSCONTROL_D_VEL_P 8.0f // vertical velocity controller P gain default
# define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default
# define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter
# define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
# define POSCONTROL_D_ACC_P 0.05f // vertical acceleration controller P gain default
# define POSCONTROL_D_ACC_I 0.01f // vertical acceleration controller I gain default
# define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default
# define POSCONTROL_D_ACC_IMAX 0.1f // vertical acceleration controller IMAX gain default
# define POSCONTROL_D_ACC_FILT_HZ 20.0f // vertical acceleration controller input filter default
# define POSCONTROL_D_ACC_DT 0.0025f // vertical acceleration controller dt default
# define POSCONTROL_NE_POS_P 1.0f // horizontal position controller P gain default
# define POSCONTROL_NE_VEL_P 1.0f // horizontal velocity controller P gain default
# define POSCONTROL_NE_VEL_I 0.5f // horizontal velocity controller I gain default
# define POSCONTROL_NE_VEL_D 0.0f // horizontal velocity controller D gain default
# define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default
# define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter
# define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
#else
// default gains for Copter / TradHeli
# define POSCONTROL_D_POS_P 1.0f // vertical position controller P gain default
# define POSCONTROL_D_VEL_P 5.0f // vertical velocity controller P gain default
# define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default
# define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter
# define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
# define POSCONTROL_D_ACC_P 0.05f // vertical acceleration controller P gain default
# define POSCONTROL_D_ACC_I 0.1f // vertical acceleration controller I gain default
# define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default
# define POSCONTROL_D_ACC_IMAX 0.8f // vertical acceleration controller IMAX gain default
# define POSCONTROL_D_ACC_FILT_HZ 20.0f // vertical acceleration controller input filter default
# define POSCONTROL_D_ACC_DT 0.0025f // vertical acceleration controller dt default
# define POSCONTROL_NE_POS_P 1.0f // horizontal position controller P gain default
# define POSCONTROL_NE_VEL_P 2.0f // horizontal velocity controller P gain default
# define POSCONTROL_NE_VEL_I 1.0f // horizontal velocity controller I gain default
# define POSCONTROL_NE_VEL_D 0.25f // horizontal velocity controller D gain default
# define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default
# define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter
# define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
#endif
// vibration compensation gains
#define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
#define POSCONTROL_VIBE_COMP_I_GAIN 0.125f
// velocity offset targets timeout if not updated within 3 seconds
#define POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS 3000
AC_PosControl *AC_PosControl::_singleton;
const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// 0 was used for HOVER
// POS_ACC_XY_FILT was here.
// @Param: _D_POS_P
// @DisplayName: Position (vertical) controller P gain
// @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller. Previously _POSZ_P.
// @Range: 0.50 4.00
// @Increment: 0.01
// @User: Standard
AP_SUBGROUPINFO(_p_pos_d_m, "_D_POS_", 2, AC_PosControl, AC_P_1D),
// 3 was _VELZ_ which has become _D_VEL_
// 4 was _ACCZ_ which has become _D_ACC_
// @Param: _NE_POS_P
// @DisplayName: Position (horizontal) controller P gain
// @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller. Previously _POSXY_P.
// @Range: 0.50 4.00
// @Increment: 0.01
// @User: Standard
AP_SUBGROUPINFO(_p_pos_ne_m, "_NE_POS_", 5, AC_PosControl, AC_P_2D),
// 6 was _VELXY_ which has become _NE_VEL_
// @Param: _ANGLE_MAX
// @DisplayName: Position Control Angle Max
// @Description: Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value
// @Units: deg
// @Range: 0 45
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max_deg, 0.0f),
// IDs 8,9 used for _TC_XY and _TC_Z in beta release candidate
// @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 aircraft varies the acceleration target
// @Units: m/s/s/s
// @Range: 1 50
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("_JERK_NE", 10, AC_PosControl, _shaping_jerk_ne_msss, POSCONTROL_JERK_NE_MSSS),
// @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 aircraft varies the acceleration target
// @Units: m/s/s/s
// @Range: 1 50
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("_JERK_D", 11, AC_PosControl, _shaping_jerk_d_msss, POSCONTROL_JERK_D_MSSS),
// @Param: _D_VEL_P
// @DisplayName: Velocity (vertical) controller P gain
// @Description: Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller. Previously _VELZ_P.
// @Range: 1.0 10.0
// @Increment: 0.1
// @User: Standard
// @Param: _D_VEL_I
// @DisplayName: Velocity (vertical) controller I gain
// @Description: Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration. Previously _VELZ_I.
// @Range: 0.00 10.0
// @Increment: 0.1
// @User: Advanced
// @Param: _D_VEL_IMAX
// @DisplayName: Velocity (vertical) controller I gain maximum
// @Description: Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output. If upgrading from 4.6 this is _VELZ_IMAX * 0.01.
// @Range: 1.000 10.000
// @Increment: 0.1
// @User: Standard
// @Param: _D_VEL_D
// @DisplayName: Velocity (vertical) controller D gain
// @Description: Velocity (vertical) controller D gain. Corrects short-term changes in velocity. Previously _VELZ_D.
// @Range: 0.00 2.00
// @Increment: 0.01
// @User: Advanced
// @Param: _D_VEL_FF
// @DisplayName: Velocity (vertical) controller Feed Forward gain
// @Description: Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target. Previously _VELZ_FF.
// @Range: 0.00 2.00
// @Increment: 0.01
// @User: Advanced
// @Param: _D_VEL_FLTE
// @DisplayName: Velocity (vertical) error filter
// @Description: Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms. Previously _VELZ_FLTE.
// @Range: 0 100
// @Increment: 1.0
// @Units: Hz
// @User: Advanced
// @Param: _D_VEL_FLTD
// @DisplayName: Velocity (vertical) input filter for D term
// @Description: Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms. Previously _VELZ_FLTD.
// @Range: 0 100
// @Increment: 1.0
// @Units: Hz
// @User: Advanced
AP_SUBGROUPINFO(_pid_vel_d_m, "_D_VEL_", 12, AC_PosControl, AC_PID_Basic),
// @Param: _D_ACC_P
// @DisplayName: Acceleration (vertical) controller P gain
// @Description: Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output. If upgrading from 4.6 this is _ACCZ_P * 0.1.
// @Range: 0.010 0.250
// @Increment: 0.001
// @User: Standard
// @Param: _D_ACC_I
// @DisplayName: Acceleration (vertical) controller I gain
// @Description: Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration. If upgrading from 4.6 this is _ACCZ_I * 0.1.
// @Range: 0.000 0.500
// @Increment: 0.001
// @User: Standard
// @Param: _D_ACC_IMAX
// @DisplayName: Acceleration (vertical) controller I gain maximum
// @Description: Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate. If upgrading from 4.6 this is _ACCZ_IMAX * 0.001.
// @Range: 0.0 1.0
// @Increment: 0.01
// @Units: d%
// @User: Standard
// @Param: _D_ACC_D
// @DisplayName: Acceleration (vertical) controller D gain
// @Description: Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration. If upgrading from 4.6 this is _ACCZ_D * 0.1.
// @Range: 0.000 0.100
// @Increment: 0.001
// @User: Standard
// @Param: _D_ACC_FF
// @DisplayName: Acceleration (vertical) controller feed forward
// @Description: Acceleration (vertical) controller feed forward. If upgrading from 4.6 this is _ACCZ_FF * 0.1.
// @Range: 0.000 0.100
// @Increment: 0.001
// @User: Standard
// @Param: _D_ACC_FLTT
// @DisplayName: Acceleration (vertical) controller target frequency in Hz
// @Description: Acceleration (vertical) controller target frequency in Hz. Previously _ACCZ_FLTT.
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _D_ACC_FLTE
// @DisplayName: Acceleration (vertical) controller error frequency in Hz
// @Description: Acceleration (vertical) controller error frequency in Hz. Previously _ACCZ_FLTE.
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _D_ACC_FLTD
// @DisplayName: Acceleration (vertical) controller derivative frequency in Hz
// @Description: Acceleration (vertical) controller derivative frequency in Hz. Previously _ACCZ_FLTD.
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _D_ACC_SMAX
// @DisplayName: Accel (vertical) slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 100
// @Increment: 0.1
// @User: Advanced
// @Param: _D_ACC_PDMX
// @DisplayName: Acceleration (vertical) controller PD sum maximum
// @Description: Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output. If upgrading from 4.6 this is _ACCZ_P * 0.1.
// @Range: 0.00 1.00
// @Increment: 0.01
// @Units: d%
// @Param: _D_ACC_D_FF
// @DisplayName: Accel (vertical) Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target. If upgrading from 4.6 this is _ACCZ_P * 0.1.
// @Range: 0.000 0.050
// @Increment: 0.001
// @User: Advanced
// @Param: _D_ACC_NTF
// @DisplayName: Accel (vertical) Target notch filter index
// @Description: Accel (vertical) Target notch filter index. If upgrading from 4.6 this is Previously _ACCZ_NTF.
// @Range: 1 8
// @User: Advanced
// @Param: _D_ACC_NEF
// @DisplayName: Accel (vertical) Error notch filter index
// @Description: Accel (vertical) Error notch filter index. If upgrading from 4.6 this is Previously _ACCZ_NEF.
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_accel_d_m, "_D_ACC_", 13, AC_PosControl, AC_PID),
// @Param: _NE_VEL_P
// @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration. Previously _VELXY_P.
// @Range: 0.10 10.00
// @Increment: 0.01
// @User: Advanced
// @Param: _NE_VEL_I
// @DisplayName: Velocity (horizontal) I gain
// @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration. Previously _VELXY_I.
// @Range: 0.10 10.00
// @Increment: 0.01
// @User: Advanced
// @Param: _NE_VEL_D
// @DisplayName: Velocity (horizontal) D gain
// @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity. Previously _VELXY_D.
// @Range: 0.00 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: _NE_VEL_IMAX
// @DisplayName: Velocity (horizontal) integrator maximum
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output. If upgrading from 4.6 this is _VELXY_IMAX * 0.01.
// @Range: 0 10
// @Increment: 1
// @Units: m/s/s
// @User: Advanced
// @Param: _NE_VEL_FLTE
// @DisplayName: Velocity (horizontal) input filter
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms. Previously _VELXY_FLTE.
// @Range: 0 100
// @Increment: 1
// @Units: Hz
// @User: Advanced
// @Param: _NE_VEL_FLTD
// @DisplayName: Velocity (horizontal) input filter
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term. Previously _VELXY_FLTD.
// @Range: 0 100
// @Increment: 1
// @Units: Hz
// @User: Advanced
// @Param: _NE_VEL_FF
// @DisplayName: Velocity (horizontal) feed forward gain
// @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration. Previously _VELXY_FF.
// @Range: 0.10 10.00
// @Increment: 0.01
// @User: Advanced
AP_SUBGROUPINFO(_pid_vel_ne_m, "_NE_VEL_", 14, AC_PosControl, AC_PID_2D),
AP_GROUPEND
};
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
_ahrs(ahrs),
_motors(motors),
_attitude_control(attitude_control),
_p_pos_ne_m(POSCONTROL_NE_POS_P),
_p_pos_d_m(POSCONTROL_D_POS_P),
_pid_vel_ne_m(POSCONTROL_NE_VEL_P, POSCONTROL_NE_VEL_I, POSCONTROL_NE_VEL_D, 0.0f, POSCONTROL_NE_VEL_IMAX, POSCONTROL_NE_VEL_FILT_HZ, POSCONTROL_NE_VEL_FILT_D_HZ),
_pid_vel_d_m(POSCONTROL_D_VEL_P, 0.0f, 0.0f, 0.0f, POSCONTROL_D_VEL_IMAX, POSCONTROL_D_VEL_FILT_HZ, POSCONTROL_D_VEL_FILT_D_HZ),
_pid_accel_d_m(POSCONTROL_D_ACC_P, POSCONTROL_D_ACC_I, POSCONTROL_D_ACC_D, 0.0f, POSCONTROL_D_ACC_IMAX, 0.0f, POSCONTROL_D_ACC_FILT_HZ, 0.0f),
_vel_max_ne_ms(POSCONTROL_SPEED_MS),
_vel_max_up_ms(POSCONTROL_SPEED_UP_MS),
_vel_max_down_ms(POSCONTROL_SPEED_DOWN_MS),
_accel_max_ne_mss(POSCONTROL_ACCEL_NE_MSS),
_accel_max_d_mss(POSCONTROL_ACCEL_D_MSS),
_jerk_max_ne_msss(POSCONTROL_JERK_NE_MSSS),
_jerk_max_d_msss(POSCONTROL_JERK_D_MSSS)
{
AP_Param::setup_object_defaults(this, var_info);
_singleton = this;
}
///
/// 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 AC_PosControl::input_pos_NED_m(const Vector3p& pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m)
{
// Terrain following velocity scalar must be calculated before we remove the position offset
const float offset_d_scalar = terrain_scaler_D_m(pos_terrain_target_d_m, terrain_margin_m);
set_pos_terrain_target_D_m(pos_terrain_target_d_m);
// calculated increased maximum acceleration and jerk if over speed
const float overspeed_gain = calculate_overspeed_gain();
const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain;
const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain;
update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
// adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
// calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos_ne_m
float vel_max_ne_ms = 0.0f;
float vel_max_d_ms = 0.0f;
Vector3f travel_dir_unit = (pos_ned_m - _pos_desired_ned_m).tofloat();
if (is_positive(travel_dir_unit.length_squared()) ) {
travel_dir_unit.normalize();
float travel_dir_unit_ne_length = travel_dir_unit.xy().length();
float vel_max_ms = kinematic_limit(travel_dir_unit, _vel_max_ne_ms, _vel_max_up_ms, _vel_max_down_ms);
vel_max_ne_ms = vel_max_ms * travel_dir_unit_ne_length;
vel_max_d_ms = fabsf(vel_max_ms * travel_dir_unit.z);
}
// reduce speed if we are reaching the edge of our vertical buffer
vel_max_ne_ms *= offset_d_scalar;
Vector2f vel_ne_ms;
Vector2f accel_ne_mss;
shape_pos_vel_accel_xy(pos_ned_m.xy(), vel_ne_ms, accel_ne_mss, _pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(),
vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false);
float pos_d_m = pos_ned_m.z;
shape_pos_vel_accel(pos_d_m, 0, 0,
_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z,
-vel_max_d_ms, vel_max_d_ms,
-accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5),
jerk_max_d_msss, _dt_s, false);
}
// Returns a scaling factor for horizontal velocity in m/s to ensure
// the vertical controller maintains a safe distance above terrain.
float AC_PosControl::terrain_scaler_D_m(float pos_terrain_d_m, float terrain_margin_m) const
{
if (is_zero(terrain_margin_m)) {
return 1.0;
}
float pos_offset_error_d_m = _pos_estimate_ned_m.z - (_pos_target_ned_m.z + (pos_terrain_d_m - _pos_terrain_d_m));
return constrain_float((1.0 - (fabsf(pos_offset_error_d_m) - 0.5 * terrain_margin_m) / (0.5 * terrain_margin_m)), 0.01, 1.0);
}
///
/// 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 AC_PosControl::NE_set_max_speed_accel_cm(float speed_ne_cms, float accel_ne_cmss)
{
NE_set_max_speed_accel_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01);
}
// 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 AC_PosControl::NE_set_max_speed_accel_m(float speed_ne_ms, float accel_ne_mss)
{
_vel_max_ne_ms = fabsf(speed_ne_ms);
_accel_max_ne_mss = fabsf(accel_ne_mss);
// ensure the horizontal jerk is less than the vehicle is capable of
const float jerk_max_msss = MIN(_attitude_control.get_ang_vel_roll_max_rads(), _attitude_control.get_ang_vel_pitch_max_rads()) * GRAVITY_MSS;
const float snap_max_mssss = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS;
// get specified jerk limit
_jerk_max_ne_msss = _shaping_jerk_ne_msss;
// limit maximum jerk based on maximum angular rate
if (is_positive(jerk_max_msss) && _attitude_control.get_bf_feedforward()) {
_jerk_max_ne_msss = MIN(_jerk_max_ne_msss, jerk_max_msss);
}
// limit maximum jerk to maximum possible average jerk based on angular acceleration
if (is_positive(snap_max_mssss) && _attitude_control.get_bf_feedforward()) {
_jerk_max_ne_msss = MIN(0.5 * safe_sqrt(_accel_max_ne_mss * snap_max_mssss), _jerk_max_ne_msss);
}
}
// 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 AC_PosControl::NE_set_correction_speed_accel_cm(float speed_ne_cms, float accel_ne_cmss)
{
NE_set_correction_speed_accel_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01);
}
// 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 AC_PosControl::NE_set_correction_speed_accel_m(float speed_ne_ms, float accel_ne_mss)
{
// limits that are not positive are ignored
_p_pos_ne_m.set_limits(speed_ne_ms, accel_ne_mss, 0.0f);
}
// 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 AC_PosControl::NE_init_controller_stopping_point()
{
NE_init_controller();
get_stopping_point_NE_m(_pos_desired_ned_m.xy());
_pos_target_ned_m.xy() = _pos_desired_ned_m.xy() + _pos_offset_ned_m.xy();
_vel_desired_ned_ms.xy().zero();
_accel_desired_ned_mss.xy().zero();
}
// 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 AC_PosControl::NE_relax_velocity_controller()
{
// decay acceleration and therefore current attitude target to zero
// this will be reset by NE_init_controller() if !NE_is_active()
if (is_positive(_dt_s)) {
float decay = 1.0 - _dt_s / (_dt_s + POSCONTROL_RELAX_TC);
_accel_target_ned_mss.xy() *= decay;
}
NE_init_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 AC_PosControl::NE_soften_for_landing()
{
// decay position error to zero
if (is_positive(_dt_s)) {
_pos_target_ned_m.xy() += (_pos_estimate_ned_m.xy() - _pos_target_ned_m.xy()) * (_dt_s / (_dt_s + POSCONTROL_RELAX_TC));
_pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
}
// Prevent I term build up in xy velocity controller.
// Note that this flag is reset on each loop in NE_update_controller()
NE_set_externally_limited();
}
// 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 AC_PosControl::NE_init_controller()
{
// initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
NE_init_offsets();
// set roll, pitch lean angle targets to current attitude
const Vector3f &att_target_euler_rad = _attitude_control.get_att_target_euler_rad();
_roll_target_rad = att_target_euler_rad.x;
_pitch_target_rad = att_target_euler_rad.y;
_yaw_target_rad = att_target_euler_rad.z; // todo: this should be thrust vector heading, not yaw.
_yaw_rate_target_rads = 0.0f;
_angle_max_override_rad = 0.0;
_pos_target_ned_m.xy() = _pos_estimate_ned_m.xy();
_pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
_vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy();
_vel_desired_ned_ms.xy() = _vel_target_ned_ms.xy() - _vel_offset_ned_ms.xy();
// Set desired acceleration to zero because raw acceleration is prone to noise
_accel_desired_ned_mss.xy().zero();
if (!NE_is_active()) {
lean_angles_to_accel_NE_mss(_accel_target_ned_mss.x, _accel_target_ned_mss.y);
}
// limit acceleration using maximum lean angles
const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad());
const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad);
_accel_target_ned_mss.xy().limit_length(accel_max_mss);
// initialise I terms from lean angles
_pid_vel_ne_m.reset_filter();
// initialise the I term to (_accel_target_ned_mss - _accel_desired_ned_mss)
// _accel_desired_ned_mss is zero and can be removed from the equation
_pid_vel_ne_m.set_integrator((_accel_target_ned_mss.xy() - _vel_target_ned_ms.xy() * _pid_vel_ne_m.ff()));
// initialise ekf xy reset handler
NE_init_ekf_reset();
// initialise z_controller time out
_last_update_ne_ticks = AP::scheduler().ticks32();
}
// 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 AC_PosControl::input_accel_NE_m(const Vector2f& accel_ne_mss)
{
update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
shape_accel_xy(accel_ne_mss, _accel_desired_ned_mss.xy(), _jerk_max_ne_msss, _dt_s);
}
// 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 AC_PosControl::input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output)
{
Vector2f vel_ne_ms = vel_ne_cms * 0.01;
input_vel_accel_NE_m(vel_ne_ms, accel_ne_cmss * 0.01, limit_output);
vel_ne_cms = vel_ne_ms * 100.0;
}
// 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 AC_PosControl::input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output)
{
update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
shape_vel_accel_xy(vel_ne_ms, accel_ne_mss, _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(),
_accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output);
update_vel_accel_xy(vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f());
}
// 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 AC_PosControl::input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output)
{
Vector2p pos_ne_m = pos_ne_cm * 0.01;
Vector2f vel_ne_ms = vel_ne_cms * 0.01;
input_pos_vel_accel_NE_m(pos_ne_m, vel_ne_ms, accel_ne_cmss * 0.01, limit_output);
pos_ne_cm = pos_ne_m * 100.0;
vel_ne_cms = vel_ne_ms * 100.0;
}
// 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 AC_PosControl::input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output)
{
update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
shape_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(),
_vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output);
update_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f(), Vector2f());
}
// Updates NE offsets by gradually moving them toward their targets.
void AC_PosControl::NE_update_offsets()
{
// Check if NE offset targets have timed out
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
// Timeout: reset all NE offset targets to zero
_pos_offset_target_ned_m.xy().zero();
_vel_offset_target_ned_ms.xy().zero();
_accel_offset_target_ned_mss.xy().zero();
}
// Advance offset target kinematic state (position, velocity, accel)
update_pos_vel_accel_xy(_pos_offset_target_ned_m.xy(), _vel_offset_target_ned_ms.xy(), _accel_offset_target_ned_mss.xy(), _dt_s, Vector2f(), Vector2f(), Vector2f());
update_pos_vel_accel_xy(_pos_offset_ned_m.xy(), _vel_offset_ned_ms.xy(), _accel_offset_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
// Shape the offset path from current to target using jerk-limited smoothing
shape_pos_vel_accel_xy(_pos_offset_target_ned_m.xy(), _vel_offset_target_ned_ms.xy(), _accel_offset_target_ned_mss.xy(),
_pos_offset_ned_m.xy(), _vel_offset_ned_ms.xy(), _accel_offset_ned_mss.xy(),
_vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false);
}
// Disables NE position correction by setting the target position to the current position.
// Useful to freeze positional control without disrupting velocity control.
void AC_PosControl::NE_stop_pos_stabilisation()
{
_pos_target_ned_m.xy() = _pos_estimate_ned_m.xy();
_pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
}
// 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 AC_PosControl::NE_stop_vel_stabilisation()
{
_pos_target_ned_m.xy() = _pos_estimate_ned_m.xy();
_pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
_vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy();
_vel_desired_ned_ms.xy() = _vel_target_ned_ms.xy() - _vel_offset_ned_ms.xy();
// reset I terms
_pid_vel_ne_m.reset_filter();
_pid_vel_ne_m.reset_I();
}
// Returns true if the NE position controller has run in the last 5 control loop cycles.
bool AC_PosControl::NE_is_active() const
{
const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_ne_ticks;
return dt_ticks <= 1;
}
// 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 AC_PosControl::NE_update_controller()
{
// check for ekf xy position reset
NE_handle_ekf_reset();
// Check for position control time out
if (!NE_is_active()) {
NE_init_controller();
if (has_good_timing()) {
// call internal error because initialisation has not been done
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
_last_update_ne_ticks = AP::scheduler().ticks32();
float ahrsGndSpdLimit, ahrsControlScaleXY;
AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);
// Update lateral position, velocity, and acceleration offsets using path shaping
NE_update_offsets();
// Position Controller
// Combine position target with active NE offset to get absolute target
_pos_target_ned_m.xy() = _pos_desired_ned_m.xy() + _pos_offset_ned_m.xy();
// determine the combined position of the actual position and the disturbance from system ID mode
// calculate the target velocity correction
Vector2p comb_pos_ne_m = _pos_estimate_ned_m.xy();
comb_pos_ne_m += _disturb_pos_ne_m.topostype();
// Run P controller to compute velocity setpoint from position error
Vector2f vel_target_ne_ms = _p_pos_ne_m.update_all(_pos_target_ned_m.xy(), comb_pos_ne_m);
_pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
// Velocity Controller
// Apply AHRS scaling (e.g. for optical flow noise compensation)
vel_target_ne_ms *= ahrsControlScaleXY;
vel_target_ne_ms *= _ne_control_scale_factor;
_vel_target_ned_ms.xy() = vel_target_ne_ms;
_vel_target_ned_ms.xy() += _vel_desired_ned_ms.xy() + _vel_offset_ned_ms.xy();
// Velocity Controller
// determine the combined velocity of the actual velocity and the disturbance from system ID mode
Vector2f comb_vel_ne_ms = _vel_estimate_ned_ms.xy();
comb_vel_ne_ms += _disturb_vel_ne_ms;
// Run velocity PID controller and scale result for control authority
Vector2f accel_target_ne_mss = _pid_vel_ne_m.update_all(_vel_target_ned_ms.xy(), comb_vel_ne_ms, _dt_s, _limit_vector_ned.xy());
// Acceleration Controller
// Apply AHRS scaling again to correct for measurement distortions
accel_target_ne_mss *= ahrsControlScaleXY;
accel_target_ne_mss *= _ne_control_scale_factor;
_ne_control_scale_factor = 1.0;
// pass the correction acceleration to the target acceleration output
_accel_target_ned_mss.xy() = accel_target_ne_mss;
_accel_target_ned_mss.xy() += _accel_desired_ned_mss.xy() + _accel_offset_ned_mss.xy();
// limit acceleration using maximum lean angles
const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad());
const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad);
// Save unbounded target for use in "limited" check (not unit-consistent with z!)
_limit_vector_ned.xy() = _accel_target_ned_mss.xy();
if (!limit_accel_xy(_vel_desired_ned_ms.xy(), _accel_target_ned_mss.xy(), accel_max_mss)) {
// _accel_target_ned_mss was not limited so we can zero the xy limit vector
_limit_vector_ned.xy().zero();
}
// Convert acceleration to roll/pitch angle targets (used by attitude controller)
accel_NE_mss_to_lean_angles_rad(_accel_target_ned_mss.x, _accel_target_ned_mss.y, _roll_target_rad, _pitch_target_rad);
// Update yaw and yaw rate targets to match heading of motion
calculate_yaw_and_rate_yaw();
// reset the disturbance from system ID mode to zero
_disturb_pos_ne_m.zero();
_disturb_vel_ne_ms.zero();
}
///
/// Vertical position controller
///
// Sets maximum climb/descent rate (cm/s) and vertical acceleration (cm/s²) for the U-axis.
// See D_set_max_speed_accel_m() for full details.
// All values must be positive.
void AC_PosControl::D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss)
{
D_set_max_speed_accel_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01);
}
// 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 AC_PosControl::D_set_max_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss)
{
// sanity check and update
if (!is_zero(decent_speed_max_ms)) {
_vel_max_down_ms = fabsf(decent_speed_max_ms);
}
if (!is_zero(climb_speed_max_ms)) {
_vel_max_up_ms = fabsf(climb_speed_max_ms);
}
if (!is_zero(accel_max_d_mss)) {
_accel_max_d_mss = fabsf(accel_max_d_mss);
}
// ensure the vertical Jerk is not limited by the filters in the Z acceleration PID object
_jerk_max_d_msss = _shaping_jerk_d_msss;
if (is_positive(_pid_accel_d_m.filt_T_hz())) {
_jerk_max_d_msss = MIN(_jerk_max_d_msss, MIN(GRAVITY_MSS, _accel_max_d_mss) * (M_2PI * _pid_accel_d_m.filt_T_hz()) / 5.0);
}
if (is_positive(_pid_accel_d_m.filt_E_hz())) {
_jerk_max_d_msss = MIN(_jerk_max_d_msss, MIN(GRAVITY_MSS, _accel_max_d_mss) * (M_2PI * _pid_accel_d_m.filt_E_hz()) / 5.0);
}
}
// Sets vertical correction velocity and acceleration limits (cm/s, cm/s²).
// Should only be called during initialization to avoid discontinuities.
// See set_correction_speed_accel_U_m() for full details.
// All values must be positive.
void AC_PosControl::D_set_correction_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss)
{
D_set_correction_speed_accel_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01);
}
// 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 AC_PosControl::D_set_correction_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss)
{
// define maximum position error and maximum first and second differential limits
_p_pos_d_m.set_limits(-fabsf(decent_speed_max_ms), fabsf(climb_speed_max_ms), fabsf(accel_max_d_mss), 0.0f);
}
// Initializes U-axis controller to current position, velocity, and acceleration, disallowing descent.
// Used for takeoff or hold scenarios where downward motion is prohibited.
void AC_PosControl::D_init_controller_no_descent()
{
// Initialise the position controller to the current throttle, position, velocity and acceleration.
D_init_controller();
// remove all descent if present
_vel_target_ned_ms.z = MIN(0.0, _vel_target_ned_ms.z);
_vel_desired_ned_ms.z = MIN(0.0, _vel_desired_ned_ms.z);
_vel_terrain_d_ms = MIN(0.0, _vel_terrain_d_ms);
_vel_offset_ned_ms.z = MIN(0.0, _vel_offset_ned_ms.z);
_accel_target_ned_mss.z = MIN(0.0, _accel_target_ned_mss.z);
_accel_desired_ned_mss.z = MIN(0.0, _accel_desired_ned_mss.z);
_accel_terrain_d_mss = MIN(0.0, _accel_terrain_d_mss);
_accel_offset_ned_mss.z = MIN(0.0, _accel_offset_ned_mss.z);
}
// 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 AC_PosControl::D_init_controller_stopping_point()
{
// Initialise the position controller to the current throttle, position, velocity and acceleration.
D_init_controller();
get_stopping_point_D_m(_pos_desired_ned_m.z);
_pos_target_ned_m.z = _pos_desired_ned_m.z + _pos_offset_ned_m.z;
_vel_desired_ned_ms.z = 0.0f;
_accel_desired_ned_mss.z = 0.0f;
}
// 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 AC_PosControl::D_relax_controller(float throttle_setting)
{
// Initialise the position controller to the current position, velocity and acceleration.
D_init_controller();
// D_init_controller has set the acceleration PID I term to generate the current throttle set point
// Use relax_integrator to decay the throttle set point to throttle_setting
_pid_accel_d_m.relax_integrator(-(throttle_setting - _motors.get_throttle_hover()), _dt_s, POSCONTROL_RELAX_TC);
}
// 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 AC_PosControl::D_init_controller()
{
// initialise terrain targets and offsets to zero
init_terrain();
// initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
D_init_offsets();
_pos_target_ned_m.z = _pos_estimate_ned_m.z;
_pos_desired_ned_m.z = _pos_target_ned_m.z - _pos_offset_ned_m.z;
_vel_target_ned_ms.z = _vel_estimate_ned_ms.z;
_vel_desired_ned_ms.z = _vel_target_ned_ms.z - _vel_offset_ned_ms.z;
// Reset I term of velocity PID
_pid_vel_d_m.reset_filter();
_pid_vel_d_m.set_integrator(0.0f);
_accel_target_ned_mss.z = constrain_float(get_estimated_accel_D_mss(), -_accel_max_d_mss, _accel_max_d_mss);
_accel_desired_ned_mss.z = _accel_target_ned_mss.z - (_accel_offset_ned_mss.z + _accel_terrain_d_mss);
_pid_accel_d_m.reset_filter();
// Set acceleration PID I term based on the current throttle
// Remove the expected P term due to _accel_desired_ned_mss.z being constrained to _accel_max_d_mss
// Remove the expected FF term due to non-zero _accel_target_ned_mss.z
_pid_accel_d_m.set_integrator(-(_attitude_control.get_throttle_in() - _motors.get_throttle_hover())
- _pid_accel_d_m.kP() * (_accel_target_ned_mss.z - get_estimated_accel_D_mss())
- _pid_accel_d_m.ff() * _accel_target_ned_mss.z);
// initialise ekf z reset handler
D_init_ekf_reset();
// initialise z_controller time out
_last_update_d_ticks = AP::scheduler().ticks32();
}
// 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 AC_PosControl::input_accel_D_m(float accel_d_mss)
{
// calculated increased maximum jerk if over speed
float jerk_max_d_msss = _jerk_max_d_msss * calculate_overspeed_gain();
// adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
shape_accel(accel_d_mss, _accel_desired_ned_mss.z, jerk_max_d_msss, _dt_s);
}
// 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 AC_PosControl::input_vel_accel_D_m(float &vel_d_ms, float accel_d_mss, bool limit_output)
{
// calculated increased maximum acceleration and jerk if over speed
const float overspeed_gain = calculate_overspeed_gain();
const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain;
const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain;
// adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
shape_vel_accel(vel_d_ms, accel_d_mss,
_vel_desired_ned_ms.z, _accel_desired_ned_mss.z,
-accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5),
jerk_max_d_msss, _dt_s, limit_output);
update_vel_accel(vel_d_ms, accel_d_mss, _dt_s, 0.0, 0.0);
}
// 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 set_pos_target_D_from_climb_rate_m() for full details.
void AC_PosControl::D_set_pos_target_from_climb_rate_cms(float climb_rate_cms)
{
D_set_pos_target_from_climb_rate_ms(climb_rate_cms * 0.01);
}
// 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 AC_PosControl::D_set_pos_target_from_climb_rate_ms(float climb_rate_ms, bool ignore_descent_limit)
{
if (ignore_descent_limit) {
// turn off limits in the down (positive z) direction
_limit_vector_ned.z = MIN(_limit_vector_ned.z, 0.0f);
}
float vel_d_ms = -climb_rate_ms;
input_vel_accel_D_m(vel_d_ms, 0.0);
}
// Sets vertical position, velocity, and acceleration in cm using jerk-limited shaping.
// See input_pos_vel_accel_D_m() for full details.
void AC_PosControl::input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output)
{
float pos_d_m = -pos_u_cm * 0.01;
float vel_d_ms = -vel_u_cms * 0.01;
const float accel_d_mss = -accel_u_cmss * 0.01;
input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, accel_d_mss, limit_output);
pos_u_cm = -pos_d_m * 100.0;
vel_u_cms = -vel_d_ms * 100.0;
}
// 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 AC_PosControl::input_pos_vel_accel_D_m(float &pos_d_m, float &vel_d_ms, float accel_d_mss, bool limit_output)
{
// calculated increased maximum acceleration and jerk if over speed
const float overspeed_gain = calculate_overspeed_gain();
const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain;
const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain;
// adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
shape_pos_vel_accel(pos_d_m, vel_d_ms, accel_d_mss,
_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z,
-_vel_max_up_ms, _vel_max_down_ms,
-accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5),
jerk_max_d_msss, _dt_s, limit_output);
postype_t posp = pos_d_m;
update_pos_vel_accel(posp, vel_d_ms, accel_d_mss, _dt_s, 0.0, 0.0, 0.0);
pos_d_m = posp;
}
// 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 AC_PosControl::set_alt_target_with_slew_cm(float pos_u_cm)
{
D_set_alt_target_with_slew_m(pos_u_cm * 0.01);
}
// Sets target altitude in meters using jerk-limited shaping.
void AC_PosControl::D_set_alt_target_with_slew_m(float pos_u_m)
{
float pos_d_m = -pos_u_m;
float zero = 0;
input_pos_vel_accel_D_m(pos_d_m, zero, 0);
}
// Updates vertical (U) offsets by gradually moving them toward their targets.
void AC_PosControl::D_update_offsets()
{
// Check if vertical offset targets have timed out
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_d_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
// Timeout: reset U-axis offset targets to zero
_pos_offset_target_ned_m.z = 0.0;
_vel_offset_target_ned_ms.z = 0.0;
_accel_offset_target_ned_mss.z = 0.0;
}
// Advance current offset state using PID-derived feedback and vertical limits
postype_t p_offset_d_m = _pos_offset_ned_m.z;
update_pos_vel_accel(p_offset_d_m, _vel_offset_ned_ms.z, _accel_offset_ned_mss.z, _dt_s, MIN(_limit_vector_ned.z, 0.0f), _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
_pos_offset_ned_m.z = p_offset_d_m;
// Shape offset trajectory (position/velocity/acceleration) using jerk-limited smoothing
shape_pos_vel_accel(_pos_offset_target_ned_m.z, _vel_offset_target_ned_ms.z, _accel_offset_target_ned_mss.z,
_pos_offset_ned_m.z, _vel_offset_ned_ms.z, _accel_offset_ned_mss.z,
-get_max_speed_up_ms(), get_max_speed_down_ms(),
-D_get_max_accel_mss(), D_get_max_accel_mss(),
_jerk_max_d_msss, _dt_s, false);
// Update target state forward in time with assumed zero velocity/acceleration targets
p_offset_d_m = _pos_offset_target_ned_m.z;
update_pos_vel_accel(p_offset_d_m, _vel_offset_target_ned_ms.z, _accel_offset_target_ned_mss.z, _dt_s, 0.0, 0.0, 0.0);
_pos_offset_target_ned_m.z = p_offset_d_m;
}
// Returns true if the U-axis controller has run in the last 5 control loop cycles.
bool AC_PosControl::D_is_active() const
{
const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_d_ticks;
return dt_ticks <= 1;
}
// 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 AC_PosControl::D_update_controller()
{
// check for ekf z-axis position reset
D_handle_ekf_reset();
// Check for z_controller time out
if (!D_is_active()) {
D_init_controller();
if (has_good_timing()) {
// call internal error because initialisation has not been done
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
_last_update_d_ticks = AP::scheduler().ticks32();
// Update vertical offset targets and terrain estimate
D_update_offsets();
update_terrain();
// Position Controller
// Combine desired + offset + terrain for final position target
_pos_target_ned_m.z = _pos_desired_ned_m.z + _pos_offset_ned_m.z + _pos_terrain_d_m;
// P controller: convert position error to velocity target
_vel_target_ned_ms.z = _p_pos_d_m.update_all(_pos_target_ned_m.z, _pos_estimate_ned_m.z);
_vel_target_ned_ms.z *= AP::ahrs().getControlScaleZ();
_pos_desired_ned_m.z = _pos_target_ned_m.z - (_pos_offset_ned_m.z + _pos_terrain_d_m);
// add feed forward component
_vel_target_ned_ms.z += _vel_desired_ned_ms.z + _vel_offset_ned_ms.z + _vel_terrain_d_ms;
// Velocity Controller
// PID controller: convert velocity error to acceleration
_accel_target_ned_mss.z = _pid_vel_d_m.update_all(_vel_target_ned_ms.z, _vel_estimate_ned_ms.z, _dt_s, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
_accel_target_ned_mss.z *= AP::ahrs().getControlScaleZ();
// add feed forward component
_accel_target_ned_mss.z += _accel_desired_ned_mss.z + _accel_offset_ned_mss.z + _accel_terrain_d_mss;
// Acceleration Controller
// Gravity-compensated vertical acceleration measurement (positive = up)
const float estimated_accel_d_mss = get_estimated_accel_D_mss();
// Ensure integrator can produce enough thrust to overcome hover throttle
if (_motors.get_throttle_hover() > _pid_accel_d_m.imax()) {
_pid_accel_d_m.set_imax(_motors.get_throttle_hover());
}
float thrust_d_norm;
if (_vibe_comp_enabled) {
// Use vibration-resistant throttle estimator (feedforward + scaled integrator)
thrust_d_norm = get_throttle_with_vibration_override();
} else {
// Standard PID update using vertical acceleration error
thrust_d_norm = _pid_accel_d_m.update_all(_accel_target_ned_mss.z, estimated_accel_d_mss, _dt_s, (_motors.limit.throttle_lower || _motors.limit.throttle_upper));
// Include FF contribution to reduce delay
thrust_d_norm += _pid_accel_d_m.get_ff();
}
thrust_d_norm -= _motors.get_throttle_hover();
// Actuator commands
// Send final throttle output to attitude controller (includes angle boost)
_attitude_control.set_throttle_out(-thrust_d_norm, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ);
// Check for vertical controller health
// Update health indicator based on error magnitude vs configured speed range
float error_ratio = _pid_vel_d_m.get_error() / _vel_max_down_ms;
_vel_d_control_ratio += _dt_s * 0.1f * (0.5 - error_ratio);
_vel_d_control_ratio = constrain_float(_vel_d_control_ratio, 0.0f, 2.0f);
// set vertical component of the limit vector
if (_motors.limit.throttle_upper) {
_limit_vector_ned.z = -1.0f;
} else if (_motors.limit.throttle_lower) {
_limit_vector_ned.z = 1.0f;
} else {
_limit_vector_ned.z = 0.0f;
}
}
///
/// Accessors
///
// Returns the maximum allowed roll/pitch angle in radians.
float AC_PosControl::get_lean_angle_max_rad() const
{
if (is_positive(_angle_max_override_rad)) {
return _angle_max_override_rad;
}
if (!is_positive(_lean_angle_max_deg)) {
return _attitude_control.lean_angle_max_rad();
}
return radians(_lean_angle_max_deg);
}
// 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 AC_PosControl::set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss)
{
_pos_desired_ned_m = pos_ned_m;
_vel_desired_ned_ms = vel_ned_ms;
_accel_desired_ned_mss = 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 AC_PosControl::set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss)
{
_pos_desired_ned_m.xy() = pos_ne_m;
_vel_desired_ned_ms.xy() = vel_ne_ms;
_accel_desired_ned_mss.xy() = accel_ne_mss;
}
// Converts lean angles (rad) to NED acceleration in m/s².
Vector3f AC_PosControl::lean_angles_rad_to_accel_NED_mss(const Vector3f& att_target_euler_rad) const
{
// rotate our roll, pitch angles into lat/lon frame
const float sin_roll = sinf(att_target_euler_rad.x);
const float cos_roll = cosf(att_target_euler_rad.x);
const float sin_pitch = sinf(att_target_euler_rad.y);
const float cos_pitch = cosf(att_target_euler_rad.y);
const float sin_yaw = sinf(att_target_euler_rad.z);
const float cos_yaw = cosf(att_target_euler_rad.z);
return Vector3f{
GRAVITY_MSS * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
GRAVITY_MSS * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
-GRAVITY_MSS
};
}
/// Terrain
// Initializes terrain position, velocity, and acceleration to match the terrain target.
void AC_PosControl::init_terrain()
{
// set terrain position and target to zero
_pos_terrain_target_d_m = 0.0;
_pos_terrain_d_m = 0.0;
// set velocity offset to zero
_vel_terrain_d_ms = 0.0;
// set acceleration offset to zero
_accel_terrain_d_mss = 0.0;
}
// 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 AC_PosControl::init_pos_terrain_U_cm(float pos_terrain_u_cm)
{
init_pos_terrain_D_m(-pos_terrain_u_cm * 0.01);
}
// Initializes both the terrain altitude and terrain target to the same value
// (relative to EKF origin in meters, Down-positive).
void AC_PosControl::init_pos_terrain_D_m(float pos_terrain_d_m)
{
_pos_desired_ned_m.z -= (pos_terrain_d_m - _pos_terrain_d_m);
_pos_terrain_target_d_m = pos_terrain_d_m;
_pos_terrain_d_m = pos_terrain_d_m;
}
/// Offsets
// Initializes NE position/velocity/acceleration offsets to match their respective targets.
void AC_PosControl::NE_init_offsets()
{
// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target_ned_m.xy().zero();
_vel_offset_target_ned_ms.xy().zero();
_accel_offset_target_ned_mss.xy().zero();
}
// set position offset to target
_pos_offset_ned_m.xy() = _pos_offset_target_ned_m.xy();
// set velocity offset to target
_vel_offset_ned_ms.xy() = _vel_offset_target_ned_ms.xy();
// set acceleration offset to target
_accel_offset_ned_mss.xy() = _accel_offset_target_ned_mss.xy();
}
// Initializes vertical (U) offsets to match their respective targets.
void AC_PosControl::D_init_offsets()
{
// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_d_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target_ned_m.z = 0.0;
_vel_offset_target_ned_ms.z = 0.0;
_accel_offset_target_ned_mss.z = 0.0;
}
// set position offset to target
_pos_offset_ned_m.z = _pos_offset_target_ned_m.z;
// set velocity offset to target
_vel_offset_ned_ms.z = _vel_offset_target_ned_ms.z;
// set acceleration offset to target
_accel_offset_ned_mss.z = _accel_offset_target_ned_mss.z;
}
#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 AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_ned_m, const Vector3f &vel_offset_ned_ms, const Vector3f &accel_offset_ned_mss)
{
set_posvelaccel_offset_target_NE_m(pos_offset_ned_m.topostype().xy(), vel_offset_ned_ms.xy(), accel_offset_ned_mss.xy());
set_posvelaccel_offset_target_D_m(pos_offset_ned_m.topostype().z, vel_offset_ned_ms.z, accel_offset_ned_mss.z);
return true;
}
// Retrieves current scripted offsets in meters (NED frame).
// Used in LUA
bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_ned_m, Vector3f &vel_offset_ned_ms, Vector3f &accel_offset_ned_mss)
{
pos_offset_ned_m = _pos_offset_target_ned_m.tofloat();
vel_offset_ned_ms = _vel_offset_target_ned_ms;
accel_offset_ned_mss = _accel_offset_target_ned_mss;
return true;
}
// Retrieves current target velocity (NED frame, m/s) including any scripted offset.
// Used in LUA
bool AC_PosControl::get_vel_target(Vector3f &vel_target_ned_ms)
{
if (!NE_is_active() || !D_is_active()) {
return false;
}
vel_target_ned_ms = _vel_target_ned_ms;
return true;
}
// Retrieves current target acceleration (NED frame, m/s²) including any scripted offset.
// Used in LUA
bool AC_PosControl::get_accel_target(Vector3f &accel_target_ned_mss)
{
if (!NE_is_active() || !D_is_active()) {
return false;
}
accel_target_ned_mss = _accel_target_ned_mss;
return true;
}
#endif
// Sets NE offset targets in meters, m/s, and m/s².
void AC_PosControl::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)
{
// set position offset target
_pos_offset_target_ned_m.xy() = pos_offset_target_ne_m;
// set velocity offset target
_vel_offset_target_ned_ms.xy() = vel_offset_target_ne_ms;
// set acceleration offset target
_accel_offset_target_ned_mss.xy() = accel_offset_target_ne_mss;
// record time of update so we can detect timeouts
_posvelaccel_offset_target_ne_ms = AP_HAL::millis();
}
// Sets vertical offset targets (m, m/s, m/s²) relative to EKF origin in meters, Down-positive.
void AC_PosControl::set_posvelaccel_offset_target_D_m(float pos_offset_target_d_m, float vel_offset_target_d_ms, const float accel_offset_target_d_mss)
{
// set position offset target
_pos_offset_target_ned_m.z = pos_offset_target_d_m;
// set velocity offset target
_vel_offset_target_ned_ms.z = vel_offset_target_d_ms;
// set acceleration offset target
_accel_offset_target_ned_mss.z = accel_offset_target_d_mss;
// record time of update so we can detect timeouts
_posvelaccel_offset_target_d_ms = AP_HAL::millis();
}
// Returns desired thrust direction as a unit vector in the body frame.
Vector3f AC_PosControl::get_thrust_vector() const
{
Vector3f accel_target_ned_mss = get_accel_target_NED_mss();
accel_target_ned_mss.z = -GRAVITY_MSS;
return accel_target_ned_mss;
}
// Computes NE stopping point in meters based on current position, velocity, and acceleration.
void AC_PosControl::get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const
{
// Start from estimated NE position with offset removed
// todo: we should use the current target position and velocity if we are currently running the position controller
stopping_point_ne_m = _pos_estimate_ned_m.xy();
stopping_point_ne_m -= _pos_offset_ned_m.xy();
Vector2f vel_estimate_ne_ms = _vel_estimate_ned_ms.xy();
vel_estimate_ne_ms -= _vel_offset_ned_ms.xy();
// Compute velocity magnitude
float speed_ne_ms = vel_estimate_ne_ms.length();
if (!is_positive(speed_ne_ms)) {
return;
}
// Use current P gain and max accel to estimate stopping distance
float kP = _p_pos_ne_m.kP();
const float stopping_dist_m = stopping_distance(constrain_float(speed_ne_ms, 0.0, _vel_max_ne_ms), kP, _accel_max_ne_mss);
if (!is_positive(stopping_dist_m)) {
return;
}
// Project stopping distance along current velocity direction
// todo: convert velocity to a unit vector instead.
const float stopping_time_s = stopping_dist_m / speed_ne_ms;
stopping_point_ne_m += (vel_estimate_ne_ms * stopping_time_s).topostype();
}
// Computes vertical stopping point in meters based on current velocity and acceleration.
void AC_PosControl::get_stopping_point_D_m(postype_t &stopping_point_d_m) const
{
float curr_pos_d_m = _pos_estimate_ned_m.z;
curr_pos_d_m -= _pos_offset_ned_m.z;
float curr_vel_d_ms = _vel_estimate_ned_ms.z;
curr_vel_d_ms -= _vel_offset_ned_ms.z;
// If controller is unconfigured or disabled, return current position
if (!is_positive(_p_pos_d_m.kP()) || !is_positive(_accel_max_d_mss)) {
stopping_point_d_m = curr_pos_d_m;
return;
}
// Estimate stopping point using current velocity, P gain, and max vertical acceleration
stopping_point_d_m = curr_pos_d_m + constrain_float(stopping_distance(curr_vel_d_ms, _p_pos_d_m.kP(), _accel_max_d_mss), - POSCONTROL_STOPPING_DIST_UP_MAX_M, POSCONTROL_STOPPING_DIST_DOWN_MAX_M);
}
// Returns bearing from current position to position target in radians.
// 0 = North, positive = clockwise.
float AC_PosControl::get_bearing_to_target_rad() const
{
return (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()).angle();
}
///
/// System methods
///
// 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 AC_PosControl::update_estimates(bool high_vibes)
{
Vector3p pos_estimate_ned_m;
if (!AP::ahrs().get_relative_position_NED_origin(pos_estimate_ned_m)) {
float posD;
if (AP::ahrs().get_relative_position_D_origin_float(posD)) {
pos_estimate_ned_m.z = posD;
}
}
_pos_estimate_ned_m = pos_estimate_ned_m;
Vector3f vel_estimate_ned_ms;
if (!AP::ahrs().get_velocity_NED(vel_estimate_ned_ms) || high_vibes) {
float rate_z;
if (AP::ahrs().get_vert_pos_rate_D(rate_z)) {
vel_estimate_ned_ms.z = rate_z;
}
}
_vel_estimate_ned_ms = vel_estimate_ned_ms;
}
// 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 AC_PosControl::get_throttle_with_vibration_override()
{
const float thr_per_accel_d_mss = _motors.get_throttle_hover();
// Estimate throttle based on desired acceleration (manual feedforward gain).
// Used when IMU vibrations corrupt raw acceleration measurements.
// Allow integrator to compensate for velocity error only if not thrust-limited,
// or if integrator is actively helping counteract velocity error direction.
// ToDo: clear pid_info P, I and D terms for logging
if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_d_m.get_i()) && is_negative(_pid_vel_d_m.get_error())) || (is_negative(_pid_accel_d_m.get_i()) && is_positive(_pid_vel_d_m.get_error())))) {
// Adjust integrator to help reduce velocity error.
// Note: scale by velocity P-gain and an override-specific I gain.
_pid_accel_d_m.set_integrator(_pid_accel_d_m.get_i() + _dt_s * thr_per_accel_d_mss * _pid_vel_d_m.get_error() * _pid_vel_d_m.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
}
// Final throttle = P term (feedforward) + scaled I term.
return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accel_d_mss * _accel_target_ned_mss.z + _pid_accel_d_m.get_i();
}
// Resets NED position controller state to prevent transients when exiting standby.
// Zeros I-terms and aligns targets to current position.
void AC_PosControl::NED_standby_reset()
{
// Reset vertical acceleration controller integrator to prevent throttle bias on reentry
_pid_accel_d_m.set_integrator(0.0f);
// Reset position controller targets to match current estimate — avoids position jumps
_pos_target_ned_m = _pos_estimate_ned_m;
// Reset horizontal velocity controller integrator and derivative filter
_pid_vel_ne_m.reset_filter();
// Reset EKF XY position reset tracking for NE controller
NE_init_ekf_reset();
}
#if HAL_LOGGING_ENABLED
// Writes position controller diagnostic logs (PSCN, PSCE, etc).
void AC_PosControl::write_log()
{
if (NE_is_active()) {
float accel_n_mss, accel_e_mss;
lean_angles_to_accel_NE_mss(accel_n_mss, accel_e_mss);
// Log North-axis position control (PSCN): desired, target, and actual
Write_PSCN(_pos_desired_ned_m.x, _pos_target_ned_m.x, _pos_estimate_ned_m.x ,
_vel_desired_ned_ms.x, _vel_target_ned_ms.x, _vel_estimate_ned_ms.x,
_accel_desired_ned_mss.x, _accel_target_ned_mss.x, accel_n_mss);
// Log East-axis position control (PSCE): desired, target, and actual
Write_PSCE(_pos_desired_ned_m.y, _pos_target_ned_m.y, _pos_estimate_ned_m.y,
_vel_desired_ned_ms.y, _vel_target_ned_ms.y, _vel_estimate_ned_ms.y,
_accel_desired_ned_mss.y, _accel_target_ned_mss.y, accel_e_mss);
// log offsets if they are being used
if (!_pos_offset_ned_m.xy().is_zero()) {
// Log North offset tracking (PSON)
Write_PSON(_pos_offset_target_ned_m.x, _pos_offset_ned_m.x, _vel_offset_target_ned_ms.x, _vel_offset_ned_ms.x, _accel_offset_target_ned_mss.x, _accel_offset_ned_mss.x);
// Log East offset tracking (PSOE)
Write_PSOE(_pos_offset_target_ned_m.y, _pos_offset_ned_m.y, _vel_offset_target_ned_ms.y, _vel_offset_ned_ms.y, _accel_offset_target_ned_mss.y, _accel_offset_ned_mss.y);
}
}
if (D_is_active()) {
// Log Down-axis position control (PSCD)
Write_PSCD(_pos_desired_ned_m.z, _pos_target_ned_m.z, _pos_estimate_ned_m.z,
_vel_desired_ned_ms.z, _vel_target_ned_ms.z, _vel_estimate_ned_ms.z,
_accel_desired_ned_mss.z, _accel_target_ned_mss.z, get_estimated_accel_D_mss());
// log down and terrain offsets if they are being used
if (!is_zero(_pos_offset_ned_m.z)) {
// Log Down offset tracking (PSOD)
Write_PSOD(_pos_offset_target_ned_m.z, _pos_offset_ned_m.z, _vel_offset_target_ned_ms.z, _vel_offset_ned_ms.z, _accel_offset_target_ned_mss.z, _accel_offset_ned_mss.z);
}
if (!is_zero(_pos_terrain_d_m)) {
// Log terrain-following offset (PSOT)
Write_PSOT(_pos_terrain_target_d_m, _pos_terrain_d_m, 0, _vel_terrain_d_ms, 0, _accel_terrain_d_mss);
}
}
}
#endif // HAL_LOGGING_ENABLED
// Returns lateral distance to closest point on active trajectory in meters.
// Used to assess horizontal deviation from path.
float AC_PosControl::crosstrack_error_m() const
{
const Vector2f pos_error = (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()).tofloat();
if (is_zero(_vel_desired_ned_ms.xy().length_squared())) {
// No desired velocity → return direct distance to target
return pos_error.length();
} else {
// Project position error onto desired velocity vector
const Vector2f vel_unit = _vel_desired_ned_ms.xy().normalized();
const float dot_error = pos_error * vel_unit;
// Use Pythagorean difference to isolate perpendicular (cross-track) component
// todo: remove MAX of zero when safe_sqrt fixed
return safe_sqrt(MAX(pos_error.length_squared() - sq(dot_error), 0.0));
}
}
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// Returns true if the requested forward pitch is limited by the configured tilt constraint.
bool AC_PosControl::get_fwd_pitch_is_limited() const
{
if (_limit_vector_ned.xy().is_zero()) {
return false;
}
const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad());
const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad);
// Check for pitch limiting in the forward direction
const float accel_fwd_unlimited_mss = _limit_vector_ned.x * _ahrs.cos_yaw() + _limit_vector_ned.y * _ahrs.sin_yaw();
const float pitch_target_unlimited_deg = accel_mss_to_angle_deg(- MIN(accel_fwd_unlimited_mss, accel_max_mss));
const float accel_fwd_limited = _accel_target_ned_mss.x * _ahrs.cos_yaw() + _accel_target_ned_mss.y * _ahrs.sin_yaw();
const float pitch_target_limited_deg = accel_mss_to_angle_deg(- accel_fwd_limited);
return is_negative(pitch_target_unlimited_deg) && pitch_target_unlimited_deg < pitch_target_limited_deg;
}
#endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)
///
/// private methods
///
/// Terrain
// Updates terrain estimate (_pos_terrain_d_m) toward target using filter time constants.
void AC_PosControl::update_terrain()
{
// update position, velocity, acceleration offsets for this iteration
postype_t pos_terrain_d_m = _pos_terrain_d_m;
update_pos_vel_accel(pos_terrain_d_m, _vel_terrain_d_ms, _accel_terrain_d_mss, _dt_s, MIN(_limit_vector_ned.z, 0.0f), _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
_pos_terrain_d_m = pos_terrain_d_m;
// input shape horizontal position, velocity and acceleration offsets
shape_pos_vel_accel(_pos_terrain_target_d_m, 0.0, 0.0,
_pos_terrain_d_m, _vel_terrain_d_ms, _accel_terrain_d_mss,
-get_max_speed_up_ms(), get_max_speed_down_ms(),
-D_get_max_accel_mss(), D_get_max_accel_mss(),
_jerk_max_d_msss, _dt_s, false);
// we do not have to update _pos_terrain_target_d_m because we assume the target velocity and acceleration are zero
// if we know how fast the terrain altitude is changing we would add update_pos_vel_accel for _pos_terrain_target_d_m here
}
// Converts horizontal acceleration (m/s²) to roll/pitch lean angles in radians.
void AC_PosControl::accel_NE_mss_to_lean_angles_rad(float accel_n_mss, float accel_e_mss, float& roll_target_rad, float& pitch_target_rad) const
{
// rotate accelerations into body forward-right frame
const float accel_forward_mss = accel_n_mss * _ahrs.cos_yaw() + accel_e_mss * _ahrs.sin_yaw();
const float accel_right_mss = -accel_n_mss * _ahrs.sin_yaw() + accel_e_mss * _ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
pitch_target_rad = accel_mss_to_angle_rad(-accel_forward_mss);
float cos_pitch_target = cosf(pitch_target_rad);
roll_target_rad = accel_mss_to_angle_rad(accel_right_mss * cos_pitch_target);
}
// Converts current target lean angles to NE acceleration in m/s².
void AC_PosControl::lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel_e_mss) const
{
// rotate our roll, pitch angles into lat/lon frame
Vector3f att_target_euler_rad = _attitude_control.get_att_target_euler_rad();
att_target_euler_rad.z = _ahrs.yaw;
Vector3f accel_ne_mss = lean_angles_rad_to_accel_NED_mss(att_target_euler_rad);
accel_n_mss = accel_ne_mss.x;
accel_e_mss = accel_ne_mss.y;
}
// 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 AC_PosControl::calculate_yaw_and_rate_yaw()
{
// Calculate the turn rate
float turn_rate_rads = 0.0f;
const float vel_desired_length_ne_ms = _vel_desired_ned_ms.xy().length();
if (is_positive(vel_desired_length_ne_ms)) {
// Project acceleration vector into velocity direction to extract forward acceleration component
const float accel_forward_mss = (_accel_desired_ned_mss.x * _vel_desired_ned_ms.x + _accel_desired_ned_mss.y * _vel_desired_ned_ms.y) / vel_desired_length_ne_ms;
// Subtract forward component to isolate turn acceleration perpendicular to velocity vector
const Vector2f accel_turn_ne_mss = _accel_desired_ned_mss.xy() - _vel_desired_ned_ms.xy() * accel_forward_mss / vel_desired_length_ne_ms;
// Compute turn rate from lateral acceleration and velocity (centripetal formula)
const float accel_turn_length_ne_mss = accel_turn_ne_mss.length();
turn_rate_rads = accel_turn_length_ne_mss / vel_desired_length_ne_ms;
// Determine turn direction: positive = clockwise (right)
if ((accel_turn_ne_mss.y * _vel_desired_ned_ms.x - accel_turn_ne_mss.x * _vel_desired_ned_ms.y) < 0.0) {
turn_rate_rads = -turn_rate_rads;
}
}
// If vehicle is moving significantly, align yaw to velocity vector and apply computed turn rate
if (vel_desired_length_ne_ms > _vel_max_ne_ms * 0.05f) {
_yaw_target_rad = _vel_desired_ned_ms.xy().angle();
_yaw_rate_target_rads = turn_rate_rads;
return;
}
// If motion is too slow, retain last yaw target from attitude controller
_yaw_target_rad = _attitude_control.get_att_target_euler_rad().z;
_yaw_rate_target_rads = 0;
}
// Computes scaling factor to increase max vertical accel/jerk if vertical speed exceeds configured limits.
float AC_PosControl::calculate_overspeed_gain()
{
// If desired descent speed exceeds configured max, scale acceleration/jerk proportionally
if (_vel_desired_ned_ms.z > _vel_max_down_ms && !is_zero(_vel_max_down_ms)) {
return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_ned_ms.z / _vel_max_down_ms;
}
// If desired climb speed exceeds configured max, scale acceleration/jerk proportionally
if (_vel_desired_ned_ms.z < -_vel_max_up_ms && !is_zero(_vel_max_up_ms)) {
return -POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_ned_ms.z / _vel_max_up_ms;
}
// Within normal speed limits — use nominal acceleration and jerk
return 1.0;
}
// Initializes tracking of NE EKF position resets.
void AC_PosControl::NE_init_ekf_reset()
{
Vector2f pos_shift;
_ekf_ne_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
}
// Handles NE position reset detection and response (e.g., clearing accumulated errors).
void AC_PosControl::NE_handle_ekf_reset()
{
// Check for EKF-reported NE position shift since last update
Vector2f pos_shift_ne_m;
uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift_ne_m);
// todo: the actual difference in position and velocity estimation.
// This will prevent the need to pause error calculation for one cycle.
if (reset_ms != _ekf_ne_reset_ms) {
// This ensures controller output remains continuous after EKF realigns the origin.
// Reconstruct position target relative to the to new EKF estimation to maintain the current position error
Vector2p delta_pos_estimate_ne_m = _p_pos_ne_m.get_error().topostype() - (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy());
_pos_target_ned_m.xy() += delta_pos_estimate_ne_m;
// Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error
Vector2f delta_vel_estimate_ne_ms = _pid_vel_ne_m.get_error() - (_vel_target_ned_ms.xy() - _vel_estimate_ned_ms.xy());
_vel_target_ned_ms.xy() += delta_vel_estimate_ne_ms;
switch (_ekf_reset_method) {
case EKFResetMethod::MoveTarget:
// Reset NE controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc.
_pos_desired_ned_m.xy() += delta_pos_estimate_ne_m;
_vel_desired_ned_ms.xy() += delta_vel_estimate_ne_ms;
break;
case EKFResetMethod::MoveVehicle:
// Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc.
_pos_offset_ned_m.xy() += delta_pos_estimate_ne_m;
_vel_offset_ned_ms.xy() += delta_vel_estimate_ne_ms;
break;
}
_ekf_ne_reset_ms = reset_ms;
}
}
// Initializes tracking of vertical (U) EKF resets.
void AC_PosControl::D_init_ekf_reset()
{
float alt_shift_d_m;
_ekf_d_reset_ms = _ahrs.getLastPosDownReset(alt_shift_d_m);
}
// Handles U EKF reset detection and response.
void AC_PosControl::D_handle_ekf_reset()
{
// Check for EKF-reported Down-axis shift since last update
float pos_shift_d_m;
uint32_t reset_ms = _ahrs.getLastPosDownReset(pos_shift_d_m);
// todo: the actual difference in position and velocity estimation.
// This will prevent the need to pause error calculation for one cycle.
if (reset_ms != 0 && reset_ms != _ekf_d_reset_ms) {
// This ensures controller output remains continuous after EKF realigns the origin.
// Reconstruct position target relative to the to new EKF estimation to maintain the current position error
postype_t delta_pos_estimate_d_m = _p_pos_d_m.get_error() - (_pos_target_ned_m.z - _pos_estimate_ned_m.z);
_pos_target_ned_m.z += delta_pos_estimate_d_m;
// Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error
float delta_vel_estimate_d_ms = _pid_vel_d_m.get_error() - (_vel_target_ned_ms.z - _vel_estimate_ned_ms.z);
_vel_target_ned_ms.z += delta_vel_estimate_d_ms;
switch (_ekf_reset_method) {
case EKFResetMethod::MoveTarget:
// Reset U controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc.
_pos_desired_ned_m.z += delta_pos_estimate_d_m;
_vel_desired_ned_ms.z += delta_vel_estimate_d_ms;
break;
case EKFResetMethod::MoveVehicle:
// Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc.
_pos_offset_ned_m.z += delta_pos_estimate_d_m;
_vel_offset_ned_ms.z += delta_vel_estimate_d_ms;
break;
}
_ekf_d_reset_ms = reset_ms;
}
}
// Performs pre-arm checks for position control parameters and EKF readiness.
// Returns false if failure_msg is populated.
bool AC_PosControl::pre_arm_checks(const char *param_prefix,
char *failure_msg,
const uint8_t failure_msg_len)
{
if (!is_positive(NE_get_pos_p().kP())) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s_NE_POS_P must be > 0", param_prefix);
return false;
}
if (!is_positive(D_get_pos_p().kP())) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_POS_P must be > 0", param_prefix);
return false;
}
if (!is_positive(D_get_vel_pid().kP())) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_VEL_P must be > 0", param_prefix);
return false;
}
if (!is_positive(D_get_accel_pid().kP())) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_ACC_P must be > 0", param_prefix);
return false;
}
if (!is_positive(D_get_accel_pid().kI())) {
hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_ACC_I must be > 0", param_prefix);
return false;
}
return true;
}
// return true if on a real vehicle or SITL with lock-step scheduling
bool AC_PosControl::has_good_timing(void) const
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
auto *sitl = AP::sitl();
if (sitl) {
return sitl->state.is_lock_step_scheduled;
}
#endif
// real boards are assumed to have good timing
return true;
}
// perform any required parameter conversions
void AC_PosControl::convert_parameters()
{
// find param table key
uint16_t k_param_psc_key;
if (!AP_Param::find_key_by_pointer(this, k_param_psc_key)) {
return;
}
// return immediately if parameter conversion has already been performed
if (_pid_accel_d_m.kP().configured()) {
return;
}
// PARAMETER_CONVERSION - Added: Nov-2025 for 4.7
// parameters that are simply moved
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
static const AP_Param::ConversionInfo conversion_info[] = {
{ k_param_psc_key, 258257, AP_PARAM_FLOAT, "Q_P_D_VEL_P" }, // Q_P_VELZ_P moved to Q_P_D_VEL_P
{ k_param_psc_key, 4305, AP_PARAM_FLOAT, "Q_P_D_VEL_I" }, // Q_P_VELZ_I moved to Q_P_D_VEL_I
{ k_param_psc_key, 16593, AP_PARAM_FLOAT, "Q_P_D_VEL_D" }, // Q_P_VELZ_D moved to Q_P_D_VEL_D
{ k_param_psc_key, 12497, AP_PARAM_FLOAT, "Q_P_D_VEL_FLTE" }, // Q_P_VELZ_FLTE moved to Q_P_D_VEL_FLTE
{ k_param_psc_key, 20689, AP_PARAM_FLOAT, "Q_P_D_VEL_FLTD" }, // Q_P_VELZ_FLTD moved to Q_P_D_VEL_FLTD
{ k_param_psc_key, 24785, AP_PARAM_FLOAT, "Q_P_D_VEL_FF" }, // Q_P_VELZ_FF moved to Q_P_D_VEL_FF
{ k_param_psc_key, 16657, AP_PARAM_FLOAT, "Q_P_D_ACC_FF" }, // Q_P_ACCZ_FF moved to Q_P_D_ACC_FF
{ k_param_psc_key, 37137, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTT" }, // Q_P_ACCZ_FLTT moved to Q_P_D_ACC_FLTT
{ k_param_psc_key, 41233, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTE" }, // Q_P_ACCZ_FLTE moved to Q_P_D_ACC_FLTE
{ k_param_psc_key, 45329, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTD" }, // Q_P_ACCZ_FLTD moved to Q_P_D_ACC_FLTD
{ k_param_psc_key, 49425, AP_PARAM_FLOAT, "Q_P_D_ACC_SMAX" }, // Q_P_ACCZ_SMAX moved to Q_P_D_ACC_SMAX
{ k_param_psc_key, 53521, AP_PARAM_FLOAT, "Q_P_D_ACC_PDMX" }, // Q_P_ACCZ_PDMX moved to Q_P_D_ACC_PDMX
{ k_param_psc_key, 57617, AP_PARAM_FLOAT, "Q_P_D_ACC_D_FF" }, // Q_P_ACCZ_D_FF moved to Q_P_D_ACC_D_FF
{ k_param_psc_key, 65809, AP_PARAM_INT8, "Q_P_D_ACC_NEF" }, // Q_P_ACCZ_NEF moved to Q_P_D_ACC_NEF
{ k_param_psc_key, 61713, AP_PARAM_INT8, "Q_P_D_ACC_NTF" }, // Q_P_ACCZ_NTF moved to Q_P_D_ACC_NTF
{ k_param_psc_key, 258449, AP_PARAM_FLOAT, "Q_P_NE_VEL_P" }, // Q_P_VELXY_P moved to Q_P_NE_VEL_P
{ k_param_psc_key, 4497, AP_PARAM_FLOAT, "Q_P_NE_VEL_I" }, // Q_P_VELXY_I moved to Q_P_NE_VEL_I
{ k_param_psc_key, 16785, AP_PARAM_FLOAT, "Q_P_NE_VEL_D" }, // Q_P_VELXY_D moved to Q_P_NE_VEL_D
{ k_param_psc_key, 12689, AP_PARAM_FLOAT, "Q_P_NE_VEL_FLTE" },// Q_P_VELXY_FLTE moved to Q_P_NE_VEL_FLTE
{ k_param_psc_key, 20881, AP_PARAM_FLOAT, "Q_P_NE_VEL_FLTD" },// Q_P_VELXY_FLTD moved to Q_P_NE_VEL_FLTD
{ k_param_psc_key, 24977, AP_PARAM_FLOAT, "Q_P_NE_VEL_FF" }, // Q_P_VELXY_FF moved to Q_P_NE_VEL_FF
};
#else
static const AP_Param::ConversionInfo conversion_info[] = {
{ k_param_psc_key, 4035, AP_PARAM_FLOAT, "PSC_D_VEL_P" }, // PSC_VELZ_P moved to PSC_D_VEL_P
{ k_param_psc_key, 67, AP_PARAM_FLOAT, "PSC_D_VEL_I" }, // PSC_VELZ_I moved to PSC_D_VEL_I
{ k_param_psc_key, 259, AP_PARAM_FLOAT, "PSC_D_VEL_D" }, // PSC_VELZ_D moved to PSC_D_VEL_D
{ k_param_psc_key, 195, AP_PARAM_FLOAT, "PSC_D_VEL_FLTE" }, // PSC_VELZ_FLTE moved to PSC_D_VEL_FLTE
{ k_param_psc_key, 323, AP_PARAM_FLOAT, "PSC_D_VEL_FLTD" }, // PSC_VELZ_FLTD moved to PSC_D_VEL_FLTD
{ k_param_psc_key, 387, AP_PARAM_FLOAT, "PSC_D_VEL_FF" }, // PSC_VELZ_FF moved to PSC_D_VEL_FF
{ k_param_psc_key, 260, AP_PARAM_FLOAT, "PSC_D_ACC_FF" }, // PSC_ACCZ_FF moved to PSC_D_ACC_FF
{ k_param_psc_key, 580, AP_PARAM_FLOAT, "PSC_D_ACC_FLTT" }, // PSC_ACCZ_FLTT moved to PSC_D_ACC_FLTT
{ k_param_psc_key, 644, AP_PARAM_FLOAT, "PSC_D_ACC_FLTE" }, // PSC_ACCZ_FLTE moved to PSC_D_ACC_FLTE
{ k_param_psc_key, 708, AP_PARAM_FLOAT, "PSC_D_ACC_FLTD" }, // PSC_ACCZ_FLTD moved to PSC_D_ACC_FLTD
{ k_param_psc_key, 772, AP_PARAM_FLOAT, "PSC_D_ACC_SMAX" }, // PSC_ACCZ_SMAX moved to PSC_D_ACC_SMAX
{ k_param_psc_key, 836, AP_PARAM_FLOAT, "PSC_D_ACC_PDMX" }, // PSC_ACCZ_PDMX moved to PSC_D_ACC_PDMX
{ k_param_psc_key, 900, AP_PARAM_FLOAT, "PSC_D_ACC_D_FF" }, // PSC_ACCZ_D_FF moved to PSC_D_ACC_D_FF
{ k_param_psc_key, 1028, AP_PARAM_INT8, "PSC_D_ACC_NEF" }, // PSC_ACCZ_NEF moved to PSC_D_ACC_NEF
{ k_param_psc_key, 964, AP_PARAM_INT8, "PSC_D_ACC_NTF" }, // PSC_ACCZ_NTF moved to PSC_D_ACC_NTF
{ k_param_psc_key, 4038, AP_PARAM_FLOAT, "PSC_NE_VEL_P" }, // PSC_VELXY_P moved to PSC_NE_VEL_P
{ k_param_psc_key, 70, AP_PARAM_FLOAT, "PSC_NE_VEL_I" }, // PSC_VELXY_I moved to PSC_NE_VEL_I
{ k_param_psc_key, 262, AP_PARAM_FLOAT, "PSC_NE_VEL_D" }, // PSC_VELXY_D moved to PSC_NE_VEL_D
{ k_param_psc_key, 198, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTE" },// PSC_VELXY_FLTE moved to PSC_NE_VEL_FLTE
{ k_param_psc_key, 326, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTD" },// PSC_VELXY_FLTD moved to PSC_NE_VEL_FLTD
{ k_param_psc_key, 390, AP_PARAM_FLOAT, "PSC_NE_VEL_FF" }, // PSC_VELXY_FF moved to PSC_NE_VEL_FF
};
#endif
AP_Param::convert_old_parameters(conversion_info, ARRAY_SIZE(conversion_info));
// parameters moved and scaled by 0.1
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
static const AP_Param::ConversionInfo conversion_info_01[] = {
{ k_param_psc_key, 258321, AP_PARAM_FLOAT, "Q_P_D_ACC_P" }, // Q_P_ACCZ_P moved to Q_P_D_ACC_P
{ k_param_psc_key, 4369, AP_PARAM_FLOAT, "Q_P_D_ACC_I" }, // Q_P_ACCZ_I moved to Q_P_D_ACC_I
{ k_param_psc_key, 8465, AP_PARAM_FLOAT, "Q_P_D_ACC_D" }, // Q_P_ACCZ_D moved to Q_P_D_ACC_D
};
#else
static const AP_Param::ConversionInfo conversion_info_01[] = {
{ k_param_psc_key, 4036, AP_PARAM_FLOAT, "PSC_D_ACC_P" }, // PSC_ACCZ_P moved to PSC_D_ACC_P
{ k_param_psc_key, 68, AP_PARAM_FLOAT, "PSC_D_ACC_I" }, // PSC_ACCZ_I moved to PSC_D_ACC_I
{ k_param_psc_key, 132, AP_PARAM_FLOAT, "PSC_D_ACC_D" }, // PSC_ACCZ_D moved to PSC_D_ACC_D
};
#endif
AP_Param::convert_old_parameters_scaled(conversion_info_01, ARRAY_SIZE(conversion_info_01), 0.1, 0);
// store PSC_D_ACC_P as flag that parameter conversion was completed
_pid_accel_d_m.kP().save(true);
// parameters moved and scaled by 0.01
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
static const AP_Param::ConversionInfo conversion_info_001[] = {
{ k_param_psc_key, 8401, AP_PARAM_FLOAT, "Q_P_D_VEL_IMAX" }, // Q_P_VELZ_IMAX moved to Q_P_D_VEL_IMAX
{ k_param_psc_key, 8593, AP_PARAM_FLOAT, "Q_P_NE_VEL_IMAX" }, // Q_P_VELXY_IMAX moved to Q_P_NE_VEL_IMAX
};
#else
static const AP_Param::ConversionInfo conversion_info_001[] = {
{ k_param_psc_key, 131, AP_PARAM_FLOAT, "PSC_D_VEL_IMAX" }, // PSC_VELZ_IMAX moved to PSC_D_VEL_IMAX
{ k_param_psc_key, 134, AP_PARAM_FLOAT, "PSC_NE_VEL_IMAX" }, // PSC_VELXY_IMAX moved to PSC_NE_VEL_IMAX
};
#endif
AP_Param::convert_old_parameters_scaled(conversion_info_001, ARRAY_SIZE(conversion_info_001), 0.01, 0);
// parameters moved and scaled by 0.001
// PSC_ACCZ_IMAX replaced by PSC_D_ACC_IMAX
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
static const AP_Param::ConversionInfo psc_d_acc_imax_info = { k_param_psc_key, 20753, AP_PARAM_FLOAT, "Q_P_D_ACC_IMAX" };
#else
static const AP_Param::ConversionInfo psc_d_acc_imax_info = { k_param_psc_key, 324, AP_PARAM_FLOAT, "PSC_D_ACC_IMAX" };
#endif
AP_Param::convert_old_parameter(&psc_d_acc_imax_info, 0.001f);
}
Reference in New Issue View Git Blame Copy Permalink