zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
zr-v5.1
Leonard Hall 5 years ago committed by Randy Mackay
parent
b73ad1a1bc
commit
ba00c2edd6
2 changed files with 169 additions and 219 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 342
      libraries/AC_AttitudeControl/AC_PosControl.cpp
  2. 46
      libraries/AC_AttitudeControl/AC_PosControl.h

342
libraries/AC_AttitudeControl/AC_PosControl.cpp
Unescape View File

@ -9,6 +9,9 @@ extern const AP_HAL::HAL& hal; @@ -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; @@ -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; @@ -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[] = { @@ -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[] = { @@ -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[] = { @@ -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, @@ -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, @@ -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) @@ -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) @@ -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 @@ -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 @@ -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) @@ -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) @@ -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,10 +507,7 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const @@ -454,10 +507,7 @@ 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;
}
curr_vel_z -= _vel_desired.z;
}
// avoid divide by zero by using current position if kP is very low or acceleration is zero
@ -490,9 +540,6 @@ void AC_PosControl::init_takeoff() @@ -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,125 +593,67 @@ void AC_PosControl::calc_leash_length_z() @@ -546,125 +593,67 @@ 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()
{
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;
}
// Position Controller
float curr_alt = _inav.get_altitude();
// 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;
_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);
}
thr_out = POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f;
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 * _pid_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
}
return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f;
}
///
/// lateral position controller
///
@ -698,8 +687,6 @@ void AC_PosControl::set_max_speed_xy(float speed_cms) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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, @@ -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);
return false;
}
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;

46
libraries/AC_AttitudeControl/AC_PosControl.h
Unescape View File

@ -5,7 +5,10 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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

Write Preview
Loading…
Cancel
Save