Skip to content

Commit

Permalink
improve autotune
Browse files Browse the repository at this point in the history
  • Loading branch information
@shellixyz
shellixyz committed Dec 12, 2023
1 parent b16ddba commit a61bfd1
Show file tree
Hide file tree
Showing 11 changed files with 65 additions and 14 deletions.
2 changes: 1 addition & 1 deletion ArduPlane/Log.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 0 additions & 1 deletion ArduPlane/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,6 @@ class ModeAuto : public Mode
void _exit() override;
};


class ModeAutoTune : public Mode
{
public:
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/quadplane.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
31 changes: 27 additions & 4 deletions libraries/APM_Control/AP_AutoTune.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand All @@ -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);
}
5 changes: 4 additions & 1 deletion libraries/APM_Control/AP_AutoTune.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -60,6 +62,7 @@ class AP_AutoTune
private:
// the current gains
ATGains &current;
AP_Float *pidP;
class AC_PID &rpid;

// what type of autotune is this
Expand Down
10 changes: 9 additions & 1 deletion libraries/APM_Control/AP_PitchController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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);
}
}
4 changes: 4 additions & 0 deletions libraries/APM_Control/AP_PitchController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(); }
Expand All @@ -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;

Expand Down
10 changes: 9 additions & 1 deletion libraries/APM_Control/AP_RollController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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);
}
}
4 changes: 4 additions & 0 deletions libraries/APM_Control/AP_RollController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(); }
Expand All @@ -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;

Expand Down
6 changes: 4 additions & 2 deletions libraries/APM_Control/AP_YawController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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");
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ADSB/AP_ADSB.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#include <AP_HAL/AP_HAL_Boards.h>

#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
Expand Down

0 comments on commit a61bfd1

Please sign in to comment.