aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AutoTune: adjust code to move parameter variables into subclasses

Browse Source
This commit is contained in:
Bill Geyer 2021-12-26 23:53:50 -05:00 committed by Bill Geyer
parent c817e92ada
commit 36d627c1b0
6 changed files with 164 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -49,26 +49,8 @@
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_ANGLE_MAX_RLLPIT 30.0f // maximum allowable angle during testing
// ifdef is not working. Modified multi values to reflect heli requirements
#if APM_BUILD_TYPE(APM_BUILD_Heli)
// heli defines
#define AUTOTUNE_RP_ACCEL_MIN 20000.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_Y_ACCEL_MIN 10000.0f // Minimum acceleration for Yaw
#define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
#else
// Frame specific defaults
#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw
#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
#endif // HELI_BUILD
// second table of user settable parameters for quadplanes, this
// allows us to go beyond the 64 parameter limit
@ -526,71 +508,8 @@ void AC_AutoTune::control_attitude()
step_scaler = 1.0f;
// move to the next tuning type
switch (tune_type) {
case RD_UP:
break;
case RD_DOWN:
switch (axis) {
case ROLL:
tune_roll_rd = MAX(min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
tune_roll_rp = MAX(get_rp_min(), tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break;
case PITCH:
tune_pitch_rd = MAX(min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
tune_pitch_rp = MAX(get_rp_min(), tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break;
case YAW:
tune_yaw_rLPF = MAX(get_yaw_rate_filt_min(), tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
tune_yaw_rp = MAX(get_rp_min(), tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
}
break;
case RP_UP:
switch (axis) {
case ROLL:
tune_roll_rp = MAX(get_rp_min(), tune_roll_rp * AUTOTUNE_RP_BACKOFF);
break;
case PITCH:
tune_pitch_rp = MAX(get_rp_min(), tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break;
case YAW:
tune_yaw_rp = MAX(get_rp_min(), tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break;
}
break;
case SP_DOWN:
break;
case SP_UP:
switch (axis) {
case ROLL:
tune_roll_sp = MAX(get_sp_min(), tune_roll_sp * AUTOTUNE_SP_BACKOFF);
// trad heli uses original parameter value rather than max demostrated through test
if (set_accel_to_max_test_value()) {
tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
}
break;
case PITCH:
tune_pitch_sp = MAX(get_sp_min(), tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
// trad heli uses original parameter value rather than max demostrated through test
if (set_accel_to_max_test_value()) {
tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
}
break;
case YAW:
tune_yaw_sp = MAX(get_sp_min(), tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
// trad heli uses original parameter value rather than max demostrated through test
if (set_accel_to_max_test_value()) {
tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
}
break;
}
break;
case RFF_UP:
case MAX_GAINS:
case TUNE_COMPLETE:
break;
}
// set gains for post tune before moving to the next tuning type
set_gains_post_tune(axis);
// increment the tune type to the next one in tune sequence
next_tune_type(tune_type, false);
@ -683,8 +602,6 @@ void AC_AutoTune::backup_gains_and_initialise()
step_scaler = 1.0f;
desired_yaw_cd = ahrs_view->yaw_sensor;
aggressiveness = constrain_float(aggressiveness, 0.05f, 0.2f);
}
/*
@ -739,17 +656,17 @@ void AC_AutoTune::update_gcs(uint8_t message_id) const
// axis helper functions
bool AC_AutoTune::roll_enabled() const
{
return axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL;
return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_ROLL;
}
bool AC_AutoTune::pitch_enabled() const
{
return axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH;
return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_PITCH;
}
bool AC_AutoTune::yaw_enabled() const
{
return axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_YAW;
}
/*

23
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -142,20 +142,8 @@ protected:
// update gains for the angle p down tune type
virtual void updating_angle_p_down_all(AxisType test_axis)=0;
// returns true if rate P gain of zero is acceptable for this vehicle
virtual bool allow_zero_rate_p() = 0;
// returns true if max tested accel is used for parameter
virtual bool set_accel_to_max_test_value() = 0;
// get minimum rate P (for any axis)
virtual float get_rp_min() const = 0;
// get minimum angle P (for any axis)
virtual float get_sp_min() const = 0;
// get minimum rate Yaw filter value
virtual float get_yaw_rate_filt_min() const = 0;
// set gains post tune for the tune type
virtual void set_gains_post_tune(AxisType test_axis)=0;
// reverse direction for twitch test
virtual bool twitch_reverse_direction() = 0;
@ -219,11 +207,8 @@ protected:
// Sets customizable tune sequence for the vehicle
virtual void set_tune_sequence() = 0;
// parameters
AP_Int8 axis_bitmask; // axes to be tuned
AP_Float aggressiveness; // aircraft response aggressiveness to be tuned
AP_Float min_d; // minimum rate d gain allowed during tuning
AP_Float vel_hold_gain; // gain for velocity hold
// get_axis_bitmask accessor
virtual uint8_t get_axis_bitmask() const = 0;
// copies of object pointers to make code a bit clearer
AC_AttitudeControl *attitude_control;

82
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -40,6 +40,14 @@
#define AUTOTUNE_RFF_MIN 0.025f // maximum Stab P value
#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in
#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_RP_ACCEL_MIN 20000.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_Y_ACCEL_MIN 10000.0f // Minimum acceleration for Yaw
#define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
#define AUTOTUNE_SEQ_BITMASK_VFF 1
#define AUTOTUNE_SEQ_BITMASK_RATE_D 2
#define AUTOTUNE_SEQ_BITMASK_ANGLE_P 4
@ -1424,6 +1432,62 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
}
}
// set gains post tune for the tune type
void AC_AutoTune_Heli::set_gains_post_tune(AxisType test_axis)
{
switch (tune_type) {
case RD_UP:
break;
case RD_DOWN:
switch (test_axis) {
case ROLL:
tune_roll_rd = MAX(0.0f, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break;
case PITCH:
tune_pitch_rd = MAX(0.0f, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break;
case YAW:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
}
break;
case RP_UP:
switch (test_axis) {
case ROLL:
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF);
break;
case PITCH:
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break;
case YAW:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break;
}
break;
case SP_DOWN:
break;
case SP_UP:
switch (test_axis) {
case ROLL:
tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF);
break;
case PITCH:
tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
break;
case YAW:
tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
break;
}
break;
case RFF_UP:
case MAX_GAINS:
case TUNE_COMPLETE:
break;
}
}
// updating_rate_ff_up - adjust FF to ensure the target is reached
// FF is adjusted until rate requested is acheived
void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command)
@ -2045,24 +2109,6 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float pha
phase);
}
// get minimum rate P (for any axis)
float AC_AutoTune_Heli::get_rp_min() const
{
return AUTOTUNE_RP_MIN;
}
// get minimum angle P (for any axis)
float AC_AutoTune_Heli::get_sp_min() const
{
return AUTOTUNE_SP_MIN;
}
// get minimum rate Yaw filter value
float AC_AutoTune_Heli::get_yaw_rate_filt_min() const
{
return AUTOTUNE_RLPF_MIN;
}
// reset the test vaariables for each vehicle
void AC_AutoTune_Heli::reset_vehicle_test_variables()
{

37
libraries/AC_AutoTune/AC_AutoTune_Heli.h
View File

@ -83,14 +83,8 @@ protected:
// update gains for the max gain tune type
void updating_max_gains_all(AxisType test_axis) override;
// get minimum rate P (for any axis)
float get_rp_min() const override;
// get minimum angle P (for any axis)
float get_sp_min() const override;
// get minimum rate Yaw filter value
float get_yaw_rate_filt_min() const override;
// set gains post tune for the tune type
void set_gains_post_tune(AxisType test_axis) override;
// reverse direction for twitch test
bool twitch_reverse_direction() override { return positive_direction; }
@ -107,29 +101,14 @@ protected:
void Log_AutoTuneSweep() override;
void Log_Write_AutoTuneSweep(float freq, float gain, float phase);
// returns true if rate P gain of zero is acceptable for this vehicle
bool allow_zero_rate_p() override { return true; }
// returns true if max tested accel is used for parameter
bool set_accel_to_max_test_value() override { return false; }
// send intermittant updates to user on status of tune
void do_gcs_announcements() override;
// set the tuning test sequence
void set_tune_sequence() override;
// tuning sequence bitmask
AP_Int8 seq_bitmask;
// minimum sweep frequency
AP_Float min_sweep_freq;
// maximum sweep frequency
AP_Float max_sweep_freq;
// maximum response gain
AP_Float max_resp_gain;
// get_axis_bitmask accessor
uint8_t get_axis_bitmask() const override { return axis_bitmask; }
private:
// max_gain_data type stores information from the max gain test
@ -281,6 +260,14 @@ private:
};
sweep_data sweep;
// parameters
AP_Int8 axis_bitmask; // axes to be tuned
AP_Int8 seq_bitmask; // tuning sequence bitmask
AP_Float min_sweep_freq; // minimum sweep frequency
AP_Float max_sweep_freq; // maximum sweep frequency
AP_Float max_resp_gain; // maximum response gain
AP_Float vel_hold_gain; // gain for velocity hold
// freqresp object for the rate frequency response tests
AC_AutoTune_FreqResp freqresp_rate;
// freqresp object for the angle frequency response tests

88
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -38,6 +38,14 @@
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw
#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw
#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
#include "AC_AutoTune_Multi.h"
const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = {
@ -153,6 +161,8 @@ void AC_AutoTune_Multi::backup_gains_and_initialise()
{
AC_AutoTune::backup_gains_and_initialise();
aggressiveness = constrain_float(aggressiveness, 0.05f, 0.2f);
orig_bf_feedforward = attitude_control->get_bf_feedforward();
// backup original pids and initialise tuned pid values
@ -663,6 +673,66 @@ void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis)
}
}
// set gains post tune for the tune type
void AC_AutoTune_Multi::set_gains_post_tune(AxisType test_axis)
{
switch (tune_type) {
case RD_UP:
break;
case RD_DOWN:
switch (test_axis) {
case ROLL:
tune_roll_rd = MAX(min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break;
case PITCH:
tune_pitch_rd = MAX(min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break;
case YAW:
tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
}
break;
case RP_UP:
switch (test_axis) {
case ROLL:
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF);
break;
case PITCH:
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break;
case YAW:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break;
}
break;
case SP_DOWN:
break;
case SP_UP:
switch (test_axis) {
case ROLL:
tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF);
tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
break;
case PITCH:
tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
break;
case YAW:
tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
break;
}
break;
case RFF_UP:
case MAX_GAINS:
case TUNE_COMPLETE:
break;
}
}
// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back
// optimize D term while keeping the maximum just below the target by adjusting P
void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
@ -1106,21 +1176,3 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
break;
}
}
// get minimum rate P (for any axis)
float AC_AutoTune_Multi::get_rp_min() const
{
return AUTOTUNE_RP_MIN;
}
// get minimum angle P (for any axis)
float AC_AutoTune_Multi::get_sp_min() const
{
return AUTOTUNE_SP_MIN;
}
// get minimum rate Yaw filter value
float AC_AutoTune_Multi::get_yaw_rate_filt_min() const
{
return AUTOTUNE_RLPF_MIN;
}

23
libraries/AC_AutoTune/AC_AutoTune_Multi.h
View File

@ -81,20 +81,8 @@ protected:
// update gains for the max gain tune type
void updating_max_gains_all(AxisType test_axis) override {};
// returns true if rate P gain of zero is acceptable for this vehicle
bool allow_zero_rate_p() override { return false; }
// returns true if max tested accel is used for parameter
bool set_accel_to_max_test_value() override { return true; }
// get minimum rate P (for any axis)
float get_rp_min() const override;
// get minimum angle P (for any axis)
float get_sp_min() const override;
// get minimum yaw rate filter value
float get_yaw_rate_filt_min() const override;
// set gains post tune for the tune type
void set_gains_post_tune(AxisType test_axis) override;
// reverse direction for twitch test
bool twitch_reverse_direction() override { return !positive_direction; }
@ -114,6 +102,9 @@ protected:
tune_seq[5] = TUNE_COMPLETE;
}
// get_axis_bitmask accessor
uint8_t get_axis_bitmask() const override { return axis_bitmask; }
private:
// twitch test functions for multicopter
void twitch_test_init();
@ -146,4 +137,8 @@ private:
// P is increased until we achieve our target within a reasonable time
void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max);
// parameters
AP_Int8 axis_bitmask; // axes to be tuned
AP_Float aggressiveness; // aircraft response aggressiveness to be tuned
AP_Float min_d; // minimum rate d gain allowed during tuning
};