diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index aa6530fc5b..b283d47329 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -147,6 +147,19 @@ void AP_AutoTune::stop(void) } } +const char *AP_AutoTune::axis_string(void) const +{ + switch (type) { + case AUTOTUNE_ROLL: + return "Roll"; + case AUTOTUNE_PITCH: + return "Pitch"; + case AUTOTUNE_YAW: + return "Yaw"; + } + return ""; +} + /* one update cycle of the autotuner */ @@ -189,13 +202,21 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e max_SRate_P = MAX(max_SRate_P, slew_limiter_P.get_slew_rate()); max_SRate_D = MAX(max_SRate_D, slew_limiter_D.get_slew_rate()); - float att_limit_deg; - if (type == AUTOTUNE_ROLL) { + float att_limit_deg = 0; + switch (type) { + case AUTOTUNE_ROLL: att_limit_deg = aparm.roll_limit_cd * 0.01; - } else { + break; + case AUTOTUNE_PITCH: att_limit_deg = MIN(abs(aparm.pitch_limit_max_cd),abs(aparm.pitch_limit_min_cd))*0.01; + break; + case AUTOTUNE_YAW: + // arbitrary value for yaw angle + att_limit_deg = 20; + break; } + // thresholds for when we consider an event to start and end const float rate_threshold1 = 0.4 * MIN(att_limit_deg / current.tau.get(), current.rmax_pos); const float rate_threshold2 = 0.25 * rate_threshold1; @@ -337,7 +358,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e D_limit = D; D_set_ms = now; action = Action::LOWER_D; - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", axis_string(), D_limit); } } else if (min_Dmod < 1.0) { // oscillation, with D_limit set @@ -349,7 +370,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e D_limit = D; D_set_ms = now; action = Action::LOWER_D; - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", axis_string(), D_limit); done_count = 0; } else if (now - P_set_ms > 2500) { if (is_positive(P_limit)) { @@ -365,7 +386,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e P_set_ms = now; action = Action::LOWER_P; done_count = 0; - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", P_limit); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", axis_string(), P_limit); } } } else if (ff_count < 4) { @@ -384,7 +405,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e // have done 3 cycles without reducing P if (done_count < 3) { if (++done_count == 3) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", type==AUTOTUNE_ROLL?"Roll":"Pitch"); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", axis_string()); save_gains(); } } diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 56859809d0..3030fed6a9 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -19,7 +19,8 @@ public: enum ATType { AUTOTUNE_ROLL = 0, - AUTOTUNE_PITCH = 1 + AUTOTUNE_PITCH = 1, + AUTOTUNE_YAW = 2, }; struct PACKED log_ATRP { @@ -105,6 +106,7 @@ private: void save_float_if_changed(AP_Float &v, float value); void save_int16_if_changed(AP_Int16 &v, int16_t value); void state_change(ATState newstate); + const char *axis_string(void) const; // get gains with PID components ATGains get_gains(void); diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index b505214b25..7e833edc26 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -148,6 +148,16 @@ const AP_Param::GroupInfo AP_YawController::var_info[] = { AP_GROUPEND }; +AP_YawController::AP_YawController(const AP_Vehicle::FixedWing &parms) + : aparm(parms) +{ + AP_Param::setup_object_defaults(this, var_info); + _pid_info.target = 0; + _pid_info.FF = 0; + _pid_info.P = 0; + rate_pid.set_slew_limit_scale(45); +} + int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) { uint32_t tnow = AP_HAL::millis(); @@ -315,12 +325,12 @@ float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disa // remember the last output to trigger the I limit _last_out = out; -#if 0 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 * gains.tau; // let autotune have a go at the values autotune->update(pinfo, scaler, angle_err_deg); } -#endif // output is scaled to notional centidegrees of deflection return constrain_float(out * 100, -4500, 4500); @@ -330,3 +340,36 @@ void AP_YawController::reset_I() { _integrator = 0; } + +/* + start an autotune + */ +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); + if (autotune == nullptr) { + if (!failed_autotune_alloc) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed yaw allocation"); + } + failed_autotune_alloc = true; + } + } + if (autotune != nullptr) { + autotune->start(); + } +} + +/* + restore autotune gains + */ +void AP_YawController::autotune_restore(void) +{ + if (autotune != nullptr) { + autotune->stop(); + } +} diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 3ca72f750b..4ff955f846 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -4,18 +4,12 @@ #include #include #include +#include "AP_AutoTune.h" class AP_YawController { public: - AP_YawController(const AP_Vehicle::FixedWing &parms) - : aparm(parms) - { - AP_Param::setup_object_defaults(this, var_info); - _pid_info.target = 0; - _pid_info.FF = 0; - _pid_info.P = 0; - } + AP_YawController(const AP_Vehicle::FixedWing &parms); /* Do not allow copies */ AP_YawController(const AP_YawController &other) = delete; @@ -47,6 +41,10 @@ public: return _pid_info; } + // start/stop auto tuner + void autotune_start(void); + void autotune_restore(void); + static const struct AP_Param::GroupInfo var_info[]; private: @@ -67,5 +65,9 @@ private: float _integrator; + AP_AutoTune::ATGains gains; + AP_AutoTune *autotune; + bool failed_autotune_alloc; + AP_Logger::PID_Info _pid_info; };