From bf15c064e64e688be79ea3f8352e0af5212e273b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 8 Nov 2026 08:54:59 +1030 Subject: [PATCH 1/2] AC_AutoTune: use radians instead of centidegrees --- libraries/AC_AutoTune/AC_AutoTune.cpp | 42 ++-- libraries/AC_AutoTune/AC_AutoTune.h | 22 +- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 66 +++--- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 12 +- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 242 ++++++++++---------- libraries/AC_AutoTune/AC_AutoTune_Multi.h | 36 +-- 6 files changed, 214 insertions(+), 206 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 999cd90175be..fa3136795faf 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -12,13 +12,13 @@ #define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) - # define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level (Plane uses more relaxed 5deg) - # define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec) + # define AUTOTUNE_LEVEL_ANGLE_RAD radians(5.0) // angle which qualifies as level (Plane uses more relaxed 5deg) + # define AUTOTUNE_LEVEL_RATE_RP_RADS radians(10.0) // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec) #else - # define AUTOTUNE_LEVEL_ANGLE_CD 250 // angle which qualifies as level - # define AUTOTUNE_LEVEL_RATE_RP_CD 500 // rate which qualifies as level for roll and pitch + # define AUTOTUNE_LEVEL_ANGLE_RAD radians(2.5) // angle which qualifies as level + # define AUTOTUNE_LEVEL_RATE_RP_RADS radians(5.0) // rate which qualifies as level for roll and pitch #endif -#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw +#define AUTOTUNE_LEVEL_RATE_Y_RADS radians(7.5) // rate which qualifies as level for yaw #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 @@ -342,22 +342,22 @@ bool AC_AutoTune::currently_level() // relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS const float threshold_mul = constrain_float((float)(now_ms - level_start_time_ms) / (float)AUTOTUNE_LEVEL_TIMEOUT_MS, 0.0, 2.0); - if (fabsf(ahrs_view->get_roll_rad() - desired_roll_rad) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) { + if (fabsf(ahrs_view->get_roll_rad() - desired_roll_rad) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_RAD) { return false; } - if (fabsf(ahrs_view->get_pitch_rad() - desired_pitch_rad) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) { + if (fabsf(ahrs_view->get_pitch_rad() - desired_pitch_rad) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_RAD) { return false; } - if (fabsf(wrap_PI(ahrs_view->get_yaw_rad() - desired_yaw_rad)) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) { + if (fabsf(wrap_PI(ahrs_view->get_yaw_rad() - desired_yaw_rad)) > threshold_mul * AUTOTUNE_LEVEL_ANGLE_RAD) { return false; } - if (fabsf(ahrs_view->get_gyro().x) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_RP_CD)) { + if (fabsf(ahrs_view->get_gyro().x) > threshold_mul * AUTOTUNE_LEVEL_RATE_RP_RADS) { return false; } - if (fabsf(ahrs_view->get_gyro().y) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_RP_CD)) { + if (fabsf(ahrs_view->get_gyro().y) > threshold_mul * AUTOTUNE_LEVEL_RATE_RP_RADS) { return false; } - if (fabsf(ahrs_view->get_gyro().z) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_Y_CD)) { + if (fabsf(ahrs_view->get_gyro().z) > threshold_mul * AUTOTUNE_LEVEL_RATE_Y_RADS) { return false; } return true; @@ -367,8 +367,8 @@ bool AC_AutoTune::currently_level() // Updates attitude controller targets and processes test results to adjust PID gains. void AC_AutoTune::control_attitude() { - rotation_rate = 0.0f; - lean_angle = 0.0f; + rotation_rate_rads = 0.0f; + lean_angle_rad = 0.0f; const float direction_sign = positive_direction ? 1.0f : -1.0f; const uint32_t now_ms = AP_HAL::millis(); @@ -394,17 +394,17 @@ void AC_AutoTune::control_attitude() // Record starting angular position and rate switch (axis) { case AxisType::ROLL: - start_rate = degrees(ahrs_view->get_gyro().x) * 100.0f; - start_angle = ahrs_view->roll_sensor; + start_rate_rads = ahrs_view->get_gyro().x; + start_angle_rad = ahrs_view->get_roll_rad(); break; case AxisType::PITCH: - start_rate = degrees(ahrs_view->get_gyro().y) * 100.0f; - start_angle = ahrs_view->pitch_sensor; + start_rate_rads = ahrs_view->get_gyro().y; + start_angle_rad = ahrs_view->get_pitch_rad(); break; case AxisType::YAW: case AxisType::YAW_D: - start_rate = degrees(ahrs_view->get_gyro().z) * 100.0f; - start_angle = ahrs_view->yaw_sensor; + start_rate_rads = ahrs_view->get_gyro().z; + start_angle_rad = ahrs_view->get_yaw_rad(); break; } @@ -421,8 +421,8 @@ void AC_AutoTune::control_attitude() test_run(axis, direction_sign); // Detect failure due to reverse response or excessive lean angle - if (lean_angle <= -angle_lim_neg_rpy_cd() || - attitude_control->lean_angle_deg() * 100 > angle_lim_max_rp_cd()) { + if (lean_angle_rad <= -angle_lim_neg_rpy_rad() || + attitude_control->lean_angle_rad() > angle_lim_max_rp_rad()) { step = Step::ABORT; } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 9f5b94043b10..da42fa6f26ca 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -276,9 +276,9 @@ class AC_AutoTune uint32_t step_timeout_ms; // time limit of current autotune process uint32_t level_start_time_ms; // start time of waiting for level int8_t success_counter; // counter for tuning gains - float start_angle; // start angle - float start_rate; // start rate - parent and multi - float test_accel_max_cdss; // maximum acceleration variable + float start_angle_rad; // start angle in radians + float start_rate_rads; // start rate in radians/second - parent and multi + float test_accel_max_radss; // maximum acceleration variable in radians/second/second float step_scaler; // scaler to reduce maximum target step - parent and multi LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second @@ -296,8 +296,8 @@ class AC_AutoTune float tune_roll_rff, tune_pitch_rff, tune_yaw_rd, tune_yaw_rff; uint32_t last_announce_ms; - float lean_angle; - float rotation_rate; + float lean_angle_rad; + float rotation_rate_rads; float desired_roll_rad, desired_pitch_rad, desired_yaw_rad; // desired attitude target setpoints and test origins // heli specific variables @@ -309,22 +309,22 @@ class AC_AutoTune virtual bool position_ok(); // methods subclasses must implement to specify max/min test angles: - virtual float target_angle_max_rp_cd() const = 0; + virtual float target_angle_max_rp_rad() const = 0; // methods subclasses must implement to specify max/min test angles: - virtual float target_angle_max_y_cd() const = 0; + virtual float target_angle_max_y_rad() const = 0; // methods subclasses must implement to specify max/min test angles: - virtual float target_angle_min_rp_cd() const = 0; + virtual float target_angle_min_rp_rad() const = 0; // methods subclasses must implement to specify max/min test angles: - virtual float target_angle_min_y_cd() const = 0; + virtual float target_angle_min_y_rad() const = 0; // methods subclasses must implement to specify max/min test angles: - virtual float angle_lim_max_rp_cd() const = 0; + virtual float angle_lim_max_rp_rad() const = 0; // methods subclasses must implement to specify max/min test angles: - virtual float angle_lim_neg_rpy_cd() const = 0; + virtual float angle_lim_neg_rpy_rad() 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 39965b8851f5..2b1ad38fc1af 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -61,12 +61,12 @@ #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 +#define AUTOTUNE_ANGLE_TARGET_MAX_RP_RAD radians(20.0) // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_ANGLE_TARGET_MIN_RP_RAD radians(10.0) // minimum target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_ANGLE_TARGET_MAX_Y_RAD radians(30.0) // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_ANGLE_TARGET_MIN_Y_RAD radians(5.0) // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_ANGLE_MAX_RP_RAD radians(30.0) // maximum allowable angle during testing +#define AUTOTUNE_ANGLE_NEG_RPY_RAD radians(10.0) // maximum allowable angle against the test direction const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = { @@ -174,7 +174,7 @@ void AC_AutoTune_Heli::test_init() } else { start_freq = next_test_freq; stop_freq = start_freq; - test_accel_max_cdss = 0.0f; + test_accel_max_radss = 0.0f; } filter_freq = start_freq; @@ -225,7 +225,7 @@ void AC_AutoTune_Heli::test_init() } else { start_freq = next_test_freq; stop_freq = start_freq; - test_accel_max_cdss = 0.0f; + test_accel_max_radss = 0.0f; } filter_freq = start_freq; attitude_control->bf_feedforward(false); @@ -241,7 +241,7 @@ void AC_AutoTune_Heli::test_init() // initialize start frequency start_freq = min_sweep_freq; stop_freq = max_sweep_freq; - test_accel_max_cdss = 0.0f; + test_accel_max_radss = 0.0f; filter_freq = start_freq; // variables needed to initialize frequency response object and test method @@ -382,7 +382,7 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { } else if (tune_type == TuneType::RATE_FF_UP) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff)); } else if (tune_type == TuneType::ANGLE_P_UP) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(degrees(tune_accel_radss)), (double)(test_accel_max_cdss * 0.01f)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(degrees(tune_accel_radss)), (double)(degrees(test_accel_max_radss))); } } else { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain)); @@ -771,7 +771,7 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float am rotation_rate_filt.reset(0); command_filt.reset(0); target_rate_filt.reset(0); - rotation_rate = 0.0f; + rotation_rate_rads = 0.0f; command_out = 0.0f; filt_target_rate = 0.0f; @@ -904,7 +904,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) } // looks at gain and phase of input rate to output rate - rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()), + rotation_rate_rads = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()), AP::scheduler().get_loop_period_s()); filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()), AP::scheduler().get_loop_period_s()); @@ -917,8 +917,8 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) float dwell_phase_tgt = 0.0f; // wait for dwell to start before determining gain and phase if ((float)(now_ms - dwell_start_time_ms) > pre_calc_cycles * cycle_time_ms || (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && settle_time == 0)) { - freqresp_mtr.update(command_out,command_out,rotation_rate, dwell_freq); - freqresp_tgt.update(command_out,filt_target_rate,rotation_rate, dwell_freq); + freqresp_mtr.update(command_out,command_out,rotation_rate_rads, dwell_freq); + freqresp_tgt.update(command_out,filt_target_rate,rotation_rate_rads, dwell_freq); if (freqresp_mtr.is_cycle_complete()) { if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { @@ -950,7 +950,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) curr_test_tgt.freq = freqresp_tgt.get_freq(); curr_test_tgt.gain = freqresp_tgt.get_gain(); curr_test_tgt.phase = freqresp_tgt.get_phase(); - if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();} + if (test_calc_type == DRB) {test_accel_max_radss = cd_to_rad(freqresp_tgt.get_accel_max());} } // reset cycle_complete to allow indication of next cycle freqresp_tgt.reset_cycle_complete(); @@ -961,7 +961,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) } else { dwell_gain_tgt = freqresp_tgt.get_gain(); dwell_phase_tgt = freqresp_tgt.get_phase(); - if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();} + if (test_calc_type == DRB) {test_accel_max_radss = cd_to_rad(freqresp_tgt.get_accel_max());} cycle_complete_tgt = true; } } @@ -1453,34 +1453,34 @@ void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_fre } -float AC_AutoTune_Heli::target_angle_max_rp_cd() const +float AC_AutoTune_Heli::target_angle_max_rp_rad() const { - return AUTOTUNE_ANGLE_TARGET_MAX_RP_CD; + return AUTOTUNE_ANGLE_TARGET_MAX_RP_RAD; } -float AC_AutoTune_Heli::target_angle_max_y_cd() const +float AC_AutoTune_Heli::target_angle_max_y_rad() const { - return AUTOTUNE_ANGLE_TARGET_MAX_Y_CD; + return AUTOTUNE_ANGLE_TARGET_MAX_Y_RAD; } -float AC_AutoTune_Heli::target_angle_min_rp_cd() const +float AC_AutoTune_Heli::target_angle_min_rp_rad() const { - return AUTOTUNE_ANGLE_TARGET_MIN_RP_CD; + return AUTOTUNE_ANGLE_TARGET_MIN_RP_RAD; } -float AC_AutoTune_Heli::target_angle_min_y_cd() const +float AC_AutoTune_Heli::target_angle_min_y_rad() const { - return AUTOTUNE_ANGLE_TARGET_MIN_Y_CD; + return AUTOTUNE_ANGLE_TARGET_MIN_Y_RAD; } -float AC_AutoTune_Heli::angle_lim_max_rp_cd() const +float AC_AutoTune_Heli::angle_lim_max_rp_rad() const { - return AUTOTUNE_ANGLE_MAX_RP_CD; + return AUTOTUNE_ANGLE_MAX_RP_RAD; } -float AC_AutoTune_Heli::angle_lim_neg_rpy_cd() const +float AC_AutoTune_Heli::angle_lim_neg_rpy_rad() const { - return AUTOTUNE_ANGLE_NEG_RPY_CD; + return AUTOTUNE_ANGLE_NEG_RPY_RAD; } // freq_search_for_phase: general search strategy for specified phase. interpolation done once specified phase has been bounded. @@ -1530,14 +1530,14 @@ void AC_AutoTune_Heli::Log_AutoTune() switch (axis) { case AxisType::ROLL: - Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, rad_to_cd(test_accel_max_radss)); break; case AxisType::PITCH: - Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, rad_to_cd(test_accel_max_radss)); break; case AxisType::YAW: case AxisType::YAW_D: - Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, rad_to_cd(test_accel_max_radss)); break; } } @@ -1546,9 +1546,9 @@ void AC_AutoTune_Heli::Log_AutoTune() void AC_AutoTune_Heli::Log_AutoTuneDetails() { if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) { - Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate); + Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate_rads); } else { - Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f); + Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate_rads, 0.0f, 0.0f); } } diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 97482b525403..121cc84cf1e1 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -163,17 +163,17 @@ class AC_AutoTune_Heli : public AC_AutoTune TARGET = 1, }; - float target_angle_max_rp_cd() const override; + float target_angle_max_rp_rad() const override; - float target_angle_max_y_cd() const override; + float target_angle_max_y_rad() const override; - float target_angle_min_rp_cd() const override; + float target_angle_min_rp_rad() const override; - float target_angle_min_y_cd() const override; + float target_angle_min_y_rad() const override; - float angle_lim_max_rp_cd() const override; + float angle_lim_max_rp_rad() const override; - float angle_lim_neg_rpy_cd() const override; + float angle_lim_neg_rpy_rad() const override; // initialize dwell test or angle dwell test variables void dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type); diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 26b5fcb62ace..32f6a2b10f70 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -20,7 +20,7 @@ * 4) Wait for a calm day and go to a large open area. * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude. * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning: - * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back. + * a) You will see it twitch left and right for a few minutes, then it will repeat forward and back. The twitch size scales with the maximum lean angle. * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests). * When you release the sticks it will continue auto tuning where it left off. * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs. @@ -32,16 +32,24 @@ * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm * * What it's doing during each "twitch": - * a) invokes 90 deg/sec rate request - * b) records maximum "forward" roll rate and bounce back rate - * c) when copter reaches 20 degrees or 1 second has passed, it commands level - * d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P - * e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec) - * f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec) - * g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec) - * h) invokes a 20deg angle request on roll or pitch - * i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg) - * j) decreases stab P by 25% + * a) invokes a rate request set by the rate step limit, constrained to 45 ~ 180 deg/sec on roll and pitch, or 0.75 of the limit constrained to 15 ~ 90 deg/sec on yaw + * b) records the maximum rate in the test direction and the bounce back + * c) commands level when the rate passes the requested rate (plus half of AGGR when increasing rate P), the bounce back exceeds AGGR of the maximum rate, + * the step times out (3 times the time taken to reach 63% of the requested rate, at most 2 seconds), + * or the angle reaches the abort angle (1/2 of the maximum lean angle on roll and pitch, the maximum lean angle on yaw). + * If the abort angle is reached before the bounce back is measured the requested rate is reduced and the twitch repeated. + * d) tries to keep max rotation rate between 80% ~ 100% of requested rate by adjusting rate P + * e) increases rate D until the bounce back becomes greater than AGGR of the maximum rate + * f) decreases rate D until the bounce back becomes less than AGGR of the maximum rate + * g) increases rate P until the maximum rate becomes greater than the requested rate plus half of AGGR, + * reducing rate D and P if the bounce back exceeds AGGR of the maximum rate + * h) invokes an angle request set by the angle step limit, constrained to 1/3 ~ 1/2 of the maximum lean angle on roll and pitch, + * or 0.75 of the limit constrained to 1/6 ~ 1 of the maximum lean angle on yaw + * i) decreases stab P until the maximum angle stays below the requested angle plus half of AGGR + * j) increases stab P until the maximum angle becomes greater than the requested angle plus half of AGGR, + * or passes the requested angle and the rate reverses past zero by more than AGGR of the maximum rate + * k) reduces the tuned rate P, rate D and stab P by GMBK, reduces stab P by a further AGGR, + * and sets the maximum acceleration from the measured acceleration * */ @@ -66,29 +74,29 @@ #define AUTOTUNE_SP_MAX 40.0 // maximum Stab P value #endif -#define AUTOTUNE_SP_MIN 0.5 // maximum Stab P value -#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw +#define AUTOTUNE_SP_MIN 0.5 // minimum Stab P value +#define AUTOTUNE_RP_ACCEL_MIN_RADSS radians(40.0) // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN_RADSS radians(10.0) // Minimum acceleration for Yaw #define AUTOTUNE_Y_FILT_FREQ 10.0 // Autotune filter frequency when testing Yaw #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2 // The margin below the target that we tune D in #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0 // back off from maximum acceleration #define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration // roll and pitch axes -#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step -#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target min roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_RATE_RLLPIT_RADS radians(180.0) // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_RADS radians(45.0) // target min roll/pitch rate during AUTOTUNE_STEP_TWITCHING step // yaw axis -#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_RATE_YAW_RADS radians(90.0) // target yaw rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_RATE_YAW_RADS radians(15.0) // 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_MAX_RP_SCALE 1.0 / 2.0 // maximum 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 // maximum 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 +#define AUTOTUNE_ANGLE_NEG_RP_SCALE 1.0 / 5.0 // maximum allowable angle against the test direction, as a fraction of angle_max // second table of user settable parameters for quadplanes, this // allows us to go beyond the 64 parameter limit @@ -145,30 +153,30 @@ void AC_AutoTune_Multi::do_gcs_announcements() // Prepares all tuning state variables and target values for a new twitch test. void AC_AutoTune_Multi::test_init() { - float target_max_rate; + float target_max_rate_rads; switch (axis) { case AxisType::ROLL: - angle_abort = target_angle_max_rp_cd(); - target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - target_rate = constrain_float(degrees(attitude_control->max_rate_step_bf_roll()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); - target_angle = constrain_float(degrees(attitude_control->max_angle_step_bf_roll()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); + angle_abort_rad = target_angle_max_rp_rad(); + target_max_rate_rads = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_RADS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_RADS); + target_rate_rads = constrain_float(attitude_control->max_rate_step_bf_roll(), AUTOTUNE_TARGET_MIN_RATE_RLLPIT_RADS, target_max_rate_rads); + target_angle_rad = constrain_float(attitude_control->max_angle_step_bf_roll(), target_angle_min_rp_rad(), target_angle_max_rp_rad()); rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz() * 2.0); break; case AxisType::PITCH: - angle_abort = target_angle_max_rp_cd(); - target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - target_rate = constrain_float(degrees(attitude_control->max_rate_step_bf_pitch()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); - target_angle = constrain_float(degrees(attitude_control->max_angle_step_bf_pitch()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); + angle_abort_rad = target_angle_max_rp_rad(); + target_max_rate_rads = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_RADS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_RADS); + target_rate_rads = constrain_float(attitude_control->max_rate_step_bf_pitch(), AUTOTUNE_TARGET_MIN_RATE_RLLPIT_RADS, target_max_rate_rads); + target_angle_rad = constrain_float(attitude_control->max_angle_step_bf_pitch(), target_angle_min_rp_rad(), target_angle_max_rp_rad()); rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz() * 2.0); break; case AxisType::YAW: case AxisType::YAW_D: - angle_abort = target_angle_max_y_cd(); - target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS); - target_rate = constrain_float(degrees(attitude_control->max_rate_step_bf_yaw() * 0.75) * 100.0, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate); - target_angle = constrain_float(degrees(attitude_control->max_angle_step_bf_yaw() * 0.75) * 100.0, target_angle_min_y_cd(), target_angle_max_y_cd()); + angle_abort_rad = target_angle_max_y_rad(); + target_max_rate_rads = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_RADS, step_scaler * AUTOTUNE_TARGET_RATE_YAW_RADS); + target_rate_rads = constrain_float(attitude_control->max_rate_step_bf_yaw() * 0.75, AUTOTUNE_TARGET_MIN_RATE_YAW_RADS, target_max_rate_rads); + target_angle_rad = constrain_float(attitude_control->max_angle_step_bf_yaw() * 0.75, target_angle_min_y_rad(), target_angle_max_y_rad()); if (axis == AxisType::YAW_D) { rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz() * 2.0); } else { @@ -179,16 +187,16 @@ void AC_AutoTune_Multi::test_init() if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) { // TODO: Evaluate whether initializing rotation_rate_filt with start_rate improves results for other axes - rotation_rate_filt.reset(positive_direction ? start_rate : -start_rate); + rotation_rate_filt.reset(positive_direction ? start_rate_rads : -start_rate_rads); } else { rotation_rate_filt.reset(0.0); } angle_step_commanded = false; - test_rate_max = 0.0; - test_rate_min = 0.0; - test_angle_max = 0.0; - test_angle_min = 0.0; - accel_measure_rate_max = 0.0; + test_rate_max_rads = 0.0; + test_rate_min_rads = 0.0; + test_angle_max_rad = 0.0; + test_angle_min_rad = 0.0; + accel_measure_rate_max_rads = 0.0; } void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign) @@ -202,17 +210,17 @@ void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign) // Testing increasing stabilize P gain so will set lean angle target switch (test_axis) { case AxisType::ROLL: - // request roll to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(dir_sign * cd_to_rad(target_angle), 0.0, 0.0); + // request the target roll angle + attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(dir_sign * target_angle_rad, 0.0, 0.0); break; case AxisType::PITCH: - // request pitch to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, dir_sign * cd_to_rad(target_angle), 0.0); + // request the target pitch angle + attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, dir_sign * target_angle_rad, 0.0); break; case AxisType::YAW: case AxisType::YAW_D: - // request yaw to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, 0.0, dir_sign * cd_to_rad(target_angle)); + // request the target yaw angle + attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, 0.0, dir_sign * target_angle_rad); break; } } else { @@ -225,35 +233,35 @@ void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign) switch (test_axis) { case AxisType::ROLL: // override body-frame roll rate - attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(cd_to_rad(dir_sign * target_rate + start_rate), 0.0f, 0.0f); + attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(dir_sign * target_rate_rads + start_rate_rads, 0.0f, 0.0f); break; case AxisType::PITCH: // override body-frame pitch rate - attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, cd_to_rad(dir_sign * target_rate + start_rate), 0.0f); + attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, dir_sign * target_rate_rads + start_rate_rads, 0.0f); break; case AxisType::YAW: case AxisType::YAW_D: // override body-frame yaw rate - attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, 0.0f, cd_to_rad(dir_sign * target_rate + start_rate)); + attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, 0.0f, dir_sign * target_rate_rads + start_rate_rads); break; } } // capture this iteration's rotation rate and lean angle - float gyro_reading = 0; + float gyro_rads = 0; switch (test_axis) { case AxisType::ROLL: - gyro_reading = ahrs_view->get_gyro().x; - lean_angle = dir_sign * (ahrs_view->roll_sensor - (int32_t)start_angle); + gyro_rads = ahrs_view->get_gyro().x; + lean_angle_rad = dir_sign * (ahrs_view->get_roll_rad() - start_angle_rad); break; case AxisType::PITCH: - gyro_reading = ahrs_view->get_gyro().y; - lean_angle = dir_sign * (ahrs_view->pitch_sensor - (int32_t)start_angle); + gyro_rads = ahrs_view->get_gyro().y; + lean_angle_rad = dir_sign * (ahrs_view->get_pitch_rad() - start_angle_rad); break; case AxisType::YAW: case AxisType::YAW_D: - gyro_reading = ahrs_view->get_gyro().z; - lean_angle = dir_sign * wrap_180_cd(ahrs_view->yaw_sensor-(int32_t)start_angle); + gyro_rads = ahrs_view->get_gyro().z; + lean_angle_rad = dir_sign * wrap_PI(ahrs_view->get_yaw_rad() - start_angle_rad); break; } @@ -262,31 +270,31 @@ void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign) switch (tune_type) { case TuneType::ANGLE_P_DOWN: case TuneType::ANGLE_P_UP: - filter_value = dir_sign * (degrees(gyro_reading) * 100.0); + filter_value = dir_sign * gyro_rads; break; default: - filter_value = dir_sign * (degrees(gyro_reading) * 100.0 - start_rate); + filter_value = dir_sign * (gyro_rads - start_rate_rads); break; } - rotation_rate = rotation_rate_filt.apply(filter_value, + rotation_rate_rads = rotation_rate_filt.apply(filter_value, AP::scheduler().get_loop_period_s()); switch (tune_type) { case TuneType::RATE_D_UP: case TuneType::RATE_D_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_cdss, rotation_rate, accel_measure_rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); + twitching_test_rate(lean_angle_rad, rotation_rate_rads, target_rate_rads, test_rate_min_rads, test_rate_max_rads, test_angle_min_rad); + twitching_measure_acceleration(test_accel_max_radss, rotation_rate_rads, accel_measure_rate_max_rads); + twitching_abort_rate(lean_angle_rad, rotation_rate_rads, angle_abort_rad, test_rate_min_rads, test_angle_min_rad); break; case TuneType::RATE_P_UP: - twitching_test_rate(lean_angle, rotation_rate, target_rate * (1 + 0.5 * aggressiveness), test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max_cdss, rotation_rate, accel_measure_rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); + twitching_test_rate(lean_angle_rad, rotation_rate_rads, target_rate_rads * (1 + 0.5 * aggressiveness), test_rate_min_rads, test_rate_max_rads, test_angle_min_rad); + twitching_measure_acceleration(test_accel_max_radss, rotation_rate_rads, accel_measure_rate_max_rads); + twitching_abort_rate(lean_angle_rad, rotation_rate_rads, angle_abort_rad, test_rate_min_rads, test_angle_min_rad); break; case TuneType::ANGLE_P_DOWN: case TuneType::ANGLE_P_UP: - twitching_test_angle(lean_angle, rotation_rate, target_angle * (1 + 0.5 * aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max_cdss, rotation_rate - dir_sign * start_rate, accel_measure_rate_max); + twitching_test_angle(lean_angle_rad, rotation_rate_rads, target_angle_rad * (1 + 0.5 * aggressiveness), test_angle_min_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); + twitching_measure_acceleration(test_accel_max_radss, rotation_rate_rads - dir_sign * start_rate_rads, accel_measure_rate_max_rads); break; case TuneType::RATE_FF_UP: case TuneType::MAX_GAINS: @@ -819,16 +827,16 @@ void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: - updating_rate_p_up_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_p_up_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::PITCH: - updating_rate_p_up_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_p_up_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW: - updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max, false); + updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads, false); break; case AxisType::YAW_D: - updating_rate_p_up_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_p_up_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; } } @@ -838,16 +846,16 @@ void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: - updating_rate_d_up(tune_roll_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_up(tune_roll_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::PITCH: - updating_rate_d_up(tune_pitch_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_up(tune_pitch_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW: - updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW_D: - updating_rate_d_up(tune_yaw_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_up(tune_yaw_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; } } @@ -857,16 +865,16 @@ void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: - updating_rate_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::PITCH: - updating_rate_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW: - updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW_D: - updating_rate_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + updating_rate_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate_rads, test_rate_min_rads, test_rate_max_rads); break; } } @@ -876,14 +884,14 @@ void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: - updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); break; case AxisType::PITCH: - updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW: case AxisType::YAW_D: - updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); break; } } @@ -893,14 +901,14 @@ void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis) { switch (test_axis) { case AxisType::ROLL: - updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); break; case AxisType::PITCH: - updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); break; case AxisType::YAW: case AxisType::YAW_D: - updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); break; } } @@ -941,16 +949,16 @@ void AC_AutoTune_Multi::set_tuning_gains_with_backoff(AxisType test_axis) switch (test_axis) { case AxisType::ROLL: tune_roll_sp = tune_roll_sp * (1.0f - gain_backoff) * (1.0f - aggressiveness); - tune_roll_accel_radss = cd_to_rad(MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_RP_BACKOFF)); + tune_roll_accel_radss = MAX(AUTOTUNE_RP_ACCEL_MIN_RADSS, test_accel_max_radss * AUTOTUNE_ACCEL_RP_BACKOFF); break; case AxisType::PITCH: tune_pitch_sp = tune_pitch_sp * (1.0f - gain_backoff) * (1.0f - aggressiveness); - tune_pitch_accel_radss = cd_to_rad(MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_RP_BACKOFF)); + tune_pitch_accel_radss = MAX(AUTOTUNE_RP_ACCEL_MIN_RADSS, test_accel_max_radss * AUTOTUNE_ACCEL_RP_BACKOFF); break; case AxisType::YAW: case AxisType::YAW_D: tune_yaw_sp = tune_yaw_sp * (1.0f - gain_backoff) * (1.0f - aggressiveness); - tune_yaw_accel_radss = cd_to_rad(MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_Y_BACKOFF)); + tune_yaw_accel_radss = MAX(AUTOTUNE_Y_ACCEL_MIN_RADSS, test_accel_max_radss * AUTOTUNE_ACCEL_Y_BACKOFF); break; } break; @@ -1203,31 +1211,31 @@ void AC_AutoTune_Multi::Log_AutoTune() if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) { switch (axis) { case AxisType::ROLL: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_angle_rad, test_angle_min_rad, test_angle_max_rad, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_radss); break; case AxisType::PITCH: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_angle_rad, test_angle_min_rad, test_angle_max_rad, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_radss); break; case AxisType::YAW: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_angle_rad, test_angle_min_rad, test_angle_max_rad, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max_radss); break; case AxisType::YAW_D: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_angle_rad, test_angle_min_rad, test_angle_max_rad, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_radss); break; } } else { switch (axis) { case AxisType::ROLL: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_rate_rads, test_rate_min_rads, test_rate_max_rads, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_radss); break; case AxisType::PITCH: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_rate_rads, test_rate_min_rads, test_rate_max_rads, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_radss); break; case AxisType::YAW: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_rate_rads, test_rate_min_rads, test_rate_max_rads, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max_radss); break; case AxisType::YAW_D: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss); + Log_Write_AutoTune(axis, tune_type, target_rate_rads, test_rate_min_rads, test_rate_max_rads, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_radss); break; } } @@ -1236,7 +1244,7 @@ void AC_AutoTune_Multi::Log_AutoTune() void AC_AutoTune_Multi::Log_AutoTuneDetails() { - Log_Write_AutoTuneDetails(lean_angle, rotation_rate); + Log_Write_AutoTuneDetails(lean_angle_rad, rotation_rate_rads); } // @LoggerMessage: ATUN @@ -1254,7 +1262,7 @@ void AC_AutoTune_Multi::Log_AutoTuneDetails() // @Field: ddt: maximum measured twitching acceleration // Write an Autotune data packet -void AC_AutoTune_Multi::Log_Write_AutoTune(AxisType _axis, TuneType tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) +void AC_AutoTune_Multi::Log_Write_AutoTune(AxisType _axis, TuneType tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt_radss) { AP::logger().Write( "ATUN", @@ -1265,17 +1273,17 @@ void AC_AutoTune_Multi::Log_Write_AutoTune(AxisType _axis, TuneType tune_step, f AP_HAL::micros64(), axis, tune_step, - meas_target*0.01, - meas_min*0.01, - meas_max*0.01, + degrees(meas_target), + degrees(meas_min), + degrees(meas_max), new_gain_rp, new_gain_rd, new_gain_sp, - new_ddt*0.01); + degrees(new_ddt_radss)); } // Write an Autotune data packet -void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) +void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_rad, float rate_rads) { // @LoggerMessage: ATDE // @Description: AutoTune data packet @@ -1289,41 +1297,41 @@ void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds "F00", "Qff", AP_HAL::micros64(), - angle_cd*0.01, - rate_cds*0.01); + degrees(angle_rad), + degrees(rate_rads)); } #endif // HAL_LOGGING_ENABLED -float AC_AutoTune_Multi::target_angle_max_rp_cd() const +float AC_AutoTune_Multi::target_angle_max_rp_rad() const { - return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MAX_RP_SCALE; + return attitude_control->lean_angle_max_rad() * AUTOTUNE_TARGET_ANGLE_MAX_RP_SCALE; } -float AC_AutoTune_Multi::target_angle_max_y_cd() const +float AC_AutoTune_Multi::target_angle_max_y_rad() 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; + return attitude_control->lean_angle_max_rad() * AUTOTUNE_TARGET_ANGLE_MAX_Y_SCALE; } -float AC_AutoTune_Multi::target_angle_min_rp_cd() const +float AC_AutoTune_Multi::target_angle_min_rp_rad() const { - return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MIN_RP_SCALE; + return attitude_control->lean_angle_max_rad() * AUTOTUNE_TARGET_ANGLE_MIN_RP_SCALE; } -float AC_AutoTune_Multi::target_angle_min_y_cd() const +float AC_AutoTune_Multi::target_angle_min_y_rad() 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; + return attitude_control->lean_angle_max_rad() * AUTOTUNE_TARGET_ANGLE_MIN_Y_SCALE; } -float AC_AutoTune_Multi::angle_lim_max_rp_cd() const +float AC_AutoTune_Multi::angle_lim_max_rp_rad() const { - return attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_ABORT_RP_SCALE; + return attitude_control->lean_angle_max_rad() * AUTOTUNE_ANGLE_ABORT_RP_SCALE; } -float AC_AutoTune_Multi::angle_lim_neg_rpy_cd() const +float AC_AutoTune_Multi::angle_lim_neg_rpy_rad() const { - return attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_NEG_RP_SCALE; + return attitude_control->lean_angle_max_rad() * AUTOTUNE_ANGLE_NEG_RP_SCALE; } // get_testing_step_timeout_ms accessor diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 0499f8b9034e..df2f47311797 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -63,15 +63,15 @@ class AC_AutoTune_Multi : public AC_AutoTune // Resets vehicle-specific gain tracking state (not used in multirotor) void reset_update_gain_variables() override {}; - // Maximum/Minimum target angles for the current axis (in centidegrees) - 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; + // Maximum/Minimum target angles for the current axis (in radians) + float target_angle_max_rp_rad() const override; + float target_angle_max_y_rad() const override; + float target_angle_min_rp_rad() const override; + float target_angle_min_y_rad() const override; - // Absolute and negative angle abort limits for twitch safety (in centidegrees) - float angle_lim_max_rp_cd() const override; - float angle_lim_neg_rpy_cd() const override; + // Absolute and negative angle abort limits for twitch safety (in radians) + float angle_lim_max_rp_rad() const override; + float angle_lim_neg_rpy_rad() const override; // Prepares state and targets for a new twitch test void test_init() override; @@ -118,8 +118,8 @@ class AC_AutoTune_Multi : public AC_AutoTune void Log_Write_AutoTune(AxisType axis, TuneType tune_step, float meas_target, float meas_min, float meas_max, - float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); - void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); + float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt_radss); + void Log_Write_AutoTuneDetails(float angle_rad, float rate_rads); #endif // Defines the sequence of tuning steps (P, D, etc.) @@ -201,16 +201,16 @@ void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_rati bool ignore_next; // Skip next result (used for rate overshoot handling) // Measurement and target values for each test step - float target_angle; - float target_rate; - float angle_abort; + float target_angle_rad; + float target_rate_rads; + float angle_abort_rad; - float test_rate_min; - float test_rate_max; - float test_angle_min; - float test_angle_max; + float test_rate_min_rads; + float test_rate_max_rads; + float test_angle_min_rad; + float test_angle_max_rad; - float accel_measure_rate_max; + float accel_measure_rate_max_rads; }; #endif // AC_AUTOTUNE_ENABLED From 3ad1f77bd490036f81583da32c3fff55b40f02a7 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 8 Nov 2026 08:54:59 +1030 Subject: [PATCH 2/2] AC_AutoTune: Multi: convert the twitch measurements to the test direction once --- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 71 +++++++++------------ libraries/AC_AutoTune/AC_AutoTune_Multi.h | 1 + 2 files changed, 32 insertions(+), 40 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 32f6a2b10f70..e337d824f9cc 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -185,12 +185,7 @@ void AC_AutoTune_Multi::test_init() break; } - if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) { - // TODO: Evaluate whether initializing rotation_rate_filt with start_rate improves results for other axes - rotation_rate_filt.reset(positive_direction ? start_rate_rads : -start_rate_rads); - } else { - rotation_rate_filt.reset(0.0); - } + rotation_rate_filt.reset(start_rate_rads); angle_step_commanded = false; test_rate_max_rads = 0.0; test_rate_min_rads = 0.0; @@ -208,19 +203,20 @@ void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign) if (!angle_step_commanded) { angle_step_commanded = true; // Testing increasing stabilize P gain so will set lean angle target + const float angle_step_rad = dir_sign * target_angle_rad; switch (test_axis) { case AxisType::ROLL: // request the target roll angle - attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(dir_sign * target_angle_rad, 0.0, 0.0); + attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(angle_step_rad, 0.0, 0.0); break; case AxisType::PITCH: // request the target pitch angle - attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, dir_sign * target_angle_rad, 0.0); + attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, angle_step_rad, 0.0); break; case AxisType::YAW: case AxisType::YAW_D: // request the target yaw angle - attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, 0.0, dir_sign * target_angle_rad); + attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, 0.0, angle_step_rad); break; } } else { @@ -230,71 +226,66 @@ void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign) // Testing rate P and D gains so will set body-frame rate targets. // Rate controller will use existing body-frame rates and convert to motor outputs // for all axes except the one we override here. + const float rate_step_rads = start_rate_rads + dir_sign * target_rate_rads; switch (test_axis) { case AxisType::ROLL: // override body-frame roll rate - attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(dir_sign * target_rate_rads + start_rate_rads, 0.0f, 0.0f); + attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(rate_step_rads, 0.0f, 0.0f); break; case AxisType::PITCH: // override body-frame pitch rate - attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, dir_sign * target_rate_rads + start_rate_rads, 0.0f); + attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, rate_step_rads, 0.0f); break; case AxisType::YAW: case AxisType::YAW_D: // override body-frame yaw rate - attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, 0.0f, dir_sign * target_rate_rads + start_rate_rads); + attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, 0.0f, rate_step_rads); break; } } - // capture this iteration's rotation rate and lean angle + // capture this iteration's measured rotation rate and change in angle float gyro_rads = 0; + float angle_change_rad = 0; switch (test_axis) { case AxisType::ROLL: gyro_rads = ahrs_view->get_gyro().x; - lean_angle_rad = dir_sign * (ahrs_view->get_roll_rad() - start_angle_rad); + angle_change_rad = ahrs_view->get_roll_rad() - start_angle_rad; break; case AxisType::PITCH: gyro_rads = ahrs_view->get_gyro().y; - lean_angle_rad = dir_sign * (ahrs_view->get_pitch_rad() - start_angle_rad); + angle_change_rad = ahrs_view->get_pitch_rad() - start_angle_rad; break; case AxisType::YAW: case AxisType::YAW_D: gyro_rads = ahrs_view->get_gyro().z; - lean_angle_rad = dir_sign * wrap_PI(ahrs_view->get_yaw_rad() - start_angle_rad); + angle_change_rad = wrap_PI(ahrs_view->get_yaw_rad() - start_angle_rad); break; } + rotation_rate_rads = rotation_rate_filt.apply(gyro_rads, AP::scheduler().get_loop_period_s()); - // Add filter to measurements - float filter_value; - switch (tune_type) { - case TuneType::ANGLE_P_DOWN: - case TuneType::ANGLE_P_UP: - filter_value = dir_sign * gyro_rads; - break; - default: - filter_value = dir_sign * (gyro_rads - start_rate_rads); - break; - } - rotation_rate_rads = rotation_rate_filt.apply(filter_value, - AP::scheduler().get_loop_period_s()); + // the response since the start of the test, positive in the test direction + lean_angle_rad = dir_sign * angle_change_rad; + test_rate_rads = dir_sign * (rotation_rate_rads - start_rate_rads); + // angle P bounce back is judged against zero rate, not the start rate + const float rate_in_test_dir_rads = dir_sign * rotation_rate_rads; switch (tune_type) { case TuneType::RATE_D_UP: case TuneType::RATE_D_DOWN: - twitching_test_rate(lean_angle_rad, rotation_rate_rads, target_rate_rads, test_rate_min_rads, test_rate_max_rads, test_angle_min_rad); - twitching_measure_acceleration(test_accel_max_radss, rotation_rate_rads, accel_measure_rate_max_rads); - twitching_abort_rate(lean_angle_rad, rotation_rate_rads, angle_abort_rad, test_rate_min_rads, test_angle_min_rad); + twitching_test_rate(lean_angle_rad, test_rate_rads, target_rate_rads, test_rate_min_rads, test_rate_max_rads, test_angle_min_rad); + twitching_measure_acceleration(test_accel_max_radss, test_rate_rads, accel_measure_rate_max_rads); + twitching_abort_rate(lean_angle_rad, test_rate_rads, angle_abort_rad, test_rate_min_rads, test_angle_min_rad); break; case TuneType::RATE_P_UP: - twitching_test_rate(lean_angle_rad, rotation_rate_rads, target_rate_rads * (1 + 0.5 * aggressiveness), test_rate_min_rads, test_rate_max_rads, test_angle_min_rad); - twitching_measure_acceleration(test_accel_max_radss, rotation_rate_rads, accel_measure_rate_max_rads); - twitching_abort_rate(lean_angle_rad, rotation_rate_rads, angle_abort_rad, test_rate_min_rads, test_angle_min_rad); + twitching_test_rate(lean_angle_rad, test_rate_rads, target_rate_rads * (1 + 0.5 * aggressiveness), test_rate_min_rads, test_rate_max_rads, test_angle_min_rad); + twitching_measure_acceleration(test_accel_max_radss, test_rate_rads, accel_measure_rate_max_rads); + twitching_abort_rate(lean_angle_rad, test_rate_rads, angle_abort_rad, test_rate_min_rads, test_angle_min_rad); break; case TuneType::ANGLE_P_DOWN: case TuneType::ANGLE_P_UP: - twitching_test_angle(lean_angle_rad, rotation_rate_rads, target_angle_rad * (1 + 0.5 * aggressiveness), test_angle_min_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); - twitching_measure_acceleration(test_accel_max_radss, rotation_rate_rads - dir_sign * start_rate_rads, accel_measure_rate_max_rads); + twitching_test_angle(lean_angle_rad, rate_in_test_dir_rads, target_angle_rad * (1 + 0.5 * aggressiveness), test_angle_min_rad, test_angle_max_rad, test_rate_min_rads, test_rate_max_rads); + twitching_measure_acceleration(test_accel_max_radss, test_rate_rads, accel_measure_rate_max_rads); break; case TuneType::RATE_FF_UP: case TuneType::MAX_GAINS: @@ -1244,7 +1235,7 @@ void AC_AutoTune_Multi::Log_AutoTune() void AC_AutoTune_Multi::Log_AutoTuneDetails() { - Log_Write_AutoTuneDetails(lean_angle_rad, rotation_rate_rads); + Log_Write_AutoTuneDetails(lean_angle_rad, test_rate_rads); } // @LoggerMessage: ATUN @@ -1288,8 +1279,8 @@ void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_rad, float rate_ra // @LoggerMessage: ATDE // @Description: AutoTune data packet // @Field: TimeUS: Time since system startup - // @Field: Angle: current angle - // @Field: Rate: current angular rate + // @Field: Angle: angle change since the start of the twitch, positive in the test direction + // @Field: Rate: rate change since the start of the twitch, positive in the test direction AP::logger().WriteStreaming( "ATDE", "TimeUS,Angle,Rate", diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index df2f47311797..9a20fa1e6966 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -205,6 +205,7 @@ void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_rati float target_rate_rads; float angle_abort_rad; + float test_rate_rads; // filtered rate change since the start of the twitch, positive in the test direction float test_rate_min_rads; float test_rate_max_rads; float test_angle_min_rad;