From 685bf80357e647f2188a5498d41a596cf97cd596 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 27 Feb 2024 15:09:52 +1100 Subject: [PATCH] AC_AutoTune: allow heli to have different angle limits than multi --- libraries/AC_AutoTune/AC_AutoTune.cpp | 2 +- libraries/AC_AutoTune/AC_AutoTune.h | 3 +++ libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 10 ++++++++++ libraries/AC_AutoTune/AC_AutoTune_Heli.h | 2 ++ libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 5 +++++ libraries/AC_AutoTune/AC_AutoTune_Multi.h | 2 ++ 6 files changed, 23 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 8f25601c302f5..e83af2b5a1c48 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -389,7 +389,7 @@ 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 <= -target_min_angle_rllpit_cd()) { step = WAITING_FOR_LEVEL; positive_direction = twitch_reverse_direction(); step_start_time_ms = now; diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index e2f15dab7a3aa..67b73a7c097a1 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -314,6 +314,9 @@ 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_min_angle_rllpit_cd() const = 0; + // initialise position controller bool init_position_controller(); diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 0f972a4893807..286bbb1303e52 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -60,6 +60,11 @@ #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_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step + const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = { // @Param: AXES @@ -1724,6 +1729,11 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase } +float AC_AutoTune_Heli::target_min_angle_rllpit_cd() const +{ + return AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD; +} + #if HAL_LOGGING_ENABLED // log autotune summary data void AC_AutoTune_Heli::Log_AutoTune() diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 4890a0da8e2ce..27ade89ca6b58 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -156,6 +156,8 @@ class AC_AutoTune_Heli : public AC_AutoTune DRB = 2, }; + float target_min_angle_rllpit_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); diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index cda621c3e0e8a..4c5f95a05e44f 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1068,6 +1068,11 @@ void AC_AutoTune_Multi::Log_AutoTuneDetails() Log_Write_AutoTuneDetails(lean_angle, rotation_rate); } +float AC_AutoTune_Multi::target_min_angle_rllpit_cd() const +{ + return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE; +} + // @LoggerMessage: ATUN // @Description: Copter/QuadPlane AutoTune // @Vehicles: Copter, Plane diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 7d0922c6bbf1a..5385ff4dbec84 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -63,6 +63,8 @@ class AC_AutoTune_Multi : public AC_AutoTune // reset the update gain variables for multi void reset_update_gain_variables() override {}; + float target_min_angle_rllpit_cd() const override; + void test_init() override; void test_run(AxisType test_axis, const float dir_sign) override;