diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 4c776c08aa..d5d93460c5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -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; } /* diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 910dcc48a8..aa877d9e0f 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -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; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 14398604d4..8704be372a 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -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() { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 004e03f39d..02fdfd27b2 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -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 diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 9354dc3416..3c27209813 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -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; -} diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 7a24362ac0..30c3f5ef14 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -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 };