diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 03c21d0490..f43da4ef72 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -553,6 +553,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl } } +// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response 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; @@ -631,6 +632,7 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai } } +// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum 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; @@ -727,6 +729,7 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai } } +// updating_angle_p_up - determines maximum angle p gain for pitch and roll 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; @@ -829,6 +832,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga } } +// updating_angle_p_up_yaw - determines maximum angle p gain for yaw 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; @@ -1002,6 +1006,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase } } +// log autotune summary data void AC_AutoTune_Heli::Log_AutoTune() { switch (axis) { @@ -1018,6 +1023,7 @@ void AC_AutoTune_Heli::Log_AutoTune() // } } +// log autotune time history results for command, angular rate, and attitude void AC_AutoTune_Heli::Log_AutoTuneDetails() { if (tune_type == SP_UP) { @@ -1027,6 +1033,7 @@ void AC_AutoTune_Heli::Log_AutoTuneDetails() } } +// log autotune frequency response data void AC_AutoTune_Heli::Log_AutoTuneSweep() { Log_Write_AutoTuneSweep(curr_test_freq, curr_test_gain, curr_test_phase); @@ -1047,7 +1054,7 @@ void AC_AutoTune_Heli::Log_AutoTuneSweep() // @Field: SP: new angle P term // @Field: ACC: new max accel term -// Write an Autotune data packet +// Write an Autotune summary 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, float max_accel) { AP::logger().Write( @@ -1069,7 +1076,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, floa max_accel); } -// Write an Autotune data packet +// Write an Autotune detailed data packet 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: ATDH @@ -1095,7 +1102,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate ang_rad*57.3f); } -// Write an Autotune data packet +// Write an Autotune frequency response data packet void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float phase) { // @LoggerMessage: ATSH @@ -1171,6 +1178,7 @@ float AC_AutoTune_Heli::get_yaw_rate_filt_min() const return AUTOTUNE_RLPF_MIN; } +// set the tuning test sequence void AC_AutoTune_Heli::set_tune_sequence() { uint8_t seq_cnt = 0; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 89a91328e4..bab9d1a599 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -45,7 +45,10 @@ protected: // get tuned yaw rate d gain float get_tuned_yaw_rd() override { return tune_yaw_rd; } + // initializes test void test_init() override; + + // runs test void test_run(AxisType test_axis, const float dir_sign) override; // update gains for the rate p up tune type @@ -87,12 +90,18 @@ protected: // reverse direction for twitch test bool twitch_reverse_direction() override { return positive_direction; } + // methods to log autotune summary data void Log_AutoTune() override; - void Log_AutoTuneDetails() override; - 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); + + // methods to log autotune time history results for command, angular rate, and attitude. + void Log_AutoTuneDetails() override; void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad); + + // methods to log autotune frequency response results + void Log_AutoTuneSweep() override; void Log_Write_AutoTuneSweep(float freq, float gain, float phase); + // returns true if rate P gain of zero is acceptable for this vehicle bool allow_zero_rate_p() override { return true; } @@ -102,48 +111,79 @@ protected: // send intermittant updates to user on status of tune void do_gcs_announcements() override; + // set the tuning test sequence void set_tune_sequence() override; + // tuning sequence bitmask AP_Int8 seq_bitmask; + + // minimum sweep frequency AP_Float min_sweep_freq; + + // maximum sweep frequency AP_Float max_sweep_freq; + + // maximum response gain AP_Float max_resp_gain; 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); + + // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response 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); + + // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d); + + // updating_angle_p_up - determines maximum angle p gain for pitch and roll void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt); + + // updating_angle_p_up_yaw - determines maximum angle p gain for yaw 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 + // updating rate FF variables + // flag for completion of the initial direction for the feedforward test bool first_dir_complete; + // feedforward gain resulting from testing in the initial direction float first_dir_rff; // updating max gain variables + // flag for finding maximum p gain bool found_max_p; + // flag for finding maximum d gain bool found_max_d; + // flag for interpolating to find max response gain bool find_middle; // updating angle P up variables + // track the maximum phase float phase_max; + // previous gain float sp_prev_gain; + // flag for finding the peak of the gain response bool find_peak; // updating angle P up yaw + // counter value of previous good frequency uint8_t sp_prev_good_frq_cnt; // updating rate P up + // counter value of previous good frequency uint8_t rp_prev_good_frq_cnt; + // previous gain float rp_prev_gain; // updating rate D up + // counter value of previous good frequency uint8_t rd_prev_good_frq_cnt; + // previous gain float rd_prev_gain; };