From 0c55933378ddb94a021044449bb92bdf1a9974fa Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 30 Nov 2022 15:54:18 +1030 Subject: [PATCH] AP_Control: Support changing update period --- libraries/APM_Control/AP_PitchController.cpp | 4 +-- libraries/APM_Control/AP_PitchController.h | 2 +- libraries/APM_Control/AP_RollController.cpp | 4 +-- libraries/APM_Control/AP_RollController.h | 2 +- libraries/APM_Control/AP_YawController.cpp | 4 +-- libraries/APM_Control/AP_YawController.h | 2 +- libraries/APM_Control/AR_AttitudeControl.cpp | 28 ++++++-------------- libraries/APM_Control/AR_PosControl.cpp | 10 +++---- 8 files changed, 17 insertions(+), 39 deletions(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 502d9d9f20..bcf6419503 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -155,8 +155,6 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d float rate_y = _ahrs.get_gyro().y; float old_I = rate_pid.get_i(); - rate_pid.set_dt(dt); - bool underspeed = aspeed <= 0.5*float(aparm.airspeed_min); if (underspeed) { limit_I = true; @@ -167,7 +165,7 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d // // note that we run AC_PID in radians so that the normal scaling // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0) - rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, limit_I); + rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, dt, limit_I); if (underspeed) { // when underspeed we lock the integrator diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 167afa48cb..1b6f989669 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -56,7 +56,7 @@ private: AP_Int16 _max_rate_neg; AP_Float _roll_ff; float _last_out; - AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1}; + AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1}; float angle_err_deg; AP_PIDInfo _pid_info; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 799a9d3240..547cf1c1ba 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -140,8 +140,6 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di float aspeed; float old_I = rate_pid.get_i(); - rate_pid.set_dt(dt); - if (!_ahrs.airspeed_estimate(aspeed)) { aspeed = 0; } @@ -155,7 +153,7 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di // // note that we run AC_PID in radians so that the normal scaling // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0) - rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, limit_I); + rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, dt, limit_I); if (underspeed) { // when underspeed we lock the integrator diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 0c5d9301a7..4fe57824b7 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -61,7 +61,7 @@ private: AP_AutoTune *autotune; bool failed_autotune_alloc; float _last_out; - AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1}; + AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1}; float angle_err_deg; AP_PIDInfo _pid_info; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 493d70636e..7e4b3c86ad 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -279,8 +279,6 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa float aspeed; float old_I = rate_pid.get_i(); - rate_pid.set_dt(dt); - if (!_ahrs.airspeed_estimate(aspeed)) { aspeed = 0; } @@ -294,7 +292,7 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa // // note that we run AC_PID in radians so that the normal scaling // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0) - rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_z * scaler * scaler, limit_I); + rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_z * scaler * scaler, dt, limit_I); if (underspeed) { // when underspeed we lock the integrator diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index c4ccd84c4e..80b99c79a4 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -57,7 +57,7 @@ private: AP_Float _K_FF; AP_Int16 _imax; AP_Int8 _rate_enable; - AC_PID rate_pid{0.04, 0.15, 0, 0.15, 0.666, 3, 0, 12, 0.02, 150, 1}; + AC_PID rate_pid{0.04, 0.15, 0, 0.15, 0.666, 3, 0, 12, 150, 1}; uint32_t _last_t; float _last_out; diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 8a73c3fbef..5a3255ea29 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -467,10 +467,10 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { AR_AttitudeControl::AR_AttitudeControl() : _steer_angle_p(AR_ATTCONTROL_STEER_ANG_P), - _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_FF, AR_ATTCONTROL_STEER_RATE_IMAX, 0.0f, AR_ATTCONTROL_STEER_RATE_FILT, 0.0f, AR_ATTCONTROL_DT), - _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, 0.0f, AR_ATTCONTROL_THR_SPEED_IMAX, 0.0f, AR_ATTCONTROL_THR_SPEED_FILT, 0.0f, AR_ATTCONTROL_DT), - _pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, 0.0f, AR_ATTCONTROL_PITCH_THR_IMAX, 0.0f, AR_ATTCONTROL_PITCH_THR_FILT, 0.0f, AR_ATTCONTROL_DT), - _sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, 0.0f, AR_ATTCONTROL_HEEL_SAIL_IMAX, 0.0f, AR_ATTCONTROL_HEEL_SAIL_FILT, 0.0f, AR_ATTCONTROL_DT) + _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_FF, AR_ATTCONTROL_STEER_RATE_IMAX, 0.0f, AR_ATTCONTROL_STEER_RATE_FILT, 0.0f), + _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, 0.0f, AR_ATTCONTROL_THR_SPEED_IMAX, 0.0f, AR_ATTCONTROL_THR_SPEED_FILT, 0.0f), + _pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, 0.0f, AR_ATTCONTROL_PITCH_THR_IMAX, 0.0f, AR_ATTCONTROL_PITCH_THR_FILT, 0.0f), + _sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, 0.0f, AR_ATTCONTROL_HEEL_SAIL_IMAX, 0.0f, AR_ATTCONTROL_HEEL_SAIL_FILT, 0.0f) { AP_Param::setup_object_defaults(this, var_info); } @@ -566,10 +566,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l _desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max); } - // set PID's dt - _steer_rate_pid.set_dt(dt); - - float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), (motor_limit_left || motor_limit_right)); + float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), dt, (motor_limit_left || motor_limit_right)); output += _steer_rate_pid.get_ff(); // constrain and return final output return output; @@ -648,9 +645,6 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor // acceleration limit desired speed _desired_speed = get_desired_speed_accel_limited(desired_speed, dt); - // set PID's dt - _throttle_speed_pid.set_dt(dt); - // calculate base throttle (protect against divide by zero) float throttle_base = 0.0f; if (is_positive(cruise_speed) && is_positive(cruise_throttle)) { @@ -658,7 +652,7 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor } // calculate final output - float throttle_out = _throttle_speed_pid.update_all(_desired_speed, speed, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high)); + float throttle_out = _throttle_speed_pid.update_all(_desired_speed, speed, dt, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high)); throttle_out += _throttle_speed_pid.get_ff(); throttle_out += throttle_base; @@ -761,15 +755,12 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float _pitch_limited = false; } - // set PID's dt - _pitch_to_throttle_pid.set_dt(dt); - // initialise output to feed forward from current pitch angle const float pitch_rad = AP::ahrs().pitch; float output = sinf(pitch_rad) * _pitch_to_throttle_ff; // add regular PID control - output += _pitch_to_throttle_pid.update_all(desired_pitch, pitch_rad, motor_limit); + output += _pitch_to_throttle_pid.update_all(desired_pitch, pitch_rad, dt, motor_limit); output += _pitch_to_throttle_pid.get_ff(); // update pitch limits for next iteration @@ -821,10 +812,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt) } _heel_controller_last_ms = now; - // set PID's dt - _sailboat_heel_pid.set_dt(dt); - - _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll)); + _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll), dt); // get feed-forward const float ff = _sailboat_heel_pid.get_ff(); diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 07b7754108..6e1817ded5 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -100,8 +100,8 @@ const AP_Param::GroupInfo AR_PosControl::var_info[] = { AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) : _atc(atc), - _p_pos(AR_POSCON_POS_P, AR_POSCON_DT), - _pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D, AR_POSCON_DT) + _p_pos(AR_POSCON_POS_P), + _pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D) { AP_Param::setup_object_defaults(this, var_info); } @@ -130,10 +130,6 @@ void AR_PosControl::update(float dt) } _last_update_ms = AP_HAL::millis(); - // update P, PID object's dt - _p_pos.set_dt(dt); - _pid_vel.set_dt(dt); - // calculate position error and convert to desired velocity _vel_target.zero(); if (_pos_target_valid) { @@ -163,7 +159,7 @@ void AR_PosControl::update(float dt) // calculate desired acceleration // To-Do: fixup _limit_vel used below - _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), _limit_vel); + _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), dt, _limit_vel); if (_accel_desired_valid) { _accel_target += _accel_desired; }