Skip to content

Commit

Permalink
Copter: Autotune: Virtual methods to separate Heli and Multi
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Feb 29, 2024
1 parent 14bef1a commit 8c8a22b
Show file tree
Hide file tree
Showing 6 changed files with 131 additions and 19 deletions.
13 changes: 5 additions & 8 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the aircraft to be level before starting next test
#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_ANGLE_MAX_RLLPIT_SCALE 2.0 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max

AC_AutoTune::AC_AutoTune()
{
Expand Down Expand Up @@ -357,19 +356,18 @@ void AC_AutoTune::control_attitude()
// Initialize test-specific variables
switch (axis) {
case ROLL:
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE;
angle_finish = target_angle_max_rp_cd();
start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f;
start_angle = ahrs_view->roll_sensor;
break;
case PITCH:
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE;
angle_finish = target_angle_max_rp_cd();
start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f;
start_angle = ahrs_view->pitch_sensor;
break;
case YAW:
case YAW_D:
// Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE;
angle_finish = target_angle_max_y_cd();
start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f;
start_angle = ahrs_view->yaw_sensor;
break;
Expand All @@ -389,16 +387,15 @@ void AC_AutoTune::control_attitude()
test_run(axis, direction_sign);

// Check for failure causing reverse response
if (lean_angle <= -attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE) {
if (lean_angle <= -angle_lim_neg_rpy_cd()) {
step = WAITING_FOR_LEVEL;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
level_start_time_ms = now;
}

// protect from roll over
const float resultant_angle_cd = 100 * degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch()));
if (resultant_angle_cd > attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE) {
if (attitude_control->lean_angle_deg() * 100 > angle_lim_max_rp_cd()) {
step = WAITING_FOR_LEVEL;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
Expand Down
25 changes: 19 additions & 6 deletions libraries/AC_AutoTune/AC_AutoTune.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,6 @@

#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000

#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE 1.0 / 4.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE 1.0 / 2.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE 1.0 / 8.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_YAW_SCALE 2.0 / 3.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step

class AC_AutoTune
{
public:
Expand Down Expand Up @@ -282,7 +277,7 @@ class AC_AutoTune
float rate_max; // maximum rate variable - parent and multi
float test_accel_max; // maximum acceleration variable
float step_scaler; // scaler to reduce maximum target step - parent and multi
float abort_angle; // Angle that test is aborted- parent and multi
float angle_finish; // Angle that test is aborted- parent and multi
float desired_yaw_cd; // yaw heading during tune - parent and Tradheli

LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
Expand Down Expand Up @@ -314,6 +309,24 @@ class AC_AutoTune
// return true if we have a good position estimate
virtual bool position_ok();

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_max_rp_cd() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_max_y_cd() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_min_rp_cd() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float target_angle_min_y_cd() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float angle_lim_max_rp_cd() const = 0;

// methods subclasses must implement to specify max/min test angles:
virtual float angle_lim_neg_rpy_cd() const = 0;

// initialise position controller
bool init_position_controller();

Expand Down
38 changes: 38 additions & 0 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,14 @@
#define AUTOTUNE_SEQ_BITMASK_MAX_GAIN 8
#define AUTOTUNE_SEQ_BITMASK_TUNE_CHECK 16

// angle limits preserved from previous behaviour as Multi changed:
#define AUTOTUNE_ANGLE_TARGET_MAX_RP_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MIN_RP_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MAX_Y_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_TARGET_MIN_Y_CD 500 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_MAX_RP_CD 3000 // maximum allowable angle in degrees during testing
#define AUTOTUNE_ANGLE_NEG_RPY_CD 1000 // maximum allowable angle in degrees during testing

const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = {

// @Param: AXES
Expand Down Expand Up @@ -1724,6 +1732,36 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase

}

float AC_AutoTune_Heli::target_angle_max_rp_cd() const
{
return AUTOTUNE_ANGLE_TARGET_MAX_RP_CD;
}

float AC_AutoTune_Heli::target_angle_max_y_cd() const
{
return AUTOTUNE_ANGLE_TARGET_MAX_Y_CD;
}

float AC_AutoTune_Heli::target_angle_min_rp_cd() const
{
return AUTOTUNE_ANGLE_TARGET_MIN_RP_CD;
}

float AC_AutoTune_Heli::target_angle_min_y_cd() const
{
return AUTOTUNE_ANGLE_TARGET_MIN_Y_CD;
}

float AC_AutoTune_Heli::angle_lim_max_rp_cd() const
{
return AUTOTUNE_ANGLE_MAX_RP_CD;
}

float AC_AutoTune_Heli::angle_lim_neg_rpy_cd() const
{
return AUTOTUNE_ANGLE_NEG_RPY_CD;
}

#if HAL_LOGGING_ENABLED
// log autotune summary data
void AC_AutoTune_Heli::Log_AutoTune()
Expand Down
12 changes: 12 additions & 0 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,18 @@ class AC_AutoTune_Heli : public AC_AutoTune
DRB = 2,
};

float target_angle_max_rp_cd() const override;

float target_angle_max_y_cd() const override;

float target_angle_min_rp_cd() const override;

float target_angle_min_y_cd() const override;

float angle_lim_max_rp_cd() const override;

float angle_lim_neg_rpy_cd() const override;

// Feedforward test used to determine Rate FF gain
void rate_ff_test_init();
void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign);
Expand Down
50 changes: 45 additions & 5 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,14 @@
#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step

#define AUTOTUNE_TARGET_ANGLE_MAX_RP_SCALE 1.0 / 2.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_MAX_Y_SCALE 1.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_MIN_RP_SCALE 1.0 / 3.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_MIN_Y_SCALE 1.0 / 6.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_ANGLE_ABORT_RP_SCALE 2.5 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max
#define AUTOTUNE_ANGLE_MAX_Y_SCALE 1.0 // maximum allowable angle during testing, as a fraction of angle_max
#define AUTOTUNE_ANGLE_NEG_RP_SCALE 1.0 / 5.0 // maximum allowable angle during testing, as a fraction of angle_max

// second table of user settable parameters for quadplanes, this
// allows us to go beyond the 64 parameter limit
const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = {
Expand Down Expand Up @@ -1068,6 +1076,38 @@ void AC_AutoTune_Multi::Log_AutoTuneDetails()
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
}

float AC_AutoTune_Multi::target_angle_max_rp_cd() const
{
return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MAX_RP_SCALE;
}

float AC_AutoTune_Multi::target_angle_max_y_cd() const
{
// Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MAX_Y_SCALE;
}

float AC_AutoTune_Multi::target_angle_min_rp_cd() const
{
return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MIN_RP_SCALE;
}

float AC_AutoTune_Multi::target_angle_min_y_cd() const
{
// Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MIN_Y_SCALE;
}

float AC_AutoTune_Multi::angle_lim_max_rp_cd() const
{
return attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_ABORT_RP_SCALE;
}

float AC_AutoTune_Multi::angle_lim_neg_rpy_cd() const
{
return attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_NEG_RP_SCALE;
}

// @LoggerMessage: ATUN
// @Description: Copter/QuadPlane AutoTune
// @Vehicles: Copter, Plane
Expand Down Expand Up @@ -1130,22 +1170,22 @@ void AC_AutoTune_Multi::twitch_test_init()
case ROLL: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, target_angle_min_rp_cd(), target_angle_max_rp_cd());
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f);
break;
}
case PITCH: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, target_angle_min_rp_cd(), target_angle_max_rp_cd());
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break;
}
case YAW:
case YAW_D: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, target_angle_min_y_cd(), target_angle_max_y_cd());
if (axis == YAW_D) {
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz()*2.0f);
} else {
Expand Down Expand Up @@ -1251,12 +1291,12 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
case RD_DOWN:
twitching_test_rate(lean_angle, rotation_rate, target_rate, test_rate_min, test_rate_max, test_angle_min);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
twitching_abort_rate(lean_angle, rotation_rate, abort_angle, test_rate_min, test_angle_min);
twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min);
break;
case RP_UP:
twitching_test_rate(lean_angle, rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max, test_angle_min);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
twitching_abort_rate(lean_angle, rotation_rate, abort_angle, test_rate_min, test_angle_min);
twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min);
break;
case SP_DOWN:
case SP_UP:
Expand Down
12 changes: 12 additions & 0 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,18 @@ class AC_AutoTune_Multi : public AC_AutoTune
// reset the update gain variables for multi
void reset_update_gain_variables() override {};

float target_angle_max_rp_cd() const override;

float target_angle_max_y_cd() const override;

float target_angle_min_rp_cd() const override;

float target_angle_min_y_cd() const override;

float angle_lim_max_rp_cd() const override;

float angle_lim_neg_rpy_cd() const override;

void test_init() override;
void test_run(AxisType test_axis, const float dir_sign) override;

Expand Down

0 comments on commit 8c8a22b

Please sign in to comment.