From ba00c2edd600b3100bad99101ca3b3b33dc6802c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 4 Jan 2020 16:46:09 +1030 Subject: [PATCH] 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 --- .../AC_AttitudeControl/AC_PosControl.cpp | 342 ++++++++---------- libraries/AC_AttitudeControl/AC_PosControl.h | 46 +-- 2 files changed, 169 insertions(+), 219 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9d2268805a..a9245c43b4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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,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() _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() // 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; - _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) 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; isnprintf(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; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index c75c654c66..c887b77a81 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -5,7 +5,10 @@ #include #include // P library #include // PID library +#include // P library (1-axis) +#include // P library (2-axis) #include // PI library (2-axis) +#include // PID library (1-axis) #include // PID library (2-axis) #include // 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