diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index b9a6d90c89..7881e15b9c 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -345,7 +345,7 @@ const struct LogStructure Plane::log_structure[] = { // @Field: Action: action taken // @Field: RMAX: Rate maximum { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP), - "ATRP", "QBBffffffffBf", "TimeUS,Axis,State,Sur,PSlew,DSlew,FF0,FF,P,I,D,Action,RMAX", "s#-dkk------k", "F--00000000-0" , true }, + "ATRP", "QBBffffffffBff", "TimeUS,Axis,State,Sur,PSlew,DSlew,FF0,FF,P,I,D,Action,RMAX,TAU", "s#-dkk------ks", "F--00000000-00" , true }, // @LoggerMessage: STAT // @Description: Current status of the aircraft diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 0b5eb0e08c..4564cd6244 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -178,7 +178,6 @@ class ModeAuto : public Mode void _exit() override; }; - class ModeAutoTune : public Mode { public: diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7359f3b100..6616ed9981 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4340,8 +4340,8 @@ void QuadPlane::setup_rp_fw_angle_gains(void) { const float mc_angR = attitude_control->get_angle_roll_p().kP(); const float mc_angP = attitude_control->get_angle_pitch_p().kP(); - const float fw_angR = 2; - const float fw_angP = 2; + const float fw_angR = 1.0f/plane.rollController.tau(); + const float fw_angP = 1.0f/plane.pitchController.tau(); if (!is_positive(mc_angR) || !is_positive(mc_angP)) { // bad configuration, don't scale diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index bb321d66a3..cc9d03da27 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -44,10 +44,11 @@ extern const AP_HAL::HAL& hal; #define TRIM_TCONST 1.0f // constructor -AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, +AP_AutoTune::AP_AutoTune(ATGains &_gains, AP_Float *_pidP, ATType _type, const AP_Vehicle::FixedWing &parms, AC_PID &_rpid) : current(_gains), + pidP(_pidP), rpid(_rpid), type(_type), aparm(parms), @@ -219,7 +220,7 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) // thresholds for when we consider an event to start and end - const float rate_threshold1 = 0.4 * MIN(att_limit_deg / 0.5f, current.rmax_pos); + const float rate_threshold1 = 0.4 * MIN(att_limit_deg / current.tau, current.rmax_pos); const float rate_threshold2 = 0.25 * rate_threshold1; bool in_att_demand = fabsf(angle_err_deg) >= 0.3 * att_limit_deg; @@ -261,7 +262,8 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) I: current.I, D: current.D, action: uint8_t(action), - rmax: float(current.rmax_pos.get()) + rmax: float(current.rmax_pos.get()), + tau: current.tau }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); last_log_ms = now; @@ -421,7 +423,7 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) } // setup filters to be suitable for time constant and gyro filter - rpid.filt_T_hz().set(10.0/(0.5f * 2 * M_PI)); + rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI)); rpid.filt_E_hz().set(0); rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5); @@ -484,6 +486,9 @@ void AP_AutoTune::save_int16_if_changed(AP_Int16 &v, int16_t old_value) void AP_AutoTune::save_gains(void) { const auto &v = last_save; + if (pidP != NULL) { + pidP->set_and_save_ifchanged(1.0f/current.tau); + } save_int16_if_changed(current.rmax_pos, v.rmax_pos); save_int16_if_changed(current.rmax_neg, v.rmax_neg); save_float_if_changed(rpid.ff(), v.FF); @@ -540,14 +545,27 @@ void AP_AutoTune::update_rmax(void) uint8_t level = constrain_int32(aparm.autotune_level, 0, ARRAY_SIZE(tuning_table)); int16_t target_rmax; + float target_tau; if (level == 0) { // this level means to keep current values of RMAX and TCONST target_rmax = constrain_float(current.rmax_pos, 75, 720); + target_tau = constrain_float(current.tau, 0.1, 2); } else { target_rmax = tuning_table[level-1].rmax; + target_tau = tuning_table[level-1].tau; + if (type == AUTOTUNE_PITCH) { + // 50% longer time constant on pitch + target_tau *= 1.5; + } } + if (level > 0 && is_positive(current.FF)) { + const float invtau = ((1.0f / target_tau) + (current.I / current.FF)); + if (is_positive(invtau)) { + target_tau = MAX(target_tau,1.0f / invtau); + } + } if (current.rmax_pos == 0) { // conservative initial value @@ -561,4 +579,9 @@ void AP_AutoTune::update_rmax(void) if (level != 0 || current.rmax_neg.get() == 0) { current.rmax_neg.set(current.rmax_pos.get()); } + + // move tau by max 15% per loop + current.tau = constrain_float(target_tau, + current.tau*0.85, + current.tau*1.15); } diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index aa8ef5c9f4..8453a73bff 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -10,6 +10,7 @@ class AP_AutoTune { public: struct ATGains { + float tau; AP_Int16 rmax_pos; AP_Int16 rmax_neg; float FF, P, I, D, IMAX; @@ -37,11 +38,12 @@ class AP_AutoTune float D; uint8_t action; float rmax; + float tau; }; // constructor - AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms, class AC_PID &rpid); + AP_AutoTune(ATGains &_gains, AP_Float *_pidP, ATType type, const AP_Vehicle::FixedWing &parms, class AC_PID &rpid); // called when autotune mode is entered void start(void); @@ -60,6 +62,7 @@ class AP_AutoTune private: // the current gains ATGains ¤t; + AP_Float *pidP; class AC_PID &rpid; // what type of autotune is this diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index e2a896a012..edc60a195d 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -526,13 +526,19 @@ void AP_PitchController::convert_pid() void AP_PitchController::autotune_start(void) { if (autotune == nullptr) { - autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_PITCH, aparm, rate_pid); + angle_i_backup = angle_pid.kI(); + angle_fltt_backup = angle_pid.filt_T_hz(); + gains.tau = tau(); + autotune = new AP_AutoTune(gains, &kP(), AP_AutoTune::AUTOTUNE_PITCH, aparm, rate_pid); if (autotune == nullptr) { if (!failed_autotune_alloc) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed pitch allocation"); } failed_autotune_alloc = true; } + angle_pid.kI(0); + angle_pid.kD().set_and_save_ifchanged(0); + angle_pid.filt_T_hz(0); } if (autotune != nullptr) { autotune->start(); @@ -546,5 +552,7 @@ void AP_PitchController::autotune_restore(void) { if (autotune != nullptr) { autotune->stop(); + angle_pid.kI(angle_i_backup); + angle_pid.filt_T_hz(angle_fltt_backup); } } diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 111cee0fff..1f4b700500 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -63,6 +63,7 @@ class AP_PitchController AP_Float &kD(void) { return rate_pid.kD(); } AP_Float &kFF(void) { return rate_pid.ff(); } AP_Float &rollFF(void) { return _roll_ff; } + float tau(void) { return 1.0f/kP(); } AP_Float &angle_kP(void) { return angle_pid.kP(); } AP_Float &angle_kI(void) { return angle_pid.kI(); } @@ -84,6 +85,9 @@ class AP_PitchController float angle_err_deg; + float angle_i_backup; + float angle_fltt_backup; + AP_PIDInfo _pid_info; AP_PIDInfo _angle_pid_info; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index fce952c5fa..a039703c26 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -416,13 +416,19 @@ void AP_RollController::convert_pid() void AP_RollController::autotune_start(void) { if (autotune == nullptr) { - autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_ROLL, aparm, rate_pid); + angle_i_backup = angle_pid.kI(); + angle_fltt_backup = angle_pid.filt_T_hz(); + gains.tau = tau(); + autotune = new AP_AutoTune(gains, &kP(), AP_AutoTune::AUTOTUNE_ROLL, aparm, rate_pid); if (autotune == nullptr) { if (!failed_autotune_alloc) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed roll allocation"); } failed_autotune_alloc = true; } + angle_pid.kI(0); + angle_pid.kD().set_and_save_ifchanged(0); + angle_pid.filt_T_hz(0); } if (autotune != nullptr) { autotune->start(); @@ -436,5 +442,7 @@ void AP_RollController::autotune_restore(void) { if (autotune != nullptr) { autotune->stop(); + angle_pid.kI(angle_i_backup); + angle_pid.filt_T_hz(angle_fltt_backup); } } diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index dc20f1bcab..fbd777e569 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -61,6 +61,7 @@ class AP_RollController void kFF(float v) {rate_pid.ff().set(v); } AP_Float &kP(void) { return rate_pid.kP(); } + float tau(void) { return 1.0f/kP(); } AP_Float &kI(void) { return rate_pid.kI(); } AP_Float &kD(void) { return rate_pid.kD(); } AP_Float &kFF(void) { return rate_pid.ff(); } @@ -82,6 +83,9 @@ class AP_RollController AC_PID angle_pid{ROLL_ANGLE_PID_P_DEFAULT, ROLL_ANGLE_PID_I_DEFAULT, ROLL_ANGLE_PID_D_DEFAULT, 0, ROLL_ANGLE_PID_IMAX_DEFAULT, ROLL_ANGLE_PID_TARGET_FILTER_DEFAULT, 0, ROLL_ANGLE_PID_D_FILTER_DEFAULT, 0.02, ROLL_ANGLE_PID_SMAX_DEFAULT, 1}; float angle_err_deg; + float angle_i_backup; + float angle_fltt_backup; + AP_PIDInfo _pid_info; AP_PIDInfo _angle_pid_info; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index ef81e2d3c5..96bd541761 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -333,7 +333,7 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) { // fake up an angular error based on a notional time constant of 0.5s - const float angle_err_deg = desired_rate * 0.5f; + const float angle_err_deg = desired_rate * gains.tau; // let autotune have a go at the values autotune->update(pinfo, scaler, angle_err_deg); } @@ -361,9 +361,11 @@ void AP_YawController::reset_rate_PID() void AP_YawController::autotune_start(void) { if (autotune == nullptr && rate_control_enabled()) { + // the autotuner needs a time constant. Use an assumed tconst of 0.5 + gains.tau = 0.5; gains.rmax_pos.set(90); - autotune = new AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_YAW, aparm, rate_pid); + autotune = new AP_AutoTune(gains, NULL, AP_AutoTune::AUTOTUNE_YAW, aparm, rate_pid); if (autotune == nullptr) { if (!failed_autotune_alloc) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed yaw allocation"); diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index ca0e0eeae3..3bf8a9a8a3 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -24,7 +24,7 @@ #include #ifndef HAL_ADSB_ENABLED -#define HAL_ADSB_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 +#define HAL_ADSB_ENABLED 0 #endif #if HAL_ADSB_ENABLED