diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 545b5f4085..2889971675 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -48,12 +48,10 @@ #define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw #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_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw +#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users #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_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% 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 @@ -69,12 +67,19 @@ #define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step #define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step +// ifdef is not working. Modified multi values to reflect heli requirements #ifdef HELI_BUILD // heli defines #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 5000U // timeout for tuning mode's testing step +#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_TESTING_STEP_TIMEOUT_MS 1000U // timeout for tuning mode's testing step +#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 5000U // timeout for tuning mode's testing step +#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 #endif // HELI_BUILD // second table of user settable parameters for quadplanes, this @@ -87,19 +92,7 @@ const AP_Param::GroupInfo AC_AutoTune::var_info[] = { // @User: Standard AP_GROUPINFO("AXES", 1, AC_AutoTune, axis_bitmask, 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT - // @Param: AGGR - // @DisplayName: Autotune aggressiveness - // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term. - // @Range: 0.05 0.10 - // @User: Standard - AP_GROUPINFO("AGGR", 2, AC_AutoTune, aggressiveness, 0.1f), - - // @Param: MIN_D - // @DisplayName: AutoTune minimum D - // @Description: Defines the minimum D gain - // @Range: 0.001 0.006 - // @User: Standard - AP_GROUPINFO("MIN_D", 3, AC_AutoTune, min_d, 0.001f), +// Indices 2 and 3 where AGGR and MIN_D. These were moved to the Multi SubClass AP_GROUPEND }; @@ -137,6 +130,12 @@ bool AC_AutoTune::init_internals(bool _use_poshold, FALLTHROUGH; case UNINITIALISED: + // initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli + freq_cnt = 0; + start_freq = 0.0f; + stop_freq = 0.0f; + ff_up_first_iter = true; + // autotune has never been run // so store current gains as original gains backup_gains_and_initialise(); @@ -148,6 +147,16 @@ bool AC_AutoTune::init_internals(bool _use_poshold, case TUNING: // we are restarting tuning so restart where we left off + // reset test variables to continue where we left off + // reset dwell test variables if sweep was interrupted in order to restart sweep + if (!is_equal(start_freq,stop_freq)) { + freq_cnt = 0; + start_freq = 0.0f; + stop_freq = 0.0f; + } + step = WAITING_FOR_LEVEL; + step_start_time_ms = AP_HAL::millis(); + level_start_time_ms = step_start_time_ms; // reset gains to tuning-start gains (i.e. low I term) load_gains(GAIN_INTRA_TEST); AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART); @@ -192,11 +201,6 @@ bool AC_AutoTune::init_position_controller(void) // initialise the vertical position controller pos_control->init_z_controller(); - ff_up_first_iter = true; - - // re-initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli - freq_cnt = 0; - return true; } @@ -290,7 +294,12 @@ void AC_AutoTune::run() // get pilot desired climb rate const float target_climb_rate_cms = get_pilot_desired_climb_rate_cms(); - const bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd); + bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd); + // allow pilots to make inputs less than 5 deg in pitch and roll + if (allow_pilot_rp_input() && !pilot_override && fabsf(target_roll_cd) < 500 && fabsf(target_pitch_cd) < 500) { + zero_rp_input = true; + } + const uint32_t now = AP_HAL::millis(); if (!zero_rp_input || !is_zero(target_yaw_rate_cds) || !is_zero(target_climb_rate_cms)) { if (!pilot_override) { @@ -323,7 +332,7 @@ void AC_AutoTune::run() last_pilot_override_warning = now; } } - if (zero_rp_input) { + if (zero_rp_input && !allow_pilot_rp_input()) { // pilot input on throttle and yaw will still use position hold if enabled get_poshold_attitude(target_roll_cd, target_pitch_cd, desired_yaw_cd); } @@ -596,15 +605,24 @@ void AC_AutoTune::control_attitude() switch (axis) { case ROLL: tune_roll_sp = MAX(get_sp_min(), tune_roll_sp * AUTOTUNE_SP_BACKOFF); - tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_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); - tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_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); - tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_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; @@ -665,8 +683,8 @@ void AC_AutoTune::control_attitude() } } - // reverse direction - positive_direction = !positive_direction; + // reverse direction for multicopter twitch test + positive_direction = twitch_reverse_direction(); if (axis == YAW) { attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); @@ -699,6 +717,9 @@ void AC_AutoTune::backup_gains_and_initialise() // no axes are complete axes_completed = 0; + // set the tune sequence + set_tune_sequence(); + // start at the beginning of tune sequence tune_seq_curr = 0; tune_type = tune_seq[tune_seq_curr]; @@ -863,6 +884,7 @@ void AC_AutoTune::load_intra_test_gains() attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax); attitude_control->get_angle_roll_p().kP(orig_roll_sp); + attitude_control->set_accel_roll_max(orig_roll_accel); } if (pitch_enabled()) { attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); @@ -872,6 +894,7 @@ void AC_AutoTune::load_intra_test_gains() attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax); attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); + attitude_control->set_accel_pitch_max(orig_pitch_accel); } if (yaw_enabled()) { attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); @@ -882,6 +905,7 @@ void AC_AutoTune::load_intra_test_gains() attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax); attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF); attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); + attitude_control->set_accel_yaw_max(orig_yaw_accel); } } @@ -892,13 +916,23 @@ void AC_AutoTune::load_test_gains() { switch (axis) { case ROLL: - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); + if (tune_type == MAX_GAINS && !is_zero(tune_roll_rff)) { + attitude_control->get_rate_roll_pid().kP(0.0f); + attitude_control->get_rate_roll_pid().kD(0.0f); + } else { + attitude_control->get_rate_roll_pid().kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().kD(tune_roll_rd); + } attitude_control->get_angle_roll_p().kP(tune_roll_sp); break; case PITCH: - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); + if (tune_type == MAX_GAINS && !is_zero(tune_pitch_rff)) { + attitude_control->get_rate_pitch_pid().kP(0.0f); + attitude_control->get_rate_pitch_pid().kD(0.0f); + } else { + attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); + } attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); break; case YAW: @@ -946,7 +980,7 @@ void AC_AutoTune::save_tuning_gains() } // sanity check the rate P values - if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) { + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && (!is_zero(tune_roll_rp) || allow_zero_rate_p())) { // rate roll gains attitude_control->get_rate_roll_pid().kP(tune_roll_rp); attitude_control->get_rate_roll_pid().kD(tune_roll_rd); @@ -965,7 +999,7 @@ void AC_AutoTune::save_tuning_gains() orig_roll_accel = attitude_control->get_accel_roll_max_cdss(); } - if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) { + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && (!is_zero(tune_pitch_rp) || allow_zero_rate_p())) { // rate pitch gains attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); @@ -1409,80 +1443,145 @@ void AC_AutoTune::rate_ff_test_init() test_rate_filt = 0.0f; test_tgt_rate_filt = 0.0f; filt_target_rate = 0.0f; + settle_time = 200; + phase_out_time = 500; } -void AC_AutoTune::rate_ff_test_run(float max_angle_cds, float target_rate_cds) +void AC_AutoTune::rate_ff_test_run(float max_angle_cd, float target_rate_cds, float dir_sign) { float gyro_reading = 0.0f; float command_reading = 0.0f; float tgt_rate_reading = 0.0f; const uint32_t now = AP_HAL::millis(); + static float trim_command_reading = 0.0f; + static float trim_heading = 0.0f; + static float rate_request_cds; + static float angle_request_cd; + + // TODO make filter leak dependent on dt + const float filt_alpha = 0.0123f; + + target_rate_cds = dir_sign * target_rate_cds; + switch (axis) { case ROLL: gyro_reading = ahrs_view->get_gyro().x; command_reading = motors->get_roll(); tgt_rate_reading = attitude_control->rate_bf_targets().x; - if (ahrs_view->roll_sensor <= max_angle_cds + start_angle - 100.0f && ff_test_phase == 0) { - attitude_control->input_rate_bf_roll_pitch_yaw(target_rate_cds, 0.0f, 0.0f); - } else if (ahrs_view->roll_sensor > max_angle_cds + start_angle - 100.0f && ff_test_phase == 0) { + if (settle_time > 0) { + settle_time--; + trim_command_reading = motors->get_roll(); + rate_request_cds = gyro_reading; + } else if (((ahrs_view->roll_sensor <= max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->roll_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 0) { + rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f); + } else if (((ahrs_view->roll_sensor > max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->roll_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 0) { ff_test_phase = 1; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - attitude_control->rate_bf_roll_target(-target_rate_cds); - } else if (ahrs_view->roll_sensor >= -max_angle_cds + start_angle && ff_test_phase == 1 ) { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - attitude_control->rate_bf_roll_target(-target_rate_cds); - } else if (ahrs_view->roll_sensor < -max_angle_cds + start_angle && ff_test_phase == 1 ) { + rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f); + attitude_control->rate_bf_roll_target(rate_request_cds); + } else if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { + rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds, 0.0f, 0.0f); + attitude_control->rate_bf_roll_target(rate_request_cds); + } else if (((ahrs_view->roll_sensor < -max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->roll_sensor > max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { ff_test_phase = 2; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); + angle_request_cd = attitude_control->get_att_target_euler_cd().x; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd, start_angles.y, 0.0f); } else if (ff_test_phase == 2 ) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); + angle_request_cd += (start_angles.x - angle_request_cd) * filt_alpha; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd, start_angles.y, 0.0f); + phase_out_time--; } break; case PITCH: gyro_reading = ahrs_view->get_gyro().y; command_reading = motors->get_pitch(); tgt_rate_reading = attitude_control->rate_bf_targets().y; - if (ahrs_view->pitch_sensor <= max_angle_cds + start_angle - 100.0f && ff_test_phase == 0) { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, target_rate_cds, 0.0f); - } else if (ahrs_view->pitch_sensor > max_angle_cds + start_angle - 100.0f && ff_test_phase == 0) { + if (settle_time > 0) { + settle_time--; + trim_command_reading = motors->get_pitch(); + rate_request_cds = gyro_reading; + } else if (((ahrs_view->pitch_sensor <= max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 0) { + rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f); + } else if (((ahrs_view->pitch_sensor > max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->pitch_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 0) { ff_test_phase = 1; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - attitude_control->rate_bf_pitch_target(-target_rate_cds); - } else if (ahrs_view->pitch_sensor >= -max_angle_cds + start_angle && ff_test_phase == 1 ) { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - attitude_control->rate_bf_pitch_target(-target_rate_cds); - } else if (ahrs_view->pitch_sensor < -max_angle_cds + start_angle && ff_test_phase == 1 ) { + rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f); + attitude_control->rate_bf_pitch_target(rate_request_cds); + } else if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { + rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds, 0.0f); + attitude_control->rate_bf_pitch_target(rate_request_cds); + } else if (((ahrs_view->pitch_sensor < -max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->pitch_sensor > max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { ff_test_phase = 2; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); + angle_request_cd = attitude_control->get_att_target_euler_cd().y; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd, 0.0f); } else if (ff_test_phase == 2 ) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); + angle_request_cd += (start_angles.x - angle_request_cd) * filt_alpha; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd, 0.0f); + phase_out_time--; } break; case YAW: gyro_reading = ahrs_view->get_gyro().z; command_reading = motors->get_yaw(); tgt_rate_reading = attitude_control->rate_bf_targets().z; - if (wrap_180_cd(ahrs_view->yaw_sensor) <= wrap_180_cd(max_angle_cds + start_angle - 100.0f) && ff_test_phase == 0) { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.5f * target_rate_cds); - } else if (wrap_180_cd(ahrs_view->yaw_sensor) > wrap_180_cd(max_angle_cds + start_angle - 100.0f) && ff_test_phase == 0) { + if (settle_time > 0) { + settle_time--; + trim_command_reading = motors->get_yaw(); + trim_heading = ahrs_view->yaw_sensor; + } else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && is_positive(dir_sign)) + || (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && !is_positive(dir_sign))) + && ff_test_phase == 0) { + rate_request_cds += (target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds); + } else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && is_positive(dir_sign)) + || (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && !is_positive(dir_sign))) + && ff_test_phase == 0) { ff_test_phase = 1; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - attitude_control->rate_bf_yaw_target(-target_rate_cds); - } else if (wrap_180_cd(ahrs_view->yaw_sensor) >= wrap_180_cd(-max_angle_cds + start_angle) && ff_test_phase == 1 ) { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - attitude_control->rate_bf_yaw_target(-target_rate_cds); - } else if (wrap_180_cd(ahrs_view->yaw_sensor) < wrap_180_cd(-max_angle_cds + start_angle) && ff_test_phase == 1 ) { + rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds); + attitude_control->rate_bf_yaw_target(rate_request_cds); + } else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign)) + || (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { + rate_request_cds += (-target_rate_cds - rate_request_cds) * filt_alpha; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds); + attitude_control->rate_bf_yaw_target(rate_request_cds); + } else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && is_positive(dir_sign)) + || (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { ff_test_phase = 2; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); + angle_request_cd = attitude_control->get_att_target_euler_cd().z; + attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd, false); } else if (ff_test_phase == 2 ) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); + angle_request_cd += wrap_180_cd(trim_heading - angle_request_cd) * filt_alpha; + attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd, false); } break; } rotation_rate = rotation_rate_filt.apply(gyro_reading, AP::scheduler().get_loop_period_s()); - command_out = command_filt.apply(command_reading, + command_out = command_filt.apply((command_reading - trim_command_reading), AP::scheduler().get_loop_period_s()); filt_target_rate = target_rate_filt.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s()); @@ -1490,30 +1589,38 @@ void AC_AutoTune::rate_ff_test_run(float max_angle_cds, float target_rate_cds) // record steady state rate and motor command switch (axis) { case ROLL: - if (ahrs_view->roll_sensor >= -max_angle_cds + start_angle && ff_test_phase == 1 ) { + if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { test_rate_filt = rotation_rate; test_command_filt = command_out; test_tgt_rate_filt = filt_target_rate; } break; case PITCH: - if (ahrs_view->pitch_sensor >= -max_angle_cds + start_angle && ff_test_phase == 1 ) { + if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign)) + || (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { test_rate_filt = rotation_rate; test_command_filt = command_out; test_tgt_rate_filt = filt_target_rate; } break; case YAW: - if (wrap_180_cd(ahrs_view->yaw_sensor) >= wrap_180_cd(-max_angle_cds + start_angle) && ff_test_phase == 1 ) { + if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign)) + || (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign))) + && ff_test_phase == 1 ) { test_rate_filt = rotation_rate; test_command_filt = command_out; test_tgt_rate_filt = filt_target_rate; } break; } - if (now - step_start_time_ms >= step_time_limit_ms || ff_test_phase == 2) { + if (now - step_start_time_ms >= step_time_limit_ms || (ff_test_phase == 2 && phase_out_time == 0)) { // we have passed the maximum stop time step = UPDATE_GAINS; + rate_request_cds = 0.0f; + angle_request_cd = 0.0f; } } @@ -1531,9 +1638,46 @@ void AC_AutoTune::dwell_test_init(float filt_freq) test_tgt_rate_filt = 0.0f; filt_target_rate = 0.0f; dwell_start_time_ms = 0.0f; + settle_time = 200; + if (!is_equal(start_freq,stop_freq)) { + sweep.ph180_freq = 0.0f; + sweep.ph180_gain = 0.0f; + sweep.ph180_phase = 0.0f; + sweep.ph270_freq = 0.0f; + sweep.ph270_gain = 0.0f; + sweep.ph270_phase = 0.0f; + sweep.maxgain_gain = 0.0f; + sweep.maxgain_freq = 0.0f; + sweep.maxgain_phase = 0.0f; + sweep.progress = 0; + curr_test_gain = 0.0f; + curr_test_phase = 0.0f; + } + + // save the trim output from PID controller + float ff_term = 0.0f; + float p_term = 0.0f; + switch (axis) { + case ROLL: + trim_meas_rate = ahrs_view->get_gyro().x; + ff_term = attitude_control->get_rate_roll_pid().get_ff(); + p_term = attitude_control->get_rate_roll_pid().get_p(); + break; + case PITCH: + trim_meas_rate = ahrs_view->get_gyro().y; + ff_term = attitude_control->get_rate_pitch_pid().get_ff(); + p_term = attitude_control->get_rate_pitch_pid().get_p(); + break; + case YAW: + trim_meas_rate = ahrs_view->get_gyro().z; + ff_term = attitude_control->get_rate_yaw_pid().get_ff(); + p_term = attitude_control->get_rate_yaw_pid().get_p(); + break; + } + trim_pff_out = ff_term + p_term; } -void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float dwell_freq, float &dwell_gain, float &dwell_phase) +void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase) { float gyro_reading = 0.0f; float command_reading = 0.0f; @@ -1541,94 +1685,254 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float dwell_freq, floa float tgt_attitude = 2.5f * 0.01745f; const uint32_t now = AP_HAL::millis(); float target_rate_cds; + static float trim_command; + static Vector3f trim_attitude_cd; + float sweep_time_ms = 23000; + const float att_hold_gain = 4.5f; + static Vector3f filt_attitude_cd; + Vector3f attitude_cd; + static float filt_command_reading; + static float filt_gyro_reading; + static float filt_tgt_rate_reading; + const float vel_hold_gain = 0.04f; + + float dwell_freq = start_frq; + float cycle_time_ms = 0; + if (!is_zero(dwell_freq)) { + cycle_time_ms = 1000.0f * 2.0f * M_PI / dwell_freq; + } + + const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq); + attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor); + Vector3f velocity_ned, velocity_bf; + if (ahrs_view->get_velocity_NED(velocity_ned)) { + velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw(); + velocity_bf.y = velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw(); + } + + // keep controller from requesting too high of a rate + float target_rate_mag_cds = dwell_freq * tgt_attitude * 5730.0f; + if (target_rate_mag_cds > 5000.0f) { + target_rate_mag_cds = 5000.0f; + } + if (settle_time == 0) { + // give gentler start for the dwell + if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) { + target_rate_cds = -0.5f * target_rate_mag_cds * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); + } else { + if (is_equal(start_frq,stop_frq)) { + target_rate_cds = - target_rate_mag_cds * cosf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001); + } else { + target_rate_cds = waveform((now - dwell_start_time_ms - 0.5f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.5f * cycle_time_ms) * 0.001f, target_rate_mag_cds, start_frq, stop_frq); + dwell_freq = waveform_freq_rads; + } + } + filt_attitude_cd.x += alpha * (attitude_cd.x - filt_attitude_cd.x); + filt_attitude_cd.y += alpha * (attitude_cd.y - filt_attitude_cd.y); + filt_attitude_cd.z += alpha * wrap_180_cd(attitude_cd.z - filt_attitude_cd.z); + } else { + target_rate_cds = 0.0f; + settle_time--; + dwell_start_time_ms = now; + trim_command = command_out; + filt_attitude_cd = attitude_cd; + trim_attitude_cd = attitude_cd; + } switch (axis) { case ROLL: gyro_reading = ahrs_view->get_gyro().x; command_reading = motors->get_roll(); tgt_rate_reading = attitude_control->rate_bf_targets().x; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if (is_zero(dwell_start_time_ms) && ahrs_view->roll_sensor > -0.95f * tgt_attitude * 5730.0f + start_angle) { - attitude_control->rate_bf_roll_target(-1000.0f); - } else if (is_zero(dwell_start_time_ms) && ahrs_view->roll_sensor < -0.95f * tgt_attitude * 5730.0f + start_angle) { - attitude_control->rate_bf_roll_target(0.0f); - dwell_start_time_ms = now; + if (settle_time == 0) { + float ff_rate_contr = 0.0f; + if (tune_roll_rff > 0.0f) { + ff_rate_contr = 5730.0f * trim_command / tune_roll_rff; + } + float trim_rate_cds = ff_rate_contr + att_hold_gain * (trim_attitude_cd.x - filt_attitude_cd.x) - 5730.0f * vel_hold_gain * velocity_bf.y; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, att_hold_gain * (trim_attitude_cd.y - filt_attitude_cd.y), 0.0f); + attitude_control->rate_bf_roll_target(target_rate_cds + trim_rate_cds); } else { - target_rate_cds = tgt_attitude * dwell_freq * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); - attitude_control->rate_bf_roll_target(target_rate_cds); + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + if (!is_zero(attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP())) { + float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_roll_pid().kP()) / (attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP()); + attitude_control->rate_bf_roll_target(trim_tgt_rate_cds); + } } break; case PITCH: gyro_reading = ahrs_view->get_gyro().y; command_reading = motors->get_pitch(); tgt_rate_reading = attitude_control->rate_bf_targets().y; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if (is_zero(dwell_start_time_ms) && ahrs_view->pitch_sensor > -0.95f * tgt_attitude * 5730.0f + start_angle) { - attitude_control->rate_bf_pitch_target(-1000.0f); - } else if (is_zero(dwell_start_time_ms) && ahrs_view->pitch_sensor < -0.95f * tgt_attitude * 5730.0f + start_angle) { - attitude_control->rate_bf_pitch_target(0.0f); - dwell_start_time_ms = now; + if (settle_time == 0) { + float ff_rate_contr = 0.0f; + if (tune_pitch_rff > 0.0f) { + ff_rate_contr = 5730.0f * trim_command / tune_pitch_rff; + } + float trim_rate_cds = ff_rate_contr + att_hold_gain * (trim_attitude_cd.y - filt_attitude_cd.y) + 5730.0f * vel_hold_gain * velocity_bf.x; + attitude_control->input_rate_bf_roll_pitch_yaw(att_hold_gain * (trim_attitude_cd.x - filt_attitude_cd.x), 0.0f, 0.0f); + attitude_control->rate_bf_pitch_target(target_rate_cds + trim_rate_cds); } else { - target_rate_cds = tgt_attitude * dwell_freq * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); - attitude_control->rate_bf_pitch_target(target_rate_cds); + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + if (!is_zero(attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP())) { + float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_pitch_pid().kP()) / (attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP()); + attitude_control->rate_bf_pitch_target(trim_tgt_rate_cds); + } } break; case YAW: gyro_reading = ahrs_view->get_gyro().z; command_reading = motors->get_yaw(); tgt_rate_reading = attitude_control->rate_bf_targets().z; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if (is_zero(dwell_start_time_ms) && ahrs_view->yaw_sensor > -0.95f * tgt_attitude * 5730.0f + start_angle) { - attitude_control->rate_bf_yaw_target(-1000.0f); - } else if (is_zero(dwell_start_time_ms) && ahrs_view->yaw_sensor < -0.95f * tgt_attitude * 5730.0f + start_angle) { - attitude_control->rate_bf_yaw_target(0.0f); - dwell_start_time_ms = now; + if (settle_time == 0) { + float rp_rate_contr = 0.0f; + if (tune_yaw_rp > 0.0f) { + rp_rate_contr = 5730.0f * trim_command / tune_yaw_rp; + } + float trim_rate_cds = rp_rate_contr + att_hold_gain * wrap_180_cd(trim_attitude_cd.z - filt_attitude_cd.z); + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + attitude_control->rate_bf_yaw_target(target_rate_cds + trim_rate_cds); } else { - target_rate_cds = tgt_attitude * dwell_freq * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); - attitude_control->rate_bf_yaw_target(target_rate_cds); + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + if (!is_zero(attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP())) { + float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_yaw_pid().kP()) / (attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP()); + attitude_control->rate_bf_yaw_target(trim_tgt_rate_cds); + } } break; } + if (settle_time == 0) { + filt_command_reading += alpha * (command_reading - filt_command_reading); + filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading); + filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading); + } else { + filt_command_reading = command_reading; + filt_gyro_reading = gyro_reading; + filt_tgt_rate_reading = tgt_rate_reading; + } + // looks at gain and phase of input rate to output rate - rotation_rate = rotation_rate_filt.apply(gyro_reading, + rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading), AP::scheduler().get_loop_period_s()); - filt_target_rate = target_rate_filt.apply(tgt_rate_reading, + filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading), AP::scheduler().get_loop_period_s()); - command_out = command_filt.apply(command_reading, + command_out = command_filt.apply((command_reading - filt_command_reading), AP::scheduler().get_loop_period_s()); - // wait for dwell to start before determining gain and phase - if (!is_zero(dwell_start_time_ms)) { + // wait for dwell to start before determining gain and phase or just start if sweep + if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { if (freq_resp_input == 1) { - determine_gain(filt_target_rate,rotation_rate, dwell_freq, dwell_gain, dwell_phase, dwell_complete, false); + freqresp_rate.determine_gain(filt_target_rate,rotation_rate, dwell_freq); } else { - determine_gain(command_out,rotation_rate, dwell_freq, dwell_gain, dwell_phase, dwell_complete, false); + freqresp_rate.determine_gain(command_out,rotation_rate, dwell_freq); + } + if (freqresp_rate.is_cycle_complete()) { + if (!is_equal(start_frq,stop_frq)) { + curr_test_freq = freqresp_rate.get_freq(); + curr_test_gain = freqresp_rate.get_gain(); + curr_test_phase = freqresp_rate.get_phase(); + // reset cycle_complete to allow indication of next cycle + freqresp_rate.reset_cycle_complete(); + // log sweep data + Log_AutoTuneSweep(); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase)); + } else { + dwell_gain = freqresp_rate.get_gain(); + dwell_phase = freqresp_rate.get_phase(); + } + } } - if (now - step_start_time_ms >= step_time_limit_ms || dwell_complete) { - // we have passed the maximum stop time - step = UPDATE_GAINS; + // set sweep data if a frequency sweep is being conducted + if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) { + // track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped. + if (curr_test_phase > 180.0f && sweep.progress == 0) { + sweep.progress = 1; + } else if (curr_test_phase > 270.0f && sweep.progress == 1) { + sweep.progress = 2; + } + if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f && sweep.progress == 0) { + sweep.ph180_freq = curr_test_freq; + sweep.ph180_gain = curr_test_gain; + sweep.ph180_phase = curr_test_phase; + } + if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f && sweep.progress == 1) { + sweep.ph270_freq = curr_test_freq; + sweep.ph270_gain = curr_test_gain; + sweep.ph270_phase = curr_test_phase; + } + if (curr_test_gain > sweep.maxgain_gain) { + sweep.maxgain_gain = curr_test_gain; + sweep.maxgain_freq = curr_test_freq; + sweep.maxgain_phase = curr_test_phase; + } + if (now - step_start_time_ms >= sweep_time_ms + 200) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); + } + + } else { + if (now - step_start_time_ms >= step_time_limit_ms || freqresp_rate.is_cycle_complete()) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + } } } void AC_AutoTune::angle_dwell_test_init(float filt_freq) { - rotation_rate_filt.reset(0); rotation_rate_filt.set_cutoff_frequency(filt_freq); - command_filt.reset(0); command_filt.set_cutoff_frequency(filt_freq); - target_rate_filt.reset(0); target_rate_filt.set_cutoff_frequency(filt_freq); - test_command_filt = 0.0f; - test_rate_filt = 0.0f; - test_tgt_rate_filt = 0.0f; - filt_target_rate = 0.0f; dwell_start_time_ms = 0.0f; + settle_time = 200; + switch (axis) { + case ROLL: + rotation_rate_filt.reset(((float)ahrs_view->roll_sensor) / 5730.0f); + command_filt.reset(motors->get_roll()); + target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f); + rotation_rate = ((float)ahrs_view->roll_sensor) / 5730.0f; + command_out = motors->get_roll(); + filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; + break; + case PITCH: + rotation_rate_filt.reset(((float)ahrs_view->pitch_sensor) / 5730.0f); + command_filt.reset(motors->get_pitch()); + target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f); + rotation_rate = ((float)ahrs_view->pitch_sensor) / 5730.0f; + command_out = motors->get_pitch(); + filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; + break; + case YAW: + // yaw angle will be centered on zero by removing trim heading + rotation_rate_filt.reset(0.0f); + command_filt.reset(motors->get_yaw()); + target_rate_filt.reset(0.0f); + rotation_rate = 0.0f; + command_out = motors->get_yaw(); + filt_target_rate = 0.0f; + break; + } + if (!is_equal(start_freq,stop_freq)) { + sweep.ph180_freq = 0.0f; + sweep.ph180_gain = 0.0f; + sweep.ph180_phase = 0.0f; + sweep.ph270_freq = 0.0f; + sweep.ph270_gain = 0.0f; + sweep.ph270_phase = 0.0f; + sweep.maxgain_gain = 0.0f; + sweep.maxgain_freq = 0.0f; + sweep.maxgain_phase = 0.0f; + curr_test_gain = 0.0f; + curr_test_phase = 0.0f; + } } -void AC_AutoTune::angle_dwell_test_run(float dwell_freq, float &dwell_gain, float &dwell_phase) +void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase) { float gyro_reading = 0.0f; float command_reading = 0.0f; @@ -1636,212 +1940,175 @@ void AC_AutoTune::angle_dwell_test_run(float dwell_freq, float &dwell_gain, floa float tgt_attitude = 5.0f * 0.01745f; const uint32_t now = AP_HAL::millis(); float target_angle_cd; - float target_rate_cds; - static uint32_t settle_time = 200; - static bool dtrmn_gain; + static float trim_yaw_tgt_reading = 0.0f; + static float trim_yaw_heading_reading = 0.0f; + float sweep_time_ms = 23000; + float dwell_freq = start_frq; + static float filt_command_reading; + static float filt_gyro_reading; + static float filt_tgt_rate_reading; + + const float alpha = calc_lowpass_alpha_dt(0.0025f, 0.2f * start_frq); + + // adjust target attitude based on input_tc so amplitude decrease with increased frequency is minimized + const float freq_co = 1.0f / attitude_control->get_input_tc(); + const float added_ampl = (safe_sqrt(powf(dwell_freq,2.0) + powf(freq_co,2.0)) / freq_co) - 1.0f; + tgt_attitude = constrain_float(0.08725f * (1.0f + 0.2f * added_ampl), 0.08725f, 0.5235f); + + float cycle_time_ms = 0; + if (!is_zero(dwell_freq)) { + cycle_time_ms = 1000.0f * 6.28f / dwell_freq; + } + + if (settle_time == 0) { + // give gentler start for the dwell + if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) { + target_angle_cd = 0.5f * tgt_attitude * 5730.0f * (cosf(dwell_freq * (now - dwell_start_time_ms) * 0.001) - 1.0f); + } else { + if (is_equal(start_frq,stop_frq)) { + target_angle_cd = -tgt_attitude * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001); + } else { + target_angle_cd = -waveform((now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.25f * cycle_time_ms) * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq); + dwell_freq = waveform_freq_rads; + } + } + } else { + target_angle_cd = 0.0f; + trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z; + trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor; + settle_time--; + dwell_start_time_ms = now; + } + + float target_roll_cd, target_pitch_cd, target_yaw_rate_cds; + get_pilot_desired_rp_yrate_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds); switch (axis) { case ROLL: - // gyro_reading = ahrs_view->get_gyro().x; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll_cd + target_angle_cd, target_pitch_cd, 0.0f); command_reading = motors->get_roll(); - // tgt_rate_reading = attitude_control->rate_bf_targets().x; - - attitude_control->bf_feedforward(false); - if (settle_time == 0) { - target_angle_cd = -tgt_attitude * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + start_angles.x, start_angles.y, 0.0f); - } else { - target_angle_cd = 0.0f; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, 0.0f); - settle_time--; - dwell_start_time_ms = now; - } - tgt_rate_reading = target_angle_cd / 5730.0f; - gyro_reading = ((float)ahrs_view->roll_sensor - (float)start_angle + target_angle_cd) / 5730.0f; + tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; + gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f; break; case PITCH: - gyro_reading = ahrs_view->get_gyro().y; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll_cd, target_pitch_cd + target_angle_cd, 0.0f); command_reading = motors->get_pitch(); - tgt_rate_reading = attitude_control->rate_bf_targets().y; - target_angle_cd = tgt_attitude * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, target_angle_cd + start_angles.y, 0.0f); + tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; + gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f; break; case YAW: - gyro_reading = ahrs_view->get_gyro().z; command_reading = motors->get_yaw(); - tgt_rate_reading = attitude_control->rate_bf_targets().z; - target_rate_cds = tgt_attitude * dwell_freq * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, start_angles.y, target_rate_cds); + tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f; + gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f; + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll_cd, target_pitch_cd, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false); break; } - // need to hold off from starting determine gain until neg rate is established - if (!is_zero(dwell_start_time_ms) && tgt_rate_reading < -0.01745f && !dtrmn_gain) { - dtrmn_gain = true; + if (settle_time == 0) { + filt_command_reading += alpha * (command_reading - filt_command_reading); + filt_gyro_reading += alpha * (gyro_reading - filt_gyro_reading); + filt_tgt_rate_reading += alpha * (tgt_rate_reading - filt_tgt_rate_reading); + } else { + filt_command_reading = command_reading; + filt_gyro_reading = gyro_reading; + filt_tgt_rate_reading = tgt_rate_reading; } // looks at gain and phase of input rate to output rate - rotation_rate = rotation_rate_filt.apply(gyro_reading, - AP::scheduler().get_loop_period_s()); - filt_target_rate = target_rate_filt.apply(tgt_rate_reading, - AP::scheduler().get_loop_period_s()); - command_out = command_filt.apply(command_reading, - AP::scheduler().get_loop_period_s()); + rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading), + AP::scheduler().get_loop_period_s()); + filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading), + AP::scheduler().get_loop_period_s()); + command_out = command_filt.apply((command_reading - filt_command_reading), + AP::scheduler().get_loop_period_s()); // wait for dwell to start before determining gain and phase - if (!is_zero(dwell_start_time_ms) && dtrmn_gain) { - determine_gain(filt_target_rate,rotation_rate, dwell_freq, dwell_gain, dwell_phase, dwell_complete, false); + if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { + freqresp_angle.determine_gain_angle(command_out, filt_target_rate, rotation_rate, dwell_freq); + if (freqresp_angle.is_cycle_complete()) { + if (!is_equal(start_frq,stop_frq)) { + curr_test_freq = freqresp_angle.get_freq(); + curr_test_gain = freqresp_angle.get_gain(); + curr_test_phase = freqresp_angle.get_phase(); + test_accel_max = freqresp_angle.get_accel_max(); + // reset cycle_complete to allow indication of next cycle + freqresp_angle.reset_cycle_complete(); + // log sweep data + Log_AutoTuneSweep(); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase)); + } else { + dwell_gain = freqresp_angle.get_gain(); + dwell_phase = freqresp_angle.get_phase(); + } + } } - // if (now - step_start_time_ms >= step_time_limit_ms || dwell_complete) { - if (now - step_start_time_ms >= step_time_limit_ms || dwell_complete) { - // we have passed the maximum stop time - step = UPDATE_GAINS; - settle_time = 200; - dtrmn_gain = false; - // announce results of dwell and update - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(dwell_freq), (double)(dwell_gain), (double)(dwell_phase)); + // set sweep data if a frequency sweep is being conducted + if (!is_equal(start_frq,stop_frq)) { + if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f) { + sweep.ph180_freq = curr_test_freq; + sweep.ph180_gain = curr_test_gain; + sweep.ph180_phase = curr_test_phase; + } + if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f) { + sweep.ph270_freq = curr_test_freq; + sweep.ph270_gain = curr_test_gain; + sweep.ph270_phase = curr_test_phase; + } + if (curr_test_gain > sweep.maxgain_gain) { + sweep.maxgain_gain = curr_test_gain; + sweep.maxgain_freq = curr_test_freq; + sweep.maxgain_phase = curr_test_phase; + } + if (now - step_start_time_ms >= sweep_time_ms + 200) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + } + } else { + if (now - step_start_time_ms >= step_time_limit_ms || freqresp_angle.is_cycle_complete()) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + } } } - -// determine_gain - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input. -// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag -// is set. This function must be reset using the reset flag prior to the next dwell. -void AC_AutoTune::determine_gain(float tgt_rate, float meas_rate, float freq, float &gain, float &phase, bool &cycles_complete, bool funct_reset) +// init_test - initialises the test +float AC_AutoTune::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax) { - static float max_target, max_meas, prev_target, prev_meas; - static float min_target, min_meas, output_ampl[AUTOTUNE_DWELL_CYCLES+1], input_ampl[AUTOTUNE_DWELL_CYCLES+1]; - static float temp_max_target, temp_min_target; - static float temp_max_meas, temp_min_meas; - static uint32_t temp_max_tgt_time[AUTOTUNE_DWELL_CYCLES+1], temp_max_meas_time[AUTOTUNE_DWELL_CYCLES+1]; - static uint32_t max_tgt_time, max_meas_time, new_tgt_time_ms, new_meas_time_ms; - static uint8_t min_target_cnt, max_target_cnt, max_meas_cnt, min_meas_cnt; - static bool new_target = false; - static bool new_meas = false; - uint32_t now = AP_HAL::millis(); + float time_fade_in = 0.0f; // Time to reach maximum amplitude of chirp + float time_fade_out = 0.1 * time_record; // Time to reach zero amplitude after chirp finishes + float time_const_freq = 0.0f; - if (funct_reset) { - max_target_cnt = 0; - min_target_cnt = 0; - max_meas_cnt = 0; - min_meas_cnt = 0; - new_tgt_time_ms = 0; - new_meas_time_ms = 0; - new_target = false; - new_meas = false; - gain = 0.0f; - phase = 0.0f; - cycles_complete = false; - funct_reset = false; - return; + float window; + float output; + + float B = logf(wMax / wMin); + + if (time <= 0.0f) { + window = 0.0f; + } else if (time <= time_fade_in) { + window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in); + } else if (time <= time_record - time_fade_out) { + window = 1.0; + } else if (time <= time_record) { + window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI); + } else { + window = 0.0; } - uint32_t half_cycle_time_ms = 0; - if (!is_zero(freq)) { - half_cycle_time_ms = (uint32_t)(400 * 6.28 / freq); + if (time <= 0.0f) { + waveform_freq_rads = wMin; + output = 0.0f; + } else if (time <= time_const_freq) { + waveform_freq_rads = wMin; + output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq); + } else if (time <= time_record) { + waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq)); + output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1)); + } else { + waveform_freq_rads = wMax; + output = 0.0f; } - - // cycles are complete! determine gain and phase and exit - if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1) { - float delta_time = 0.0f; - float sum_gain = 0.0f; - uint8_t cnt = 0; - uint8_t gcnt = 0; - for (int i = 0; i < 5; i++) { - if (input_ampl[AUTOTUNE_DWELL_CYCLES - i] > 0) { - sum_gain += output_ampl[AUTOTUNE_DWELL_CYCLES - i] / input_ampl[AUTOTUNE_DWELL_CYCLES - i]; - gcnt++; - } - float d_time = (float)(temp_max_meas_time[AUTOTUNE_DWELL_CYCLES - i] - temp_max_tgt_time[AUTOTUNE_DWELL_CYCLES - i]); - if (d_time * 0.001f < 5.0f * 6.28f / freq) { - delta_time += d_time; - cnt++; - } - } - if (gcnt > 0) { - gain = sum_gain / gcnt; - } - if (cnt > 0) { - delta_time = delta_time / cnt; - } - phase = freq * delta_time * 0.001f * 360.0f / 6.28f; - if (phase > 360.0f) { - phase = phase - 360.0f; - } - cycles_complete = true; - return; - } - - // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought - if (!is_positive(prev_target) && is_positive(tgt_rate) && !new_target && now > new_tgt_time_ms) { - new_target = true; - new_tgt_time_ms = now + half_cycle_time_ms; - // reset max_target - max_target = 0.0f; - max_target_cnt++; - temp_min_target = min_target; - if (min_target_cnt > 0 && min_target_cnt < AUTOTUNE_DWELL_CYCLES + 1) { - input_ampl[min_target_cnt] = temp_max_target - temp_min_target; - } - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_tgt_cnt=%f", (double)(max_target_cnt)); - - } else if (is_positive(prev_target) && !is_positive(tgt_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { - new_target = false; - new_tgt_time_ms = now + half_cycle_time_ms; - min_target_cnt++; - temp_max_target = max_target; - if (min_target_cnt < AUTOTUNE_DWELL_CYCLES + 1) { - temp_max_tgt_time[min_target_cnt] = max_tgt_time; - } - min_target = 0.0f; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); - } - - // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought - if (!is_positive(prev_meas) && is_positive(meas_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { - new_meas = true; - new_meas_time_ms = now + half_cycle_time_ms; - // reset max_meas - max_meas = 0.0f; - max_meas_cnt++; - temp_min_meas = min_meas; - if (min_meas_cnt > 0 && min_target_cnt > 0 && min_meas_cnt < AUTOTUNE_DWELL_CYCLES + 1) { - output_ampl[min_meas_cnt] = temp_max_meas - temp_min_meas; - } - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_meas_cnt=%f", (double)(max_meas_cnt)); - } else if (is_positive(prev_meas) && !is_positive(meas_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { - new_meas = false; - new_meas_time_ms = now + half_cycle_time_ms; - min_meas_cnt++; - temp_max_meas = max_meas; - if (min_meas_cnt < AUTOTUNE_DWELL_CYCLES + 1) { - temp_max_meas_time[min_meas_cnt] = max_meas_time; - } - min_meas = 0.0f; - // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); - } - - if (tgt_rate > max_target && new_target) { - max_target = tgt_rate; - max_tgt_time = now; - } - - if (tgt_rate < min_target && !new_target) { - min_target = tgt_rate; - } - - if (meas_rate > max_meas && new_meas) { - max_meas = meas_rate; - max_meas_time = now; - } - - if (meas_rate < min_meas && !new_meas) { - min_meas = meas_rate; - } - - prev_target = tgt_rate; - prev_meas = meas_rate; + return output; } - diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 8eef08a913..32f93698e9 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -22,6 +22,7 @@ #include #include #include +#include "AC_AutoTune_FreqResp.h" #define AUTOTUNE_AXIS_BITMASK_ROLL 1 #define AUTOTUNE_AXIS_BITMASK_PITCH 2 @@ -39,8 +40,6 @@ #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 -#define AUTOTUNE_DWELL_CYCLES 10 - class AC_AutoTune { public: @@ -175,6 +174,12 @@ protected: // 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; + + // returns true if pilot is allowed to make inputs during test + virtual bool allow_pilot_rp_input() = 0; + // get minimum rate P (for any axis) virtual float get_rp_min() const = 0; @@ -184,11 +189,15 @@ protected: // get minimum rate Yaw filter value virtual float get_yaw_rate_filt_min() const = 0; + // reverse direction for twitch test + virtual bool twitch_reverse_direction() = 0; + // get attitude for slow position hold in autotune mode void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); virtual void Log_AutoTune() = 0; virtual void Log_AutoTuneDetails() = 0; + virtual void Log_AutoTuneSweep() = 0; // send message with high level status (e.g. Started, Stopped) void update_gcs(uint8_t message_id) const; @@ -252,6 +261,8 @@ protected: TuneType tune_seq[6]; // holds sequence of tune_types to be performed uint8_t tune_seq_curr; // current tune sequence step + virtual void set_tune_sequence() = 0; + // type of gains to load enum GainType { GAIN_ORIGINAL = 0, @@ -343,18 +354,17 @@ protected: // 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); + void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign); // dwell test used to perform frequency dwells for rate gains void dwell_test_init(float filt_freq); - void dwell_test_run(uint8_t freq_resp_input, float dwell_freq, float &dwell_gain, float &dwell_phase); + void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); // dwell test used to perform frequency dwells for angle gains void angle_dwell_test_init(float filt_freq); - void angle_dwell_test_run(float dwell_freq, float &dwell_gain, float &dwell_phase); + void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); - // determines the gain and phase for a dwell - void determine_gain(float tgt_rate, float meas_rate, float freq, float &gain, float &phase, bool &cycles_complete, bool funct_reset); + float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax); uint8_t ff_test_phase; // phase of feedforward test float test_command_filt; // filtered commanded output @@ -370,12 +380,35 @@ protected: uint8_t freq_cnt; uint8_t freq_cnt_max; float curr_test_freq; - bool dwell_complete; + float curr_test_gain; + float curr_test_phase; Vector3f start_angles; + uint32_t settle_time; + uint32_t phase_out_time; + float waveform_freq_rads; //current frequency for chirp waveform + float start_freq; //start freq for dwell test + float stop_freq; //ending freq for dwell test + float trim_pff_out; // trim output of the PID rate controller for P, I and FF terms + float trim_meas_rate; // trim measured gyro rate LowPassFilterFloat command_filt; // filtered command LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second + // sweep_data tracks the overall characteristics in the response to the frequency sweep + struct sweep_data { + float maxgain_freq; + float maxgain_gain; + float maxgain_phase; + float ph180_freq; + float ph180_gain; + float ph180_phase; + float ph270_freq; + float ph270_gain; + float ph270_phase; + uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg; + }; + sweep_data sweep; + struct max_gain_data { float freq; float phase; @@ -385,4 +418,8 @@ protected: max_gain_data max_rate_p; max_gain_data max_rate_d; + +AC_AutoTune_FreqResp freqresp_rate; +AC_AutoTune_FreqResp freqresp_angle; + }; diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp new file mode 100644 index 0000000000..8cfac6b8ab --- /dev/null +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -0,0 +1,450 @@ +/* +This function receives time history data during a dwell test or frequency sweep and determines the gain and phase of the response to the input. +Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag +is set. This function must be reset using the reset flag prior to the next dwell. +*/ + +#include +#include "AC_AutoTune_FreqResp.h" + +float AC_AutoTune_FreqResp::update() +{ + float dummy = 0.0f; + return dummy; +} + +void AC_AutoTune_FreqResp::init(InputType input_type) +{ + excitation = input_type; + max_target_cnt = 0; + min_target_cnt = 0; + max_meas_cnt = 0; + min_meas_cnt = 0; + input_start_time_ms = 0; + new_tgt_time_ms = 0; + new_meas_time_ms = 0; + new_target = false; + new_meas = false; + curr_test_freq = 0.0f; + curr_test_gain = 0.0f; + curr_test_phase = 0.0f; + max_accel = 0.0f; + max_meas_rate = 0.0f; + max_command = 0.0f; + meas_peak_info_buffer->clear(); + tgt_peak_info_buffer->clear(); + cycle_complete = false; +} + +// determine_gain - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input. +// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag +// is set. This function must be reset using the reset flag prior to the next dwell. +void AC_AutoTune_FreqResp::determine_gain(float tgt_rate, float meas_rate, float tgt_freq) +{ + uint32_t now = AP_HAL::millis(); + uint32_t half_cycle_time_ms = 0; + uint32_t cycle_time_ms = 0; + + if (cycle_complete) { + return; + } + + if (!is_zero(tgt_freq)) { + half_cycle_time_ms = (uint32_t)(300 * 6.28 / tgt_freq); + cycle_time_ms = (uint32_t)(1000 * 6.28 / tgt_freq); + } + + if (input_start_time_ms == 0) { + input_start_time_ms = now; + } + + // cycles are complete! determine gain and phase and exit + if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) { + float delta_time = 0.0f; + float sum_gain = 0.0f; + uint8_t cnt = 0; + uint8_t gcnt = 0; + uint16_t meas_cnt, tgt_cnt; + float meas_ampl = 0.0f; + float tgt_ampl = 0.0f; + uint32_t meas_time = 0; + uint32_t tgt_time = 0; + for (int i = 0; i < AUTOTUNE_DWELL_CYCLES; i++) { + meas_cnt=0; + tgt_cnt=0; + pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time); + pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time); + push_to_meas_buffer(0, 0.0f, 0); + push_to_tgt_buffer(0, 0.0f, 0); + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tgt_cnt=%f meas_cnt=%f", (double)(tgt_cnt), (double)(meas_cnt)); + + if (meas_cnt == tgt_cnt && meas_cnt != 0) { + if (tgt_ampl > 0.0f) { + sum_gain += meas_ampl / tgt_ampl; + gcnt++; + } + float d_time = (float)(meas_time - tgt_time); + if (d_time < 2.0f * (float)cycle_time_ms) { + delta_time += d_time; + cnt++; + } + } else if (meas_cnt > tgt_cnt) { + pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time); + push_to_tgt_buffer(0, 0.0f, 0); + } else if (meas_cnt < tgt_cnt) { + pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time); + push_to_meas_buffer(0, 0.0f, 0); + } + + } + if (gcnt > 0) { + curr_test_gain = sum_gain / gcnt; + } + if (cnt > 0) { + delta_time = delta_time / cnt; + } + curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / 6.28f; + if (curr_test_phase > 360.0f) { + curr_test_phase = curr_test_phase - 360.0f; + } + curr_test_freq = tgt_freq; + cycle_complete = true; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed"); + return; + } + + // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought + if (!is_positive(prev_target) && is_positive(tgt_rate) && !new_target && now > new_tgt_time_ms) { + new_target = true; + new_tgt_time_ms = now + half_cycle_time_ms; + // reset max_target + max_target = 0.0f; + max_target_cnt++; + temp_min_target = min_target; + if (min_target_cnt > 0) { + sweep_tgt.max_time_m1 = temp_max_tgt_time; + temp_max_tgt_time = max_tgt_time; + sweep_tgt.count_m1 = min_target_cnt - 1; + sweep_tgt.amplitude_m1 = temp_tgt_ampl; + temp_tgt_ampl = temp_max_target - temp_min_target; + push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time); + } + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); + + } else if (is_positive(prev_target) && !is_positive(tgt_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { + new_target = false; + new_tgt_time_ms = now + half_cycle_time_ms; + min_target_cnt++; + temp_max_target = max_target; + min_target = 0.0f; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); + } + + // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought + if (!is_positive(prev_meas) && is_positive(meas_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { + new_meas = true; + new_meas_time_ms = now + half_cycle_time_ms; + // reset max_meas + max_meas = 0.0f; + max_meas_cnt++; + temp_min_meas = min_meas; + if (min_meas_cnt > 0 && min_target_cnt > 0) { + sweep_meas.max_time_m1 = temp_max_meas_time; + temp_max_meas_time = max_meas_time; + sweep_meas.count_m1 = min_meas_cnt - 1; + sweep_meas.amplitude_m1 = temp_meas_ampl; + temp_meas_ampl = temp_max_meas - temp_min_meas; + push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time); + + if (excitation == SWEEP) { + float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1); + if (!is_zero(tgt_period)) { + curr_test_freq = 6.28f / tgt_period; + } else { + curr_test_freq = 0.0f; + } + if (!is_zero(sweep_tgt.amplitude_m1)) { + curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1; + } else { + curr_test_gain = 0.0f; + } + curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / 6.28f; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f sweepgain=%f", (double)(curr_test_freq), (double)(curr_test_gain)); + cycle_complete = true; + } + } + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); + } else if (is_positive(prev_meas) && !is_positive(meas_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { + new_meas = false; + new_meas_time_ms = now + half_cycle_time_ms; + min_meas_cnt++; + temp_max_meas = max_meas; + min_meas = 0.0f; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); + } + + if (tgt_rate > max_target && new_target) { + max_target = tgt_rate; + max_tgt_time = now; + } + + if (tgt_rate < min_target && !new_target) { + min_target = tgt_rate; + } + + if (meas_rate > max_meas && new_meas) { + max_meas = meas_rate; + max_meas_time = now; + } + + if (meas_rate < min_meas && !new_meas) { + min_meas = meas_rate; + } + + prev_target = tgt_rate; + prev_meas = meas_rate; +} + +// determine_gain_angle - this function receives time history data during a dwell test input and determines the gain and phase of the response to the input. +// Once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycles_complete flag +// is set. This function must be reset using the reset flag prior to the next dwell. +void AC_AutoTune_FreqResp::determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq) +{ + + uint32_t now = AP_HAL::millis(); + float dt = 0.0025; + uint32_t half_cycle_time_ms = 0; + uint32_t cycle_time_ms = 0; + + if (cycle_complete) { + return; + } + + if (!is_zero(tgt_freq)) { + half_cycle_time_ms = (uint32_t)(300 * 6.28 / tgt_freq); + cycle_time_ms = (uint32_t)(1000 * 6.28 / tgt_freq); + } + + if (input_start_time_ms == 0) { + input_start_time_ms = now; + prev_tgt_angle = tgt_angle; + prev_meas_angle = meas_angle; + prev_target = 0.0f; + prev_meas = 0.0f; + } + + target_rate = (tgt_angle - prev_tgt_angle) / dt; + measured_rate = (meas_angle - prev_meas_angle) / dt; + + // cycles are complete! determine gain and phase and exit + if (max_meas_cnt > AUTOTUNE_DWELL_CYCLES + 1 && max_target_cnt > AUTOTUNE_DWELL_CYCLES + 1 && excitation == DWELL) { + float delta_time = 0.0f; + float sum_gain = 0.0f; + uint8_t cnt = 0; + uint8_t gcnt = 0; + uint16_t meas_cnt, tgt_cnt; + float meas_ampl = 0.0f; + float tgt_ampl = 0.0f; + uint32_t meas_time = 0; + uint32_t tgt_time = 0; + for (int i = 0; i < AUTOTUNE_DWELL_CYCLES; i++) { + meas_cnt=0; + tgt_cnt=0; + pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time); + pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time); + push_to_meas_buffer(0, 0.0f, 0); + push_to_tgt_buffer(0, 0.0f, 0); + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tgt_cnt=%f meas_cnt=%f", (double)(tgt_cnt), (double)(meas_cnt)); + + if (meas_cnt == tgt_cnt && meas_cnt != 0) { + if (tgt_ampl > 0.0f) { + sum_gain += meas_ampl / tgt_ampl; + gcnt++; + } + float d_time = (float)(meas_time - tgt_time); + if (d_time < 2.0f * (float)cycle_time_ms) { + delta_time += d_time; + cnt++; + } + } else if (meas_cnt > tgt_cnt) { + pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time); + push_to_tgt_buffer(0, 0.0f, 0); + } else if (meas_cnt < tgt_cnt) { + pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time); + push_to_meas_buffer(0, 0.0f, 0); + } + + } + if (gcnt > 0) { + curr_test_gain = sum_gain / gcnt; + } + if (cnt > 0) { + delta_time = delta_time / cnt; + } + curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / 6.28f; + if (curr_test_phase > 360.0f) { + curr_test_phase = curr_test_phase - 360.0f; + } + float dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f; + if (!is_zero(max_command)) { + // normalize accel for input size + dwell_max_accel = dwell_max_accel / (2.0f * max_command); + } + if (dwell_max_accel > max_accel) { + max_accel = dwell_max_accel; + } + curr_test_freq = tgt_freq; + cycle_complete = true; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cycles completed"); + return; + } + + // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought + if (is_positive(prev_target) && !is_positive(target_rate) && !new_target && now > new_tgt_time_ms) { + new_target = true; + new_tgt_time_ms = now + half_cycle_time_ms; + // reset max_target + max_target = 0.0f; + max_target_cnt++; + temp_min_target = min_target; + if (min_target_cnt > 0) { + sweep_tgt.max_time_m1 = temp_max_tgt_time; + temp_max_tgt_time = max_tgt_time; + sweep_tgt.count_m1 = min_target_cnt - 1; + sweep_tgt.amplitude_m1 = temp_tgt_ampl; + temp_tgt_ampl = temp_max_target - temp_min_target; + push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time); + } + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_tgt_cnt=%f", (double)(max_target_cnt)); + + } else if (!is_positive(prev_target) && is_positive(target_rate) && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { + new_target = false; + new_tgt_time_ms = now + half_cycle_time_ms; + min_target_cnt++; + temp_max_target = max_target; + min_target = 0.0f; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_tgt_cnt=%f", (double)(min_target_cnt)); + } + + // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought + if (is_positive(prev_meas) && !is_positive(measured_rate) && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { + new_meas = true; + new_meas_time_ms = now + half_cycle_time_ms; + // reset max_meas + max_meas = 0.0f; + max_meas_cnt++; + temp_min_meas = min_meas; + if (min_meas_cnt > 0 && min_target_cnt > 0) { + sweep_meas.max_time_m1 = temp_max_meas_time; + temp_max_meas_time = max_meas_time; + sweep_meas.count_m1 = min_meas_cnt - 1; + sweep_meas.amplitude_m1 = temp_meas_ampl; + temp_meas_ampl = temp_max_meas - temp_min_meas; + push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time); + + if (excitation == SWEEP) { + float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1); + if (!is_zero(tgt_period)) { + curr_test_freq = 6.28f / tgt_period; + } else { + curr_test_freq = 0.0f; + } + if (!is_zero(sweep_tgt.amplitude_m1)) { + curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1; + } else { + curr_test_gain = 0.0f; + } + curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / 6.28f; + //gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase)); + cycle_complete = true; + } + } + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_meas_cnt=%f", (double)(max_meas_cnt)); + } else if (!is_positive(prev_meas) && is_positive(measured_rate) && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { + new_meas = false; + new_meas_time_ms = now + half_cycle_time_ms; + min_meas_cnt++; + temp_max_meas = max_meas; + min_meas = 0.0f; + // gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: min_meas_cnt=%f", (double)(min_meas_cnt)); + } + + if (tgt_angle > max_target && new_target) { + max_target = tgt_angle; + max_tgt_time = now; + } + + if (tgt_angle < min_target && !new_target) { + min_target = tgt_angle; + } + + if (meas_angle > max_meas && new_meas) { + max_meas = meas_angle; + max_meas_time = now; + } + + if (meas_angle < min_meas && !new_meas) { + min_meas = meas_angle; + } + + if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) { + if (measured_rate > max_meas_rate) { + max_meas_rate = measured_rate; + } + if (command > max_command) { + max_command = command; + } + } + + prev_target = target_rate; + prev_meas = measured_rate; + prev_tgt_angle = tgt_angle; + prev_meas_angle = meas_angle; +} + +// push to peak info buffer +void AC_AutoTune_FreqResp::push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms) +{ + peak_info sample; + sample.curr_count = count; + sample.amplitude = amplitude; + sample.time_ms = time_ms; + meas_peak_info_buffer->push(sample); +} + +// pull from peak info buffer +void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &litude, uint32_t &time_ms) +{ + peak_info sample; + if (!meas_peak_info_buffer->pop(sample)) { + // no sample + return; + } + count = sample.curr_count; + amplitude = sample.amplitude; + time_ms = sample.time_ms; +} + +// push to peak info buffer +void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms) +{ + peak_info sample; + sample.curr_count = count; + sample.amplitude = amplitude; + sample.time_ms = time_ms; + tgt_peak_info_buffer->push(sample); + +} + +// pull from peak info buffer +void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &litude, uint32_t &time_ms) +{ + peak_info sample; + if (!tgt_peak_info_buffer->pop(sample)) { + // no sample + return; + } + count = sample.curr_count; + amplitude = sample.amplitude; + time_ms = sample.time_ms; +} diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h new file mode 100644 index 0000000000..f75743256b --- /dev/null +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h @@ -0,0 +1,89 @@ +#pragma once + +/* + Gain and phase determination algorithm +*/ + +#include +#include + +#define AUTOTUNE_DWELL_CYCLES 6 + +class AC_AutoTune_FreqResp { +public: + // Constructor + AC_AutoTune_FreqResp() +{ + // initialize test variables + meas_peak_info_buffer = new ObjectBuffer(AUTOTUNE_DWELL_CYCLES); + tgt_peak_info_buffer = new ObjectBuffer(AUTOTUNE_DWELL_CYCLES); +} + +// CLASS_NO_COPY(AC_PI); + + // update calculations + float update(); + + enum InputType { + DWELL = 0, + SWEEP = 1, + }; + + void init(InputType input_type); + + // determines the gain and phase for a dwell + void determine_gain(float tgt_rate, float meas_rate, float tgt_freq); + + // determines the gain and phase for a dwell + void determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq); + + // enable external query if cycle is complete and phase and gain data are available + bool is_cycle_complete() { return cycle_complete;} + + void reset_cycle_complete() { cycle_complete = false; } + + // frequency response accessors + float get_freq() { return curr_test_freq; } + float get_gain() { return curr_test_gain; } + float get_phase() { return curr_test_phase; } + float get_accel_max() { return max_accel; } + +private: + float max_target, max_meas, prev_target, prev_meas, prev_tgt_angle, prev_meas_angle; + float min_target, min_meas, temp_meas_ampl, temp_tgt_ampl; + float temp_max_target, temp_min_target, target_rate, measured_rate, max_meas_rate, max_command; + float temp_max_meas, temp_min_meas; + uint32_t temp_max_tgt_time, temp_max_meas_time; + uint32_t max_tgt_time, max_meas_time, new_tgt_time_ms, new_meas_time_ms, input_start_time_ms; + uint16_t min_target_cnt, max_target_cnt, max_meas_cnt, min_meas_cnt; + bool new_target = false; + bool new_meas = false; + bool cycle_complete = false; + float curr_test_freq, curr_test_gain, curr_test_phase; + float max_accel; + InputType excitation; + + // sweep_peak_finding_data tracks the peak data + struct sweep_peak_finding_data { + uint16_t count_m1; + float amplitude_m1; + float max_time_m1; + }; + sweep_peak_finding_data sweep_meas; + sweep_peak_finding_data sweep_tgt; + + //store determine gain data in ring buffer + struct peak_info { + uint16_t curr_count; + float amplitude; + uint32_t time_ms; + + }; + ObjectBuffer *meas_peak_info_buffer; + ObjectBuffer *tgt_peak_info_buffer; + void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms); + void pull_from_meas_buffer(uint16_t &count, float &litude, uint32_t &time_ms); + void push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms); + void pull_from_tgt_buffer(uint16_t &count, float &litude, uint32_t &time_ms); + +}; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index df7a0dafa1..4beb38e178 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -32,44 +32,133 @@ #define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value #define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value #define AUTOTUNE_RLPF_MAX 20.0f // maximum Rate Yaw filter value -#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value -#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value -#define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value -#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value +#define AUTOTUNE_RP_MIN 0.001f // minimum Rate P value +#define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value +#define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value +#define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value +#define AUTOTUNE_RFF_MAX 0.5f // maximum Stab P value +#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_SEQ_BITMASK_VFF 1 +#define AUTOTUNE_SEQ_BITMASK_RATE_D 2 +#define AUTOTUNE_SEQ_BITMASK_ANGLE_P 4 +#define AUTOTUNE_SEQ_BITMASK_MAX_GAIN 8 + +const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = { + AP_NESTEDGROUPINFO(AC_AutoTune, 0), + + // @Param: SEQ + // @DisplayName: AutoTune Sequence Bitmask + // @Description: 2-byte bitmask to select what tuning should be performed. Max gain automatically performed if Rate D is selected. Values: 7:All,1:VFF Only,2:Rate D Only,4:Angle P Only,8:Max Gain Only,3:VFF and Rate D (incl max gain),5:VFF and Angle P,13:VFF max gain and angle P + // @Bitmask: 0:VFF,1:Rate D,2:Angle P,3:Max Gain Only + // @User: Standard + AP_GROUPINFO("SEQ", 1, AC_AutoTune_Heli, seq_bitmask, 5), + + // @Param: MIN_FRQ + // @DisplayName: AutoTune minimum sweep frequency + // @Description: Defines the start frequency for sweeps and dwells + // @Range: 10 30 + // @User: Standard + AP_GROUPINFO("MIN_FRQ", 2, AC_AutoTune_Heli, min_sweep_freq, 10.0f), + + // @Param: MAX_FRQ + // @DisplayName: AutoTune maximum sweep frequency + // @Description: Defines the end frequency for sweeps and dwells + // @Range: 50 120 + // @User: Standard + AP_GROUPINFO("MAX_FRQ", 3, AC_AutoTune_Heli, max_sweep_freq, 70.0f), + + // @Param: MAX_GN + // @DisplayName: AutoTune maximum response gain + // @Description: Defines the response gain (output/input) to tune + // @Range: 1 2.5 + // @User: Standard + AP_GROUPINFO("MAX_GN", 4, AC_AutoTune_Heli, max_resp_gain, 1.4f), + + AP_GROUPEND +}; + +// constructor +AC_AutoTune_Heli::AC_AutoTune_Heli() +{ + tune_seq[0] = TUNE_COMPLETE; + AP_Param::setup_object_defaults(this, var_info); +} + void AC_AutoTune_Heli::test_init() { - if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) { rate_ff_test_init(); step_time_limit_ms = 10000; } else if (tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP) { - // initialize start frequency and determine gain function when dwell test is used - if (freq_cnt == 0) { - test_freq[0] = 2.0f * 3.14159f * 2.0f; - curr_test_freq = test_freq[0]; - // reset determine_gain function for first use in the event autotune is restarted - determine_gain(0.0f, 0.0f, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt], dwell_complete, true); + if (is_zero(start_freq)) { + if (test_phase[12] > 160.0f && test_phase[12] < 180.0f && tune_type == RP_UP) { + freq_cnt = 12; + curr_test_freq = test_freq[12]; + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } else if (method == 1 && tune_type == RP_UP) { + freq_cnt = 12; + test_freq[12] = sweep.maxgain_freq; + curr_test_freq = test_freq[12]; + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } else if (!is_zero(max_rate_p.freq) && tune_type == RP_UP) { + freq_cnt = 12; + test_freq[12] = max_rate_p.freq; + curr_test_freq = test_freq[12]; + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } else if (tune_type == MAX_GAINS || tune_type == RD_UP) { + start_freq = min_sweep_freq; + stop_freq = max_sweep_freq; + method = 0; //reset the method for rate D and rate P tuning. + } else { + test_freq[0] = 2.0f * 3.14159f * 2.0f; + curr_test_freq = test_freq[0]; + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } } - dwell_test_init(curr_test_freq); - if (!is_zero(curr_test_freq)) { + if (!is_equal(start_freq,stop_freq)) { + // initialize determine_gain function whenever test is initialized + freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP); + dwell_test_init(stop_freq); + } else { + // initialize determine_gain function whenever test is initialized + freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL); + dwell_test_init(start_freq); + } + if (!is_zero(start_freq)) { // 4 seconds is added to allow aircraft to achieve start attitude. Then the time to conduct the dwells is added to it. - step_time_limit_ms = (uint32_t)(4000 + (float)(AUTOTUNE_DWELL_CYCLES + 2) * 1000.0f * 6.28f / curr_test_freq); + step_time_limit_ms = (uint32_t)(4000 + (float)(AUTOTUNE_DWELL_CYCLES + 2) * 1000.0f * 6.28f / start_freq); } } else if (tune_type == SP_UP) { - // initialize start frequency and determine gain function when dwell test is used - if (freq_cnt == 0) { - test_freq[0] = 0.5f * 3.14159f * 2.0f; + // initialize start frequency when dwell test is used + if (is_zero(start_freq)) { + test_freq[0] = 1.5f * 3.14159f * 2.0f; curr_test_freq = test_freq[0]; - // reset determine_gain function for first use in the event autotune is restarted - determine_gain(0.0f, 0.0f, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt], dwell_complete, true); + test_accel_max = 0.0f; + start_freq = min_sweep_freq; + stop_freq = max_sweep_freq; } - angle_dwell_test_init(curr_test_freq); - if (!is_zero(curr_test_freq)) { + + if (!is_equal(start_freq,stop_freq)) { + // initialize determine gain function + freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP); + dwell_test_init(stop_freq); + } else { + // initialize determine gain function + freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL); + dwell_test_init(start_freq); + } + + // TODO add time limit for sweep test + if (!is_zero(start_freq)) { // 1 seconds is added for a little buffer. Then the time to conduct the dwells is added to it. - step_time_limit_ms = (uint32_t)(2000 + (float)(AUTOTUNE_DWELL_CYCLES + 2) * 1000.0f * 6.28f / curr_test_freq); + step_time_limit_ms = (uint32_t)(2000 + (float)(AUTOTUNE_DWELL_CYCLES + 7) * 1000.0f * 6.28f / start_freq); } } else { @@ -79,15 +168,17 @@ void AC_AutoTune_Heli::test_init() void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) { - + if (tune_type == SP_UP) { - angle_dwell_test_run(curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + angle_dwell_test_run(start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); } else if ((tune_type == RFF_UP) || (tune_type == RFF_DOWN)) { - rate_ff_test_run(AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD, AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS); + rate_ff_test_run(AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD, AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS, dir_sign); } else if (tune_type == RP_UP || tune_type == RD_UP) { - dwell_test_run(1, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); } else if (tune_type == MAX_GAINS) { - dwell_test_run(0, curr_test_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + dwell_test_run(0, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + } else if (tune_type == TUNE_COMPLETE) { + return; } else { step = UPDATE_GAINS; } @@ -178,7 +269,8 @@ void AC_AutoTune_Heli::load_test_gains() if (tune_type == SP_UP) { attitude_control->get_rate_roll_pid().kI(orig_roll_ri); } else { - attitude_control->get_rate_roll_pid().kI(0.0f); + // freeze integrator to hold trim by making i term small during rate controller tuning + attitude_control->get_rate_roll_pid().kI(0.01f * orig_roll_ri); } attitude_control->get_rate_roll_pid().ff(tune_roll_rff); attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); @@ -188,18 +280,22 @@ void AC_AutoTune_Heli::load_test_gains() if (tune_type == SP_UP) { attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); } else { - attitude_control->get_rate_pitch_pid().kI(0.0f); + // freeze integrator to hold trim by making i term small during rate controller tuning + attitude_control->get_rate_pitch_pid().kI(0.01f * orig_pitch_ri); } attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff); attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); attitude_control->get_rate_pitch_pid().slew_limit(0.0f); break; case YAW: + // freeze integrator to hold trim by making i term small during rate controller tuning attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd); attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff); attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); attitude_control->get_rate_yaw_pid().slew_limit(0.0f); + if (tune_type == SP_UP) { + } break; } } @@ -212,7 +308,7 @@ void AC_AutoTune_Heli::save_tuning_gains() AC_AutoTune::save_tuning_gains(); // sanity check the rate P values - if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) { + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) { // rate roll gains attitude_control->get_rate_roll_pid().ff(tune_roll_rff); attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); @@ -225,7 +321,7 @@ void AC_AutoTune_Heli::save_tuning_gains() orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); } - if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) { + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) { // rate pitch gains attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff); attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); @@ -283,13 +379,13 @@ void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis) switch (test_axis) { case ROLL: - updating_rate_d_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); + updating_rate_p_up(tune_roll_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); break; case PITCH: - updating_rate_d_up(tune_pitch_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); + updating_rate_p_up(tune_pitch_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); break; case YAW: - updating_rate_d_up(tune_yaw_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); + updating_rate_p_up(tune_yaw_rp, test_freq, test_gain, test_phase, freq_cnt, max_rate_p); break; } } @@ -338,6 +434,10 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis) break; case YAW: updating_rate_ff_up(tune_yaw_rff, test_tgt_rate_filt*5730.0f, test_rate_filt*5730.0f, test_command_filt); + // TODO make FF updating routine determine when to set rff gain to zero based on A/C response + if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && counter == AUTOTUNE_SUCCESS_COUNT) { + tune_yaw_rff = 0.0f; + } break; } } @@ -346,7 +446,8 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis) void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis) { // announce results of dwell and update - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt])); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f accel=%f", (double)(test_phase[freq_cnt]), (double)(test_accel_max)); switch (test_axis) { case ROLL: @@ -377,6 +478,13 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis) break; case YAW: updating_max_gains(&test_freq[0], &test_gain[0], &test_phase[0], freq_cnt, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd); + // rate P and rate D can be non zero for yaw and need to be included in the max allowed gain + if (!is_zero(max_rate_p.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) { + max_rate_p.max_allowed += tune_yaw_rp; + } + if (!is_zero(max_rate_d.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) { + max_rate_d.max_allowed += tune_yaw_rd; + } break; } } @@ -385,30 +493,134 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis) // 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) { + + static bool first_dir_complete; + static float first_dir_rff; if (ff_up_first_iter) { if (!is_zero(meas_rate)) { tune_ff = 5730.0f * meas_command / meas_rate; } - tune_ff = constrain_float(tune_ff, 0.01, 1); + tune_ff = constrain_float(tune_ff, AUTOTUNE_RFF_MIN, AUTOTUNE_RFF_MAX); ff_up_first_iter = false; } else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 1.05f * fabsf(rate_target) && fabsf(meas_rate) > 0.95f * fabsf(rate_target)) { - counter = AUTOTUNE_SUCCESS_COUNT; - tune_ff = 0.75f * tune_ff; + if (!first_dir_complete) { + first_dir_rff = tune_ff; + first_dir_complete = true; + positive_direction = !positive_direction; + } else { + counter = AUTOTUNE_SUCCESS_COUNT; + tune_ff = 0.95f * 0.5 * (tune_ff + first_dir_rff); + tune_ff = constrain_float(tune_ff, AUTOTUNE_RFF_MIN, AUTOTUNE_RFF_MAX); + ff_up_first_iter = true; + first_dir_complete = false; + } } else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) > 1.05f * fabsf(rate_target)) { tune_ff = 0.98f * tune_ff; + if (tune_ff <= AUTOTUNE_RFF_MIN) { + tune_ff = AUTOTUNE_RFF_MIN; + counter = AUTOTUNE_SUCCESS_COUNT; + AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + ff_up_first_iter = true; + first_dir_complete = false; + } } else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 0.95f * fabsf(rate_target)) { tune_ff = 1.02f * tune_ff; + if (tune_ff >= AUTOTUNE_RFF_MAX) { + tune_ff = AUTOTUNE_RFF_MAX; + counter = AUTOTUNE_SUCCESS_COUNT; + AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + ff_up_first_iter = true; + first_dir_complete = false; + } } else { if (!is_zero(meas_rate)) { tune_ff = 5730.0f * meas_command / meas_rate; } - tune_ff = constrain_float(tune_ff, 0.01, 1); + tune_ff = constrain_float(tune_ff, AUTOTUNE_RFF_MIN, AUTOTUNE_RFF_MAX); } } -void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, float gain_incr, float max_gain) +void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p) { + float test_freq_incr = 0.25f * 3.14159f * 2.0f; + static uint8_t prev_good_frq_cnt; + static float prev_gain; + + if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { + if (frq_cnt == 0) { + tune_p = max_gain_p.max_allowed * 0.10f; + freq_cnt_max = 0; + } else if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) { + prev_good_frq_cnt = frq_cnt; + } else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) { + if (phase[frq_cnt] - 360.0f < 180.0f) { + prev_good_frq_cnt = frq_cnt; + } + } else if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) { + frq_cnt = 11; + } + frq_cnt++; + if (frq_cnt == 12) { + freq[frq_cnt] = freq[prev_good_frq_cnt]; + curr_test_freq = freq[frq_cnt]; + } else { + freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; + curr_test_freq = freq[frq_cnt]; + } + } else if (is_equal(start_freq,stop_freq) && method == 2) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt])); + if (is_zero(tune_p)) { + tune_p = 0.05f * max_gain_p.max_allowed; + } else if (phase[frq_cnt] > 180.0f) { + curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test_freq; + } else if (phase[frq_cnt] < 160.0f) { + curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test_freq; + } else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { + if (gain[frq_cnt] < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) { + tune_p += 0.05f * max_gain_p.max_allowed; + } else { + counter = AUTOTUNE_SUCCESS_COUNT; + // reset curr_test_freq and frq_cnt for next test + curr_test_freq = freq[0]; + frq_cnt = 0; + tune_p -= 0.05f * max_gain_p.max_allowed; + tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); +// prev_gain = 0.0f; + } + } +// prev_gain = gain[frq_cnt]; + } else if (is_equal(start_freq,stop_freq) && method == 1) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt])); + + if (is_zero(tune_p)) { + tune_p = 0.05f * max_gain_p.max_allowed; + prev_gain = gain[frq_cnt]; + } else if ((gain[frq_cnt] < prev_gain || is_zero(prev_gain)) && tune_p < 0.6f * max_gain_p.max_allowed) { + tune_p += 0.05f * max_gain_p.max_allowed; + prev_gain = gain[frq_cnt]; + } else { + counter = AUTOTUNE_SUCCESS_COUNT; + // reset curr_test_freq and frq_cnt for next test + curr_test_freq = freq[0]; + frq_cnt = 0; + prev_gain = 0.0f; + tune_p -= 0.05f * max_gain_p.max_allowed; + tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); + } + + } + + if (counter == AUTOTUNE_SUCCESS_COUNT) { + start_freq = 0.0f; //initializes next test that uses dwell test + } else { + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } + +/* float test_freq_incr = 0.5f * 3.14159f * 2.0f; if (freq_cnt < 12) { @@ -432,20 +644,224 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai freq_cnt = 0; } } - // reset determine_gain function - determine_gain(0.0f, 0.0f, curr_test_freq, gain[freq_cnt], phase[freq_cnt], dwell_complete, true); - + if (counter == AUTOTUNE_SUCCESS_COUNT) { + start_freq = 0.0f; //initializes next test that uses dwell test + } else { + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } */ } void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d) +{ + float test_freq_incr = 0.25f * 3.14159f * 2.0f; + static uint8_t prev_good_frq_cnt; + static float prev_gain; + + if (!is_equal(start_freq,stop_freq)) { + frq_cnt = 12; + if (sweep.maxgain_freq > sweep.ph180_freq) { +// freq[frq_cnt] = sweep.maxgain_freq - 0.5f * test_freq_incr; + freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; + freq_cnt_max = frq_cnt; +// method = 1; + method = 2; + } else if (!is_zero(sweep.ph180_freq)) { + freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; + // using 180 phase as max gain to start + freq_cnt_max = frq_cnt; + method = 2; + } else { + freq[frq_cnt] = 4.0f * M_PI; + method = 0; + } + curr_test_freq = freq[frq_cnt]; + } + if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { + if (frq_cnt == 0) { + tune_d = max_gain_d.max_allowed * 0.25f; + freq_cnt_max = 0; + } else if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) { + prev_good_frq_cnt = frq_cnt; + } else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) { + if (phase[frq_cnt] - 360.0f < 180.0f) { + prev_good_frq_cnt = frq_cnt; + } + } else if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) { + frq_cnt = 11; + } + frq_cnt++; + if (frq_cnt == 12) { + freq[frq_cnt] = freq[prev_good_frq_cnt]; + curr_test_freq = freq[frq_cnt]; + method = 2; + } else { + freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; + curr_test_freq = freq[frq_cnt]; + } + } else if (is_equal(start_freq,stop_freq) && method == 2) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt])); + if (is_zero(tune_d)) { + tune_d = 0.05f * max_gain_d.max_allowed; + prev_gain = gain[frq_cnt]; + } else if (phase[frq_cnt] > 180.0f) { + curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test_freq; + } else if (phase[frq_cnt] < 160.0f) { + curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test_freq; + } else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { + if ((gain[frq_cnt] < prev_gain || is_zero(prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) { + tune_d += 0.05f * max_gain_d.max_allowed; + prev_gain = gain[frq_cnt]; + } else { + counter = AUTOTUNE_SUCCESS_COUNT; + // reset curr_test_freq and frq_cnt for next test + curr_test_freq = freq[0]; + frq_cnt = 0; + prev_gain = 0.0f; + tune_d -= 0.05f * max_gain_d.max_allowed; + tune_d = constrain_float(tune_d,0.0f,0.6f * max_gain_d.max_allowed); + } + } + } else if (is_equal(start_freq,stop_freq) && method == 1) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: cnt=%f freq=%f gain=%f phase=%f", (double)(frq_cnt), (double)(curr_test_freq), (double)(gain[frq_cnt]), (double)(phase[frq_cnt])); + if (is_zero(tune_d)) { + tune_d = 0.05f * max_gain_d.max_allowed; + prev_gain = gain[frq_cnt]; + } else if ((gain[frq_cnt] < prev_gain || is_zero(prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) { + tune_d += 0.05f * max_gain_d.max_allowed; + prev_gain = gain[frq_cnt]; + } else { + counter = AUTOTUNE_SUCCESS_COUNT; + // reset curr_test_freq and frq_cnt for next test + curr_test_freq = freq[0]; + frq_cnt = 0; + prev_gain = 0.0f; + tune_d -= 0.05f * max_gain_d.max_allowed; + tune_d = constrain_float(tune_d,0.0f,0.6f * max_gain_d.max_allowed); + } + } + if (counter == AUTOTUNE_SUCCESS_COUNT) { + start_freq = 0.0f; //initializes next test that uses dwell test + } else { + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } +} + +void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt) +{ + float test_freq_incr = 0.5f * 3.14159f * 2.0f; + float gain_incr = 0.5f; + static float phase_max; + static float prev_gain; + static bool find_peak; + + if (!is_equal(start_freq,stop_freq)) { + frq_cnt = 12; + if (!is_zero(sweep.ph180_freq)) { +// freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; + freq[frq_cnt] = sweep.maxgain_freq - 0.5f * test_freq_incr; + // using 180 phase as max gain to start + freq_cnt_max = frq_cnt; + } else { + freq[frq_cnt] = 4.0f * M_PI; + } + curr_test_freq = freq[frq_cnt]; + } + if (freq_cnt < 12 && is_equal(start_freq,stop_freq)) { + if (freq_cnt == 0) { + freq_cnt_max = 0; + } else if (gain[freq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { + // exceeded max response gain already, reduce tuning gain to remain under max response gain + tune_p -= gain_incr; + // force counter to stay on frequency + freq_cnt -= 1; + } else if (gain[freq_cnt] > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) { + // exceeded max response gain at the minimum allowable tuning gain. terminate testing. + tune_p = AUTOTUNE_SP_MIN; + counter = AUTOTUNE_SUCCESS_COUNT; + AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + curr_test_freq = freq[0]; + freq_cnt = 0; + } else if (gain[freq_cnt] > gain[freq_cnt_max]) { + freq_cnt_max = freq_cnt; + phase_max = phase[freq_cnt]; + prev_gain = gain[freq_cnt]; + } else if (gain[freq_cnt] > 0.0f && gain[freq_cnt] < 0.5f) { + // must be past peak, continue on to determine angle p + freq_cnt = 11; + } + freq_cnt++; + if (freq_cnt == 12) { + freq[freq_cnt] = freq[freq_cnt_max]; + curr_test_freq = freq[freq_cnt]; + } else { + freq[freq_cnt] = freq[freq_cnt-1] + test_freq_incr; + curr_test_freq = freq[freq_cnt]; + } + } + + // once finished with sweep of frequencies, cnt = 12 is used to then tune for max response gain + if (freq_cnt >= 12 && is_equal(start_freq,stop_freq)) { + if (gain[freq_cnt] < max_resp_gain && tune_p < AUTOTUNE_SP_MAX && !find_peak) { + // keep increasing tuning gain unless phase changes or max response gain is acheived + if (phase[freq_cnt]-phase_max > 20.0f && phase[freq_cnt] < 210.0f) { + freq[freq_cnt] += 0.5 * test_freq_incr; + find_peak = true; + } else { + tune_p += gain_incr; + freq[freq_cnt] = freq[freq_cnt_max]; + if (tune_p >= AUTOTUNE_SP_MAX) { + tune_p = AUTOTUNE_SP_MAX; + counter = AUTOTUNE_SUCCESS_COUNT; + AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + curr_test_freq = freq[0]; + freq_cnt = 0; + } + } + curr_test_freq = freq[freq_cnt]; + prev_gain = gain[freq_cnt]; + } else if (gain[freq_cnt] > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN && !find_peak) { + tune_p -= gain_incr; + } else if (find_peak) { + // find the frequency where the response gain is maximum + if (gain[freq_cnt] > prev_gain) { + freq[freq_cnt] += 0.5 * test_freq_incr; + } else { + find_peak = false; + phase_max = phase[freq_cnt]; + } + curr_test_freq = freq[freq_cnt]; + prev_gain = gain[freq_cnt]; + } else { + // adjust tuning gain so max response gain is not exceeded + if (prev_gain < max_resp_gain && gain[freq_cnt] > max_resp_gain) { + float adj_factor = (max_resp_gain - gain[freq_cnt]) / (gain[freq_cnt] - prev_gain); + tune_p = tune_p + gain_incr * adj_factor; + } + counter = AUTOTUNE_SUCCESS_COUNT; + // reset curr_test_freq and freq_cnt for next test + curr_test_freq = freq[0]; + freq_cnt = 0; + } + } + if (counter == AUTOTUNE_SUCCESS_COUNT) { + start_freq = 0.0f; //initializes next test that uses dwell test + } else { + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + } +} + +void AC_AutoTune_Heli::updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt) { float test_freq_incr = 0.5f * 3.14159f * 2.0f; static uint8_t prev_good_frq_cnt; - float max_gain = 1.2f; if (frq_cnt < 12) { if (frq_cnt == 0) { - tune_d = max_gain_d.max_allowed * 0.5f; freq_cnt_max = 0; } else if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) { prev_good_frq_cnt = frq_cnt; @@ -464,164 +880,191 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; curr_test_freq = freq[frq_cnt]; } - } else { - /* if (!is_zero(phase[prev_good_frq_cnt + 1])) { - freq_cnt_max = prev_good_frq_cnt + 2; - } else { - freq_cnt_max = prev_good_frq_cnt + 1; - } - float phase_freq = (180.0f - phase[prev_good_frq_cnt]) / (phase[freq_cnt_max] - phase[prev_good_frq_cnt]); - phase_freq = freq[prev_good_frq_cnt] + phase_freq * (freq[freq_cnt_max] - freq[prev_good_frq_cnt]); */ - if (gain[frq_cnt] < max_gain && phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f && tune_d < 0.8f * max_gain_d.max_allowed) { - tune_d += 0.1f * max_gain_d.max_allowed; - } else if (gain[frq_cnt] < max_gain && phase[frq_cnt] > 180.0f) { - curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test_freq; - } else if (gain[frq_cnt] < max_gain && phase[frq_cnt] < 160.0f) { - curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test_freq; - } else if (gain[frq_cnt] >= max_gain || tune_d > 0.8f * max_gain_d.max_allowed) { - counter = AUTOTUNE_SUCCESS_COUNT; - tune_d = 0.5f * tune_d; - // reset curr_test_freq and frq_cnt for next test - curr_test_freq = freq[0]; - frq_cnt = 0; - } } - // reset determine_gain function - determine_gain(0.0f, 0.0f, curr_test_freq, gain[frq_cnt], phase[frq_cnt], dwell_complete, true); -} -void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt) -{ - float test_freq_incr = 0.5f * 3.14159f * 2.0f; - static uint8_t prev_good_frq_cnt; - float max_gain = 1.2f; - - if (frq_cnt < 12) { - if (frq_cnt == 0) { - freq_cnt_max = 0; - } else if (phase[frq_cnt] <= 180.0f && !is_zero(phase[frq_cnt])) { - prev_good_frq_cnt = frq_cnt; - } else if (frq_cnt > 1 && phase[frq_cnt] > phase[frq_cnt-1] + 360.0f && !is_zero(phase[frq_cnt])) { - if (phase[frq_cnt] - 360.0f < 180.0f) { - prev_good_frq_cnt = frq_cnt; - } - // } else if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) { - // frq_cnt = 11; - } - frq_cnt++; - if (frq_cnt == 12) { - freq[frq_cnt] = freq[prev_good_frq_cnt]; - curr_test_freq = freq[frq_cnt]; - } else { - freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; - curr_test_freq = freq[frq_cnt]; - } - } else { - if (gain[frq_cnt] < max_gain && phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { + // once finished with sweep of frequencies, cnt = 12 is used to then tune for max response gain + if (freq_cnt >= 12) { + if (gain[frq_cnt] < max_resp_gain && phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f && tune_p < AUTOTUNE_SP_MAX) { tune_p += 0.5f; - } else if (gain[frq_cnt] < max_gain && phase[frq_cnt] > 180.0f) { + if (tune_p >= AUTOTUNE_SP_MAX) { + tune_p = AUTOTUNE_SP_MAX; + counter = AUTOTUNE_SUCCESS_COUNT; + AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + curr_test_freq = freq[0]; + freq_cnt = 0; + } + } else if (gain[frq_cnt] < max_resp_gain && phase[frq_cnt] > 180.0f) { curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; freq[frq_cnt] = curr_test_freq; - } else if (gain[frq_cnt] < max_gain && phase[frq_cnt] < 160.0f) { + } else if (gain[frq_cnt] < max_resp_gain && phase[frq_cnt] < 160.0f) { curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; freq[frq_cnt] = curr_test_freq; - } else if (gain[frq_cnt] >= max_gain || tune_p > 10.0f) { + } else { counter = AUTOTUNE_SUCCESS_COUNT; // reset curr_test_freq and frq_cnt for next test curr_test_freq = freq[0]; frq_cnt = 0; } + + // guard against frequency getting too high or too low + if (curr_test_freq > 50.24f || curr_test_freq < 3.14f) { + counter = AUTOTUNE_SUCCESS_COUNT; + AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + curr_test_freq = freq[0]; + freq_cnt = 0; + } + + } + if (counter == AUTOTUNE_SUCCESS_COUNT) { + start_freq = 0.0f; //initializes next test that uses dwell test + } else { + start_freq = curr_test_freq; + stop_freq = curr_test_freq; } - // reset determine_gain function - determine_gain(0.0f, 0.0f, curr_test_freq, gain[frq_cnt], phase[frq_cnt], dwell_complete, true); } // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d) { - float test_freq_incr = 0.5f * 3.14159f * 2.0f; - static uint8_t find_max_p = 0; - static uint8_t find_max_d = 0; - if (frq_cnt < 12) { - if (frq_cnt > 1 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 200.0f && - phase[frq_cnt-1] > 90.0f && phase[frq_cnt-1] < 161.0f && - !is_zero(phase[frq_cnt]) && find_max_p == 0) { - max_gain_p.freq = linear_interpolate(freq[frq_cnt-1],freq[frq_cnt],161.0f,phase[frq_cnt-1],phase[frq_cnt]); - max_gain_p.gain = linear_interpolate(gain[frq_cnt-1],gain[frq_cnt],161.0f,phase[frq_cnt-1],phase[frq_cnt]); + float test_freq_incr = 1.0f * M_PI * 2.0f; + static bool found_max_p = false; + static bool found_max_d = false; + static bool find_middle = false; + + if (!is_equal(start_freq,stop_freq)) { + frq_cnt = 2; + if (!is_zero(sweep.ph180_freq)) { + freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; + } else { + freq[frq_cnt] = 4.0f * M_PI; + } + curr_test_freq = freq[frq_cnt]; + start_freq = curr_test_freq; + stop_freq = curr_test_freq; + + } else if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { + if (frq_cnt > 2 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 270.0f && + !find_middle && !found_max_p) { + find_middle = true; + } else if (find_middle && !found_max_p) { + if (phase[frq_cnt] > 161.0f) { + // interpolate between frq_cnt-2 and frq_cnt + max_gain_p.freq = linear_interpolate(freq[frq_cnt-2],freq[frq_cnt],161.0f,phase[frq_cnt-2],phase[frq_cnt]); + max_gain_p.gain = linear_interpolate(gain[frq_cnt-2],gain[frq_cnt],161.0f,phase[frq_cnt-2],phase[frq_cnt]); + } else { + // interpolate between frq_cnt-1 and frq_cnt + max_gain_p.freq = linear_interpolate(freq[frq_cnt],freq[frq_cnt-1],161.0f,phase[frq_cnt],phase[frq_cnt-1]); + max_gain_p.gain = linear_interpolate(gain[frq_cnt],gain[frq_cnt-1],161.0f,phase[frq_cnt],phase[frq_cnt-1]); + } max_gain_p.phase = 161.0f; max_gain_p.max_allowed = powf(10.0f,-1 * (log10f(max_gain_p.gain) * 20.0f + 2.42) / 20.0f); - find_max_p = 1; - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f ph=%f rate_d=%f", (double)(max_gain_p.freq), (double)(max_gain_p.gain), (double)(max_gain_p.phase), (double)(max_gain_p.max_allowed)); + // limit max gain allowed to be no more than 2x the max p gain limit to keep initial gains bounded + max_gain_p.max_allowed = constrain_float(max_gain_p.max_allowed, 0.0f, 2.0f * AUTOTUNE_RP_MAX); + found_max_p = true; + find_middle = false; + + if (!is_zero(sweep.ph270_freq)) { + // set freq cnt back to reinitialize process + frq_cnt = 1; // set to 1 because it will be incremented + // set frequency back at least one test_freq_incr as it will be added + freq[1] = sweep.ph270_freq - 1.5f * test_freq_incr; + } + + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f ph=%f rate_p=%f", (double)(max_gain_p.freq), (double)(max_gain_p.gain), (double)(max_gain_p.phase), (double)(max_gain_p.max_allowed)); } - if (frq_cnt > 1 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 300.0f && - phase[frq_cnt-1] > 180.0f && phase[frq_cnt-1] < 251.0f && - !is_zero(phase[frq_cnt]) && find_max_d == 0) { - max_gain_d.freq = linear_interpolate(freq[frq_cnt-1],freq[frq_cnt],251.0f,phase[frq_cnt-1],phase[frq_cnt]); - max_gain_d.gain = linear_interpolate(gain[frq_cnt-1],gain[frq_cnt],251.0f,phase[frq_cnt-1],phase[frq_cnt]); + if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f && + !find_middle && !found_max_d && found_max_p) { + find_middle = true; + } else if (find_middle && found_max_p && !found_max_d) { + if (phase[frq_cnt] > 251.0f) { + // interpolate between frq_cnt-2 and frq_cnt + max_gain_d.freq = linear_interpolate(freq[frq_cnt-2],freq[frq_cnt],251.0f,phase[frq_cnt-2],phase[frq_cnt]); + max_gain_d.gain = linear_interpolate(gain[frq_cnt-2],gain[frq_cnt],251.0f,phase[frq_cnt-2],phase[frq_cnt]); + } else { + // interpolate between frq_cnt-1 and frq_cnt + max_gain_d.freq = linear_interpolate(freq[frq_cnt],freq[frq_cnt-1],251.0f,phase[frq_cnt],phase[frq_cnt-1]); + max_gain_d.gain = linear_interpolate(gain[frq_cnt],gain[frq_cnt-1],251.0f,phase[frq_cnt],phase[frq_cnt-1]); + } max_gain_d.phase = 251.0f; max_gain_d.max_allowed = powf(10.0f,-1 * (log10f(max_gain_d.freq * max_gain_d.gain) * 20.0f + 2.42) / 20.0f); - find_max_d = 1; + // limit max gain allowed to be no more than 2x the max d gain limit to keep initial gains bounded + max_gain_d.max_allowed = constrain_float(max_gain_d.max_allowed, 0.0f, 2.0f * AUTOTUNE_RD_MAX); + found_max_d = true; + find_middle = false; gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f ph=%f rate_d=%f", (double)(max_gain_d.freq), (double)(max_gain_d.gain), (double)(max_gain_d.phase), (double)(max_gain_d.max_allowed)); } - if (frq_cnt > 1 && phase[frq_cnt] > 300.0f && !is_zero(phase[frq_cnt])) { + // stop progression in frequency. + if ((frq_cnt > 1 && phase[frq_cnt] > 330.0f && !is_zero(phase[frq_cnt])) || found_max_d) { frq_cnt = 11; } frq_cnt++; if (frq_cnt == 12) { counter = AUTOTUNE_SUCCESS_COUNT; - // reset curr_test_freq and frq_cnt for next test + // reset variables for next test curr_test_freq = freq[0]; frq_cnt = 0; - tune_p = 0.35f * max_gain_p.max_allowed; - tune_d = 0.25f * max_gain_d.max_allowed; + found_max_p = false; + found_max_d = false; + find_middle = false; +// tune_p = 0.35f * max_gain_p.max_allowed; +// tune_d = 0.25f * max_gain_d.max_allowed; + start_freq = 0.0f; //initializes next test that uses dwell test } else { - freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; + if (frq_cnt == 3 && phase[2] >= 161.0f && !found_max_p) { + // phase greater than 161 deg won't allow max p to be found + // reset freq cnt to 2 and lower dwell freq to push phase below 161 deg + frq_cnt = 2; + freq[frq_cnt] = freq[frq_cnt] - 0.5f * test_freq_incr; + } else if (frq_cnt == 3 && phase[2] >= 251.0f && !found_max_d) { + // phase greater than 161 deg won't allow max p to be found + // reset freq cnt to 2 and lower dwell freq to push phase below 161 deg + frq_cnt = 2; + freq[frq_cnt] = freq[frq_cnt] - 0.5f * test_freq_incr; + } else if (find_middle) { + freq[frq_cnt] = freq[frq_cnt-1] - 0.5f * test_freq_incr; + } else { + freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; + } curr_test_freq = freq[frq_cnt]; + start_freq = curr_test_freq; + stop_freq = curr_test_freq; } } - // reset determine_gain function - determine_gain(0.0f, 0.0f, curr_test_freq, gain[frq_cnt], phase[frq_cnt], dwell_complete, true); } void AC_AutoTune_Heli::Log_AutoTune() { - if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { switch (axis) { case ROLL: - Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp); + Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); break; case PITCH: - Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp); + Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); break; case YAW: - Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp); + Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max); break; } - } else { - switch (axis) { - case ROLL: - Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp); - break; - case PITCH: - Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp); - break; - case YAW: - Log_Write_AutoTune(axis, tune_type, test_freq[freq_cnt], test_gain[freq_cnt], test_phase[freq_cnt], tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp); - break; - } - } +// } } void AC_AutoTune_Heli::Log_AutoTuneDetails() { - Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate); + if (tune_type == SP_UP) { + Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate); + } else { + Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f); + } } -// @LoggerMessage: ATUN -// @Description: Copter/QuadPlane AutoTune -// @Vehicles: Copter, Plane +void AC_AutoTune_Heli::Log_AutoTuneSweep() +{ + Log_Write_AutoTuneSweep(curr_test_freq, curr_test_gain, curr_test_phase); +} + +// @LoggerMessage: ATNH +// @Description: Heli AutoTune +// @Vehicles: Copter // @Field: TimeUS: Time since system startup // @Field: Axis: which axis is currently being tuned // @Field: TuneStep: step in autotune process @@ -632,16 +1075,17 @@ void AC_AutoTune_Heli::Log_AutoTuneDetails() // @Field: RP: new rate gain P term // @Field: RD: new rate gain D term // @Field: SP: new angle P term +// @Field: ACC: new max accel term // Write an Autotune data packet -void AC_AutoTune_Heli::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp) +void AC_AutoTune_Heli::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel) { AP::logger().Write( - "ATUN", - "TimeUS,Axis,TuneStep,Freq,Gain,Phase,RFF,RP,RD,SP", - "s---------", - "F--000----", - "QBBfffffff", + "ATNH", + "TimeUS,Axis,TuneStep,Freq,Gain,Phase,RFF,RP,RD,SP,ACC", + "s--E-d-----", + "F--000-----", + "QBBffffffff", AP_HAL::micros64(), axis, tune_step, @@ -651,28 +1095,56 @@ void AC_AutoTune_Heli::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, floa new_gain_rff, new_gain_rp, new_gain_rd, - new_gain_sp); + new_gain_sp, + max_accel); } // Write an Autotune data packet -void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads) +void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad) { - // @LoggerMessage: ATDE - // @Description: AutoTune data packet + // @LoggerMessage: ATDH + // @Description: Heli AutoTune data packet + // @Vehicles: Copter // @Field: TimeUS: Time since system startup // @Field: Cmd: current motor command // @Field: TRate: current target angular rate // @Field: Rate: current angular rate + // @Field: TAng: current target angle + // @Field: Ang: current angle AP::logger().WriteStreaming( - "ATDE", - "TimeUS,Cmd,TRate,Rate", - "s-kk", + "ATDH", + "TimeUS,Cmd,TRate,Rate,TAng,Ang", + "s-kkdd", + "F00000", + "Qfffff", + AP_HAL::micros64(), + motor_cmd, + tgt_rate_rads*57.3, + rate_rads*57.3f, + tgt_ang_rad*57.3, + ang_rad*57.3f); +} + +// Write an Autotune data packet +void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float phase) +{ + // @LoggerMessage: ATSH + // @Description: Heli AutoTune Sweep packet + // @Vehicles: Copter + // @Field: TimeUS: Time since system startup + // @Field: freq: current frequency + // @Field: gain: current response gain + // @Field: phase: current response phase + AP::logger().WriteStreaming( + "ATSH", + "TimeUS,freq,gain,phase", + "sE-d", "F000", "Qfff", AP_HAL::micros64(), - motor_cmd, - tgt_rate_rads*57.3f, - rate_rads*57.3f); + freq, + gain, + phase); } // get intra test rate I gain for the specified axis @@ -729,3 +1201,30 @@ float AC_AutoTune_Heli::get_yaw_rate_filt_min() const return AUTOTUNE_RLPF_MIN; } +void AC_AutoTune_Heli::set_tune_sequence() +{ + uint8_t seq_cnt = 0; + + if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_VFF) { + tune_seq[seq_cnt] = RFF_UP; + seq_cnt++; + } + if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D) { + tune_seq[seq_cnt] = MAX_GAINS; + seq_cnt++; + tune_seq[seq_cnt] = RD_UP; + seq_cnt++; + tune_seq[seq_cnt] = RP_UP; + seq_cnt++; + } + if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_ANGLE_P) { + tune_seq[seq_cnt] = SP_UP; + seq_cnt++; + } + if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_MAX_GAIN && !(seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D)) { + tune_seq[seq_cnt] = MAX_GAINS; + seq_cnt++; + } + tune_seq[seq_cnt] = TUNE_COMPLETE; + +} diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index f622f2b313..3c4fdd6059 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -24,20 +24,14 @@ class AC_AutoTune_Heli : public AC_AutoTune { public: // constructor - AC_AutoTune_Heli() - { - // tune_seq[0] = 4; // RFF_UP - // tune_seq[1] = 8; // MAX_GAINS - // tune_seq[2] = 0; // RD_UP - // tune_seq[3] = 2; // RP_UP - // tune_seq[2] = 6; // SP_UP - // tune_seq[3] = 9; // tune complete - tune_seq[0] = SP_UP; - tune_seq[1] = TUNE_COMPLETE; - }; + AC_AutoTune_Heli(); + // save gained, called on disarm void save_tuning_gains() override; + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + protected: void load_test_gains() override; @@ -90,25 +84,52 @@ protected: // get minimum rate Yaw filter value float get_yaw_rate_filt_min() const override; + // reverse direction for twitch test + bool twitch_reverse_direction() override { return positive_direction; } + void Log_AutoTune() override; void Log_AutoTuneDetails() override; - void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp); - void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads); - + void Log_AutoTuneSweep() override; + void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel); + void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad); + 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; } + + // returns true if pilot is allowed to make inputs during test + bool allow_pilot_rp_input() override + { + if (!use_poshold && tune_type == SP_UP) { + return true; + } else { + return false; + } + } + // send intermittant updates to user on status of tune void do_gcs_announcements() override; + void set_tune_sequence() override; + + AP_Int8 seq_bitmask; + AP_Float min_sweep_freq; + AP_Float max_sweep_freq; + AP_Float max_resp_gain; + private: // updating_rate_ff_up - adjust FF to ensure the target is reached // FF is adjusted until rate requested is acheived void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command); - void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, float gain_incr, float max_gain); + void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p); void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d); void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt); + void updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt); // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d); + uint8_t method; //0: determine freq, 1: use max gain method, 2: use phase 180 method + }; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 6ed35d174f..ddde5fb5a8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -36,15 +36,31 @@ #include "AC_AutoTune_Multi.h" +const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = { + AP_NESTEDGROUPINFO(AC_AutoTune, 0), + + // @Param: AGGR + // @DisplayName: Autotune aggressiveness + // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term. + // @Range: 0.05 0.10 + // @User: Standard + AP_GROUPINFO("AGGR", 1, AC_AutoTune_Multi, aggressiveness, 0.1f), + + // @Param: MIN_D + // @DisplayName: AutoTune minimum D + // @Description: Defines the minimum D gain + // @Range: 0.001 0.006 + // @User: Standard + AP_GROUPINFO("MIN_D", 2, AC_AutoTune_Multi, min_d, 0.001f), + + AP_GROUPEND +}; + // constructor AC_AutoTune_Multi::AC_AutoTune_Multi() { - tune_seq[0] = RD_UP; - tune_seq[1] = RD_DOWN; - tune_seq[2] = RP_UP; - tune_seq[3] = SP_DOWN; - tune_seq[4] = SP_UP; - tune_seq[5] = TUNE_COMPLETE; + tune_seq[0] = TUNE_COMPLETE; + AP_Param::setup_object_defaults(this, var_info); } void AC_AutoTune_Multi::do_gcs_announcements() diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 9c2a5df462..6259df7ab2 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -30,6 +30,9 @@ public: // save gained, called on disarm void save_tuning_gains() override; + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + protected: // load gains void load_test_gains() override; @@ -77,6 +80,12 @@ protected: // returns true if rate P gain of zero is acceptable for this vehicle bool allow_zero_rate_p() override { return false; } + // returns true if pilot is allowed to make inputs during test + bool allow_pilot_rp_input() 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; @@ -86,8 +95,12 @@ protected: // get minimum yaw rate filter value float get_yaw_rate_filt_min() const override; + // reverse direction for twitch test + bool twitch_reverse_direction() override { return !positive_direction; } + void Log_AutoTune() override; void Log_AutoTuneDetails() override; + void Log_AutoTuneSweep() override {}; void Log_Write_AutoTune(uint8_t axis, uint8_t 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); @@ -111,4 +124,14 @@ private: // updating_angle_p_up - increase P to ensure the target is reached // 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); + + void set_tune_sequence() override { + tune_seq[0] = RD_UP; + tune_seq[1] = RD_DOWN; + tune_seq[2] = RP_UP; + tune_seq[3] = SP_DOWN; + tune_seq[4] = SP_UP; + tune_seq[5] = TUNE_COMPLETE; + } + };