mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AC_PosControl: alt hold controller update
pass accel limit top_pos_xy prearm check updated fix PSC_VELZ_FLTE/D param descriptions fix VelZ FF param desc limit maximum desired z component integrate AC_PID_Basic
This commit is contained in:
Leonard Hall
Randy Mackay
parent
b73ad1a1bc
commit
ba00c2edd6
@ -9,6 +9,9 @@ extern const AP_HAL::HAL& hal;
|
||||
// default gains for Plane
|
||||
# define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default
|
||||
# define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default
|
||||
# define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default
|
||||
# define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter
|
||||
# define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
|
||||
# define POSCONTROL_ACC_Z_P 0.3f // vertical acceleration controller P gain default
|
||||
# define POSCONTROL_ACC_Z_I 1.0f // vertical acceleration controller I gain default
|
||||
# define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default
|
||||
@ -26,6 +29,9 @@ extern const AP_HAL::HAL& hal;
|
||||
// default gains for Sub
|
||||
# define POSCONTROL_POS_Z_P 3.0f // vertical position controller P gain default
|
||||
# define POSCONTROL_VEL_Z_P 8.0f // vertical velocity controller P gain default
|
||||
# define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default
|
||||
# define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter
|
||||
# define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
|
||||
# define POSCONTROL_ACC_Z_P 0.5f // vertical acceleration controller P gain default
|
||||
# define POSCONTROL_ACC_Z_I 0.1f // vertical acceleration controller I gain default
|
||||
# define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default
|
||||
@ -43,6 +49,9 @@ extern const AP_HAL::HAL& hal;
|
||||
// default gains for Copter / TradHeli
|
||||
# define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default
|
||||
# define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default
|
||||
# define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default
|
||||
# define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter
|
||||
# define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
|
||||
# define POSCONTROL_ACC_Z_P 0.5f // vertical acceleration controller P gain default
|
||||
# define POSCONTROL_ACC_Z_I 1.0f // vertical acceleration controller I gain default
|
||||
# define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default
|
||||
@ -79,14 +88,55 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
|
||||
// @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
|
||||
// @Range: 1.000 3.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_pos_z, "_POSZ_", 2, AC_PosControl, AC_P),
|
||||
AP_SUBGROUPINFO(_p_pos_z, "_POSZ_", 2, AC_PosControl, AC_P_1D),
|
||||
|
||||
// @Param: _VELZ_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
|
||||
// @Range: 1.000 8.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_vel_z, "_VELZ_", 3, AC_PosControl, AC_P),
|
||||
|
||||
// @Param: _VELZ_I
|
||||
// @DisplayName: Velocity (vertical) controller I gain
|
||||
// @Description: Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration
|
||||
// @Range: 0.02 1.00
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _VELZ_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
|
||||
// @Range: 1.000 8.000
|
||||
// @User: Standard
|
||||
|
||||
// @Param: _VELZ_D
|
||||
// @DisplayName: Velocity (vertical) controller D gain
|
||||
// @Description: Velocity (vertical) controller D gain. Corrects short-term changes in velocity
|
||||
// @Range: 0.00 1.00
|
||||
// @Increment: 0.001
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _VELZ_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
|
||||
// @Range: 0 1
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _VELZ_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
|
||||
// @Range: 0 100
|
||||
// @Units: Hz
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _VELZ_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
|
||||
// @Range: 0 100
|
||||
// @Units: Hz
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(_pid_vel_z, "_VELZ_", 3, AC_PosControl, AC_PID_Basic),
|
||||
|
||||
// @Param: _ACCZ_P
|
||||
// @DisplayName: Acceleration (vertical) controller P gain
|
||||
@ -159,18 +209,18 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
|
||||
// @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
|
||||
// @Range: 0.500 2.000
|
||||
// @User: Standard
|
||||
AP_SUBGROUPINFO(_p_pos_xy, "_POSXY_", 5, AC_PosControl, AC_P),
|
||||
AP_SUBGROUPINFO(_p_pos_xy, "_POSXY_", 5, AC_PosControl, AC_P_2D),
|
||||
|
||||
// @Param: _VELXY_P
|
||||
// @DisplayName: Velocity (horizontal) P gain
|
||||
// @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration
|
||||
// @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration
|
||||
// @Range: 0.1 6.0
|
||||
// @Increment: 0.1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _VELXY_I
|
||||
// @DisplayName: Velocity (horizontal) I gain
|
||||
// @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration
|
||||
// @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
|
||||
// @Range: 0.02 1.00
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
@ -203,6 +253,13 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
|
||||
// @Range: 0 100
|
||||
// @Units: Hz
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: _VELXY_FF
|
||||
// @DisplayName: Velocity (horizontal) feed forward gains
|
||||
// @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration
|
||||
// @Range: 0 6
|
||||
// @Increment: 0.01
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(_pid_vel_xy, "_VELXY_", 6, AC_PosControl, AC_PID_2D),
|
||||
|
||||
// @Param: _ANGLE_MAX
|
||||
@ -227,11 +284,11 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
|
||||
_inav(inav),
|
||||
_motors(motors),
|
||||
_attitude_control(attitude_control),
|
||||
_p_pos_z(POSCONTROL_POS_Z_P),
|
||||
_p_vel_z(POSCONTROL_VEL_Z_P),
|
||||
_pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f, POSCONTROL_ACC_Z_DT),
|
||||
_p_pos_xy(POSCONTROL_POS_XY_P),
|
||||
_pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, POSCONTROL_DT_50HZ),
|
||||
_p_pos_z(POSCONTROL_POS_Z_P, POSCONTROL_DT_400HZ),
|
||||
_pid_vel_z(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ, POSCONTROL_DT_400HZ),
|
||||
_pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f, POSCONTROL_DT_400HZ),
|
||||
_p_pos_xy(POSCONTROL_POS_XY_P, POSCONTROL_DT_400HZ),
|
||||
_pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, POSCONTROL_DT_400HZ),
|
||||
_dt(POSCONTROL_DT_400HZ),
|
||||
_speed_down_cms(POSCONTROL_SPEED_DOWN),
|
||||
_speed_up_cms(POSCONTROL_SPEED_UP),
|
||||
@ -251,8 +308,6 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
|
||||
_flags.reset_desired_vel_to_pos = true;
|
||||
_flags.reset_accel_to_lean_xy = true;
|
||||
_flags.reset_rate_to_accel_z = true;
|
||||
_flags.freeze_ff_z = true;
|
||||
_flags.use_desvel_ff_z = true;
|
||||
_limit.pos_up = true;
|
||||
_limit.pos_down = true;
|
||||
_limit.vel_up = true;
|
||||
@ -270,7 +325,10 @@ void AC_PosControl::set_dt(float delta_sec)
|
||||
_dt = delta_sec;
|
||||
|
||||
// update PID controller dt
|
||||
_p_pos_z.set_dt(_dt);
|
||||
_pid_vel_z.set_dt(_dt);
|
||||
_pid_accel_z.set_dt(_dt);
|
||||
_p_pos_xy.set_dt(_dt);
|
||||
_pid_vel_xy.set_dt(_dt);
|
||||
|
||||
// update rate z-axis velocity error and accel error filters
|
||||
@ -318,20 +376,14 @@ void AC_PosControl::set_max_accel_z(float accel_cmss)
|
||||
void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
|
||||
{
|
||||
float alt_change = alt_cm - _pos_target.z;
|
||||
|
||||
// do not use z-axis desired velocity feed forward
|
||||
_flags.use_desvel_ff_z = false;
|
||||
_vel_desired.z = 0.0f;
|
||||
|
||||
// adjust desired alt if motors have not hit their limits
|
||||
if ((alt_change < 0 && !_motors.limit.throttle_lower) || (alt_change > 0 && !_motors.limit.throttle_upper)) {
|
||||
if (!is_zero(dt)) {
|
||||
float climb_rate_cms = constrain_float(alt_change / dt, _speed_down_cms, _speed_up_cms);
|
||||
_pos_target.z += climb_rate_cms * dt;
|
||||
_vel_desired.z = climb_rate_cms; // recorded for reporting purposes
|
||||
}
|
||||
} else {
|
||||
// recorded for reporting purposes
|
||||
_vel_desired.z = 0.0f;
|
||||
}
|
||||
|
||||
// do not let target get too far from current altitude
|
||||
@ -353,8 +405,7 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
|
||||
|
||||
// do not use z-axis desired velocity feed forward
|
||||
// vel_desired set to desired climb rate for reporting and land-detector
|
||||
_flags.use_desvel_ff_z = false;
|
||||
_vel_desired.z = climb_rate_cms;
|
||||
_vel_desired.z = 0.0f;
|
||||
}
|
||||
|
||||
/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
|
||||
@ -377,14 +428,23 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa
|
||||
// jerk_z is calculated to reach full acceleration in 1000ms.
|
||||
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
|
||||
|
||||
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f * fabsf(_vel_desired.z - climb_rate_cms) * jerk_z));
|
||||
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f * fabsf(climb_rate_cms - _vel_desired.z) * jerk_z));
|
||||
|
||||
// jerk limit the acceleration increase
|
||||
_accel_last_z_cms += jerk_z * dt;
|
||||
_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
|
||||
// jerk limit the decrease as zero error is approached
|
||||
_accel_last_z_cms = MIN(_accel_last_z_cms, accel_z_max);
|
||||
// remove overshoot during last time step
|
||||
_accel_last_z_cms = MIN(_accel_last_z_cms, fabsf(climb_rate_cms- _vel_desired.z) / dt);
|
||||
|
||||
if (is_positive(climb_rate_cms- _vel_desired.z)){
|
||||
_accel_desired.z = _accel_last_z_cms;
|
||||
} else {
|
||||
_accel_desired.z = -_accel_last_z_cms;
|
||||
}
|
||||
|
||||
float vel_change_limit = _accel_last_z_cms * dt;
|
||||
_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z - vel_change_limit, _vel_desired.z + vel_change_limit);
|
||||
_flags.use_desvel_ff_z = true;
|
||||
|
||||
// adjust desired alt if motors have not hit their limits
|
||||
// To-Do: add check of _limit.pos_down?
|
||||
@ -405,11 +465,6 @@ void AC_PosControl::add_takeoff_climb_rate(float climb_rate_cms, float dt)
|
||||
void AC_PosControl::shift_alt_target(float z_cm)
|
||||
{
|
||||
_pos_target.z += z_cm;
|
||||
|
||||
// freeze feedforward to avoid jump
|
||||
if (!is_zero(z_cm)) {
|
||||
freeze_ff_z();
|
||||
}
|
||||
}
|
||||
|
||||
/// relax_alt_hold_controllers - set all desired and targets to measured
|
||||
@ -417,9 +472,7 @@ void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
|
||||
{
|
||||
_pos_target.z = _inav.get_altitude();
|
||||
_vel_desired.z = 0.0f;
|
||||
_flags.use_desvel_ff_z = false;
|
||||
_vel_target.z = _inav.get_velocity_z();
|
||||
_vel_last.z = _inav.get_velocity_z();
|
||||
_accel_desired.z = 0.0f;
|
||||
_accel_last_z_cms = 0.0f;
|
||||
_flags.reset_rate_to_accel_z = true;
|
||||
@ -454,11 +507,8 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
|
||||
|
||||
// if position controller is active add current velocity error to avoid sudden jump in acceleration
|
||||
if (is_active_z()) {
|
||||
curr_vel_z += _vel_error.z;
|
||||
if (_flags.use_desvel_ff_z) {
|
||||
curr_vel_z -= _vel_desired.z;
|
||||
}
|
||||
}
|
||||
|
||||
// avoid divide by zero by using current position if kP is very low or acceleration is zero
|
||||
if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) {
|
||||
@ -490,9 +540,6 @@ void AC_PosControl::init_takeoff()
|
||||
|
||||
_pos_target.z = curr_pos.z;
|
||||
|
||||
// freeze feedforward to avoid jump
|
||||
freeze_ff_z();
|
||||
|
||||
// shift difference between last motor out and hover throttle into accelerometer I
|
||||
_pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
|
||||
|
||||
@ -546,124 +593,66 @@ void AC_PosControl::calc_leash_length_z()
|
||||
// vel_up_max, vel_down_max should have already been set before calling this method
|
||||
void AC_PosControl::run_z_controller()
|
||||
{
|
||||
// Position Controller
|
||||
|
||||
float curr_alt = _inav.get_altitude();
|
||||
|
||||
// clear position limit flags
|
||||
_limit.pos_up = false;
|
||||
_limit.pos_down = false;
|
||||
|
||||
// calculate altitude error
|
||||
_pos_error.z = _pos_target.z - curr_alt;
|
||||
|
||||
// do not let target altitude get too far from current altitude
|
||||
if (_pos_error.z > _leash_up_z) {
|
||||
_pos_target.z = curr_alt + _leash_up_z;
|
||||
_pos_error.z = _leash_up_z;
|
||||
_limit.pos_up = true;
|
||||
}
|
||||
if (_pos_error.z < -_leash_down_z) {
|
||||
_pos_target.z = curr_alt - _leash_down_z;
|
||||
_pos_error.z = -_leash_down_z;
|
||||
_limit.pos_down = true;
|
||||
}
|
||||
|
||||
// calculate _vel_target.z using from _pos_error.z using sqrt controller
|
||||
_vel_target.z = sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
|
||||
|
||||
// check speed limits
|
||||
// To-Do: check these speed limits here or in the pos->rate controller
|
||||
_limit.vel_up = false;
|
||||
_limit.vel_down = false;
|
||||
if (_vel_target.z < _speed_down_cms) {
|
||||
_vel_target.z = _speed_down_cms;
|
||||
_limit.vel_down = true;
|
||||
}
|
||||
if (_vel_target.z > _speed_up_cms) {
|
||||
_vel_target.z = _speed_up_cms;
|
||||
_limit.vel_up = true;
|
||||
}
|
||||
|
||||
// define maximum position error and maximum first and second differential limits
|
||||
_p_pos_z.set_limits_error(-fabsf(_leash_down_z), _leash_up_z, -fabsf(_speed_down_cms), _speed_up_cms);
|
||||
// calculate the target velocity correction
|
||||
_vel_target.z = _p_pos_z.update_all(_pos_target.z, curr_alt, _limit.pos_down, _limit.pos_up);
|
||||
// add feed forward component
|
||||
if (_flags.use_desvel_ff_z) {
|
||||
_vel_target.z += _vel_desired.z;
|
||||
}
|
||||
_vel_target.z += constrain_float(_vel_desired.z, -fabsf(_speed_down_cms), _speed_up_cms);
|
||||
|
||||
// the following section calculates acceleration required to achieve the velocity target
|
||||
// Velocity Controller
|
||||
|
||||
const Vector3f& curr_vel = _inav.get_velocity();
|
||||
|
||||
// TODO: remove velocity derivative calculation
|
||||
// reset last velocity target to current target
|
||||
if (_flags.reset_rate_to_accel_z) {
|
||||
_vel_last.z = _vel_target.z;
|
||||
}
|
||||
|
||||
// feed forward desired acceleration calculation
|
||||
if (_dt > 0.0f) {
|
||||
if (!_flags.freeze_ff_z) {
|
||||
_accel_desired.z = (_vel_target.z - _vel_last.z) / _dt;
|
||||
} else {
|
||||
// stop the feed forward being calculated during a known discontinuity
|
||||
_flags.freeze_ff_z = false;
|
||||
}
|
||||
} else {
|
||||
_accel_desired.z = 0.0f;
|
||||
}
|
||||
|
||||
// store this iteration's velocities for the next iteration
|
||||
_vel_last.z = _vel_target.z;
|
||||
|
||||
// reset velocity error and filter if this controller has just been engaged
|
||||
if (_flags.reset_rate_to_accel_z) {
|
||||
// Reset Filter
|
||||
_vel_error.z = 0;
|
||||
_vel_error_filter.reset(0);
|
||||
_flags.reset_rate_to_accel_z = false;
|
||||
} else {
|
||||
// calculate rate error and filter with cut off frequency of 2 Hz
|
||||
_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
|
||||
}
|
||||
|
||||
_accel_target.z = _p_vel_z.get_p(_vel_error.z);
|
||||
|
||||
_accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z);
|
||||
_accel_target.z += _accel_desired.z;
|
||||
|
||||
|
||||
// Acceleration Controller
|
||||
|
||||
// Calculate Earth Frame Z acceleration
|
||||
const float z_accel_meas = get_z_accel_cmss();
|
||||
|
||||
|
||||
// ensure imax is always large enough to overpower hover throttle
|
||||
if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
|
||||
_pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
|
||||
}
|
||||
|
||||
float thr_out;
|
||||
if (_vibe_comp_enabled) {
|
||||
_flags.freeze_ff_z = true;
|
||||
thr_out = get_throttle_with_vibration_override();
|
||||
} else {
|
||||
thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
|
||||
thr_out += _pid_accel_z.get_ff() * 0.001f;
|
||||
}
|
||||
thr_out += _motors.get_throttle_hover();
|
||||
|
||||
// Actuator commands
|
||||
|
||||
// send throttle to attitude controller with angle boost
|
||||
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
|
||||
|
||||
// Check for vertical controller health
|
||||
|
||||
// _speed_down_cms is checked to be non-zero when set
|
||||
float error_ratio = _vel_error.z/_speed_down_cms;
|
||||
_vel_z_control_ratio += _dt*0.1f*(0.5-error_ratio);
|
||||
_vel_z_control_ratio = constrain_float(_vel_z_control_ratio, 0.0f, 2.0f);
|
||||
}
|
||||
|
||||
// get throttle using vibration resistant calculation (uses feed forward with manually calculated gain)
|
||||
float AC_PosControl::get_throttle_with_vibration_override()
|
||||
{
|
||||
_accel_desired.z = 0.0f;
|
||||
const float thr_per_accelz_cmss = _motors.get_throttle_hover() / (GRAVITY_MSS * 100.0f);
|
||||
// during vibration compensation use feed forward with manually calculated gain
|
||||
// ToDo: clear pid_info P, I and D terms for logging
|
||||
if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_z.get_i()) && is_negative(_vel_error.z)) || (is_negative(_pid_accel_z.get_i()) && is_positive(_vel_error.z)))) {
|
||||
_pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _vel_error.z * _p_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
|
||||
_pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _vel_error.z * _pid_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
|
||||
}
|
||||
thr_out = POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f;
|
||||
} else {
|
||||
thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
|
||||
return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f;
|
||||
}
|
||||
thr_out += _motors.get_throttle_hover();
|
||||
|
||||
// send throttle to attitude controller with angle boost
|
||||
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
|
||||
|
||||
// _speed_down_cms is checked to be non-zero when set
|
||||
float error_ratio = _vel_error.z/_speed_down_cms;
|
||||
|
||||
_vel_z_control_ratio += _dt*0.1f*(0.5-error_ratio);
|
||||
_vel_z_control_ratio = constrain_float(_vel_z_control_ratio, 0.0f, 2.0f);
|
||||
}
|
||||
|
||||
///
|
||||
/// lateral position controller
|
||||
@ -698,8 +687,6 @@ void AC_PosControl::set_max_speed_xy(float speed_cms)
|
||||
void AC_PosControl::set_pos_target(const Vector3f& position)
|
||||
{
|
||||
_pos_target = position;
|
||||
|
||||
_flags.use_desvel_ff_z = false;
|
||||
_vel_desired.z = 0.0f;
|
||||
// initialise roll and pitch to current roll and pitch. This avoids a twitch between when the target is set and the pos controller is first run
|
||||
// To-Do: this initialisation of roll and pitch targets needs to go somewhere between when pos-control is initialised and when it completes it's first cycle
|
||||
@ -707,6 +694,14 @@ void AC_PosControl::set_pos_target(const Vector3f& position)
|
||||
//_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
|
||||
}
|
||||
|
||||
/// set position, velocity and acceleration targets
|
||||
void AC_PosControl::set_pos_vel_accel_target(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel)
|
||||
{
|
||||
_pos_target = pos;
|
||||
_vel_desired = vel;
|
||||
_accel_desired = accel;
|
||||
}
|
||||
|
||||
/// set_xy_target in cm from home
|
||||
void AC_PosControl::set_xy_target(float x, float y)
|
||||
{
|
||||
@ -1007,36 +1002,20 @@ void AC_PosControl::run_xy_controller(float dt)
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
Vector3f curr_pos = _inav.get_position();
|
||||
float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
|
||||
// Position Controller
|
||||
|
||||
// avoid divide by zero
|
||||
if (kP <= 0.0f) {
|
||||
_vel_target.x = 0.0f;
|
||||
_vel_target.y = 0.0f;
|
||||
} else {
|
||||
// calculate distance error
|
||||
_pos_error.x = _pos_target.x - curr_pos.x;
|
||||
_pos_error.y = _pos_target.y - curr_pos.y;
|
||||
|
||||
// Constrain _pos_error and target position
|
||||
// Constrain the maximum length of _vel_target to the maximum position correction velocity
|
||||
// TODO: replace the leash length with a user definable maximum position correction
|
||||
if (limit_vector_length(_pos_error.x, _pos_error.y, _leash)) {
|
||||
_pos_target.x = curr_pos.x + _pos_error.x;
|
||||
_pos_target.y = curr_pos.y + _pos_error.y;
|
||||
}
|
||||
|
||||
_vel_target = sqrt_controller_3D(_pos_error, kP, _accel_cms);
|
||||
}
|
||||
const Vector3f &curr_pos = _inav.get_position();
|
||||
Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos, _leash, _accel_cms);
|
||||
|
||||
// add velocity feed-forward
|
||||
vel_target *= ekfNavVelGainScaler;
|
||||
_vel_target.x = vel_target.x;
|
||||
_vel_target.y = vel_target.y;
|
||||
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
|
||||
_vel_target.x += _vel_desired.x;
|
||||
_vel_target.y += _vel_desired.y;
|
||||
|
||||
// the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
|
||||
|
||||
Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
|
||||
// Velocity Controller
|
||||
|
||||
// check if vehicle velocity is being overridden
|
||||
if (_flags.vehicle_horiz_vel_override) {
|
||||
@ -1045,36 +1024,13 @@ void AC_PosControl::run_xy_controller(float dt)
|
||||
_vehicle_horiz_vel.x = _inav.get_velocity().x;
|
||||
_vehicle_horiz_vel.y = _inav.get_velocity().y;
|
||||
}
|
||||
|
||||
// calculate velocity error
|
||||
_vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
|
||||
_vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;
|
||||
// TODO: constrain velocity error and velocity target
|
||||
|
||||
// call pi controller
|
||||
_pid_vel_xy.set_input(_vel_error);
|
||||
|
||||
// get p
|
||||
vel_xy_p = _pid_vel_xy.get_p();
|
||||
|
||||
// update i term if we have not hit the accel or throttle limits OR the i term will reduce
|
||||
// TODO: move limit handling into the PI and PID controller
|
||||
if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
|
||||
vel_xy_i = _pid_vel_xy.get_i();
|
||||
} else {
|
||||
vel_xy_i = _pid_vel_xy.get_i_shrink();
|
||||
}
|
||||
|
||||
// get d
|
||||
vel_xy_d = _pid_vel_xy.get_d();
|
||||
|
||||
Vector2f accel_target = _pid_vel_xy.update_all(Vector2f(_vel_target.x, _vel_target.y), _vehicle_horiz_vel, _limit.accel_xy);
|
||||
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
|
||||
accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
|
||||
accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
|
||||
accel_target *= ekfNavVelGainScaler;
|
||||
|
||||
// reset accel to current desired acceleration
|
||||
if (_flags.reset_accel_to_lean_xy) {
|
||||
_accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
|
||||
_accel_target_filter.reset(accel_target);
|
||||
_flags.reset_accel_to_lean_xy = false;
|
||||
}
|
||||
|
||||
@ -1090,7 +1046,7 @@ void AC_PosControl::run_xy_controller(float dt)
|
||||
_accel_target.x += _accel_desired.x;
|
||||
_accel_target.y += _accel_desired.y;
|
||||
|
||||
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
|
||||
// Acceleration Controller
|
||||
|
||||
// limit acceleration using maximum lean angles
|
||||
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
|
||||
@ -1204,24 +1160,18 @@ bool AC_PosControl::pre_arm_checks(const char *param_prefix,
|
||||
char *failure_msg,
|
||||
const uint8_t failure_msg_len)
|
||||
{
|
||||
// validate AC_P members:
|
||||
const struct {
|
||||
const char *pid_name;
|
||||
AC_P &p;
|
||||
} ps[] = {
|
||||
{ "POSXY", get_pos_xy_p() },
|
||||
{ "POSZ", get_pos_z_p() },
|
||||
{ "VELZ", get_vel_z_p() },
|
||||
};
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(ps); i++) {
|
||||
// all AC_P's must have a positive P value:
|
||||
if (!is_positive(ps[i].p.kP())) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, ps[i].pid_name);
|
||||
if (!is_positive(get_pos_xy_p().kP())) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "%s_POSXY_P must be > 0", param_prefix);
|
||||
return false;
|
||||
}
|
||||
if (!is_positive(get_pos_z_p().kP())) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "%s_POSZ_P must be > 0", param_prefix);
|
||||
return false;
|
||||
}
|
||||
if (!is_positive(get_vel_z_pid().kP())) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "%s_VELZ_P must be > 0", param_prefix);
|
||||
return false;
|
||||
}
|
||||
|
||||
// z-axis acceleration control PID doesn't use FF, so P and I must be positive
|
||||
if (!is_positive(get_accel_z_pid().kP())) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "%s_ACCZ_P must be > 0", param_prefix);
|
||||
return false;
|
||||
|
||||
@ -5,7 +5,10 @@
|
||||
#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_InertialNav/AP_InertialNav.h> // Inertial Navigation library
|
||||
#include "AC_AttitudeControl.h" // Attitude control library
|
||||
@ -151,9 +154,6 @@ public:
|
||||
float get_leash_down_z() const { return _leash_down_z; }
|
||||
float get_leash_up_z() const { return _leash_up_z; }
|
||||
|
||||
/// freeze_ff_z - used to stop the feed forward being calculated during a known discontinuity
|
||||
void freeze_ff_z() { _flags.freeze_ff_z = true; }
|
||||
|
||||
///
|
||||
/// xy position controller
|
||||
///
|
||||
@ -200,6 +200,9 @@ public:
|
||||
/// set_pos_target in cm from home
|
||||
void set_pos_target(const Vector3f& position);
|
||||
|
||||
/// set position, velocity and acceleration targets
|
||||
void set_pos_vel_accel_target(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel);
|
||||
|
||||
/// set_xy_target in cm from home
|
||||
void set_xy_target(float x, float y);
|
||||
|
||||
@ -212,9 +215,6 @@ public:
|
||||
/// set_desired_velocity_z - sets desired velocity in cm/s in z axis
|
||||
void set_desired_velocity_z(float vel_z_cms) {_vel_desired.z = vel_z_cms;}
|
||||
|
||||
// clear desired velocity feed-forward in z axis
|
||||
void clear_desired_velocity_ff_z() { _flags.use_desvel_ff_z = false; }
|
||||
|
||||
// set desired acceleration in cm/s in xy axis
|
||||
void set_desired_accel_xy(float accel_lat_cms, float accel_lon_cms) { _accel_desired.x = accel_lat_cms; _accel_desired.y = accel_lon_cms; }
|
||||
|
||||
@ -277,11 +277,11 @@ public:
|
||||
float get_leash_xy() const { return _leash; }
|
||||
|
||||
/// get pid controllers
|
||||
AC_P& get_pos_z_p() { return _p_pos_z; }
|
||||
AC_P& get_vel_z_p() { return _p_vel_z; }
|
||||
AC_PID& get_accel_z_pid() { return _pid_accel_z; }
|
||||
AC_P& get_pos_xy_p() { return _p_pos_xy; }
|
||||
AC_P_2D& get_pos_xy_p() { return _p_pos_xy; }
|
||||
AC_P_1D& get_pos_z_p() { return _p_pos_z; }
|
||||
AC_PID_2D& get_vel_xy_pid() { return _pid_vel_xy; }
|
||||
AC_PID_Basic& get_vel_z_pid() { return _pid_vel_z; }
|
||||
AC_PID& get_accel_z_pid() { return _pid_accel_z; }
|
||||
|
||||
/// accessors for reporting
|
||||
const Vector3f& get_vel_target() const { return _vel_target; }
|
||||
@ -317,18 +317,16 @@ protected:
|
||||
uint16_t reset_desired_vel_to_pos : 1; // 1 if we should reset the rate_to_accel_xy step
|
||||
uint16_t reset_accel_to_lean_xy : 1; // 1 if we should reset the accel to lean angle step
|
||||
uint16_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step
|
||||
uint16_t freeze_ff_z : 1; // 1 used to freeze velocity to accel feed forward for one iteration
|
||||
uint16_t use_desvel_ff_z : 1; // 1 to use z-axis desired velocity as feed forward into velocity step
|
||||
uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
|
||||
} _flags;
|
||||
|
||||
// limit flags structure
|
||||
struct poscontrol_limit_flags {
|
||||
uint8_t pos_up : 1; // 1 if we have hit the vertical position leash limit while going up
|
||||
uint8_t pos_down : 1; // 1 if we have hit the vertical position leash limit while going down
|
||||
uint8_t vel_up : 1; // 1 if we have hit the vertical velocity limit going up
|
||||
uint8_t vel_down : 1; // 1 if we have hit the vertical velocity limit going down
|
||||
uint8_t accel_xy : 1; // 1 if we have hit the horizontal accel limit
|
||||
bool pos_up; // true if we have hit the vertical position leash limit while going up
|
||||
bool pos_down; // true if we have hit the vertical position leash limit while going down
|
||||
bool vel_up; // true if we have hit the vertical velocity limit going up
|
||||
bool vel_down; // true if we have hit the vertical velocity limit going down
|
||||
bool accel_xy; // true if we have hit the horizontal accel limit
|
||||
} _limit;
|
||||
|
||||
///
|
||||
@ -342,6 +340,9 @@ protected:
|
||||
// init_takeoff
|
||||
void run_z_controller();
|
||||
|
||||
// get throttle using vibration resistant calculation (uses feed forward with manually calculated gain)
|
||||
float get_throttle_with_vibration_override();
|
||||
|
||||
// get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
|
||||
float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; }
|
||||
|
||||
@ -380,11 +381,11 @@ protected:
|
||||
// parameters
|
||||
AP_Float _accel_xy_filt_hz; // XY acceleration filter cutoff frequency
|
||||
AP_Float _lean_angle_max; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
|
||||
AC_P _p_pos_z;
|
||||
AC_P _p_vel_z;
|
||||
AC_PID _pid_accel_z;
|
||||
AC_P _p_pos_xy;
|
||||
AC_PID_2D _pid_vel_xy;
|
||||
AC_P_2D _p_pos_xy; // XY axis position controller to convert distance error to desired velocity
|
||||
AC_P_1D _p_pos_z; // Z axis position controller to convert altitude error to desired climb rate
|
||||
AC_PID_2D _pid_vel_xy; // XY axis velocity controller to convert velocity error to desired acceleration
|
||||
AC_PID_Basic _pid_vel_z; // Z axis velocity controller to convert climb rate error to desired acceleration
|
||||
AC_PID _pid_accel_z; // Z axis acceleration controller to convert desired acceleration to throttle output
|
||||
|
||||
// internal variables
|
||||
float _dt; // time difference (in seconds) between calls from the main program
|
||||
@ -410,7 +411,6 @@ protected:
|
||||
Vector3f _vel_desired; // desired velocity in cm/s
|
||||
Vector3f _vel_target; // velocity target in cm/s calculated by pos_to_rate step
|
||||
Vector3f _vel_error; // error between desired and actual acceleration in cm/s
|
||||
Vector3f _vel_last; // previous iterations velocity in cm/s
|
||||
Vector3f _accel_desired; // desired acceleration in cm/s/s (feed forward)
|
||||
Vector3f _accel_target; // acceleration target in cm/s/s
|
||||
Vector3f _accel_error; // acceleration error in cm/s/s
|
||||
|
||||
Loading…
Reference in New Issue
Block a user