From 2ccf8af2bde9a3aa1f3d077bf196ba2540f1022f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 4 Feb 2022 23:29:54 +0000 Subject: [PATCH] AC_Autotune_Multi: print gains on axis completion --- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 24 +++++++++++++++++++++ libraries/AC_AutoTune/AC_AutoTune_Multi.h | 6 ++++++ 2 files changed, 30 insertions(+) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 2e3a4d3117..dce6ca01d5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -440,6 +440,30 @@ void AC_AutoTune_Multi::save_tuning_gains() reset(); } +// report final gains for a given axis to GCS +void AC_AutoTune_Multi::report_final_gains(AxisType test_axis) const +{ + switch (test_axis) { + case ROLL: + report_axis_gains("Roll", tune_roll_rp, tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL, tune_roll_rd, tune_roll_sp, tune_roll_accel); + break; + case PITCH: + report_axis_gains("Pitch", tune_pitch_rp, tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel); + break; + case YAW: + report_axis_gains("Yaw", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, 0, tune_yaw_sp, tune_yaw_accel); + break; + } +} + +// report gain formating helper +void AC_AutoTune_Multi::report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float angle_P, float max_accel) const +{ + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string); + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.3f, I:%0.3f, D:%0.4f",axis_string,rate_P,rate_I,rate_D); + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.3f, Max Accel:%0.0f",axis_string,angle_P,max_accel); +} + // twitching_test_rate - twitching tests // update min and max and test for end conditions void AC_AutoTune_Multi::twitching_test_rate(float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 6795524004..5374a432f8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -68,6 +68,9 @@ protected: // send post test updates to user void do_post_test_gcs_announcements() override {}; + // report final gains for a given axis to GCS + void report_final_gains(AxisType test_axis) const override; + // update gains for the rate P up tune type void updating_rate_p_up_all(AxisType test_axis) override; @@ -157,6 +160,9 @@ private: // P is increased until we achieve our target within a reasonable time void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); + // report gain formating helper + void report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float angle_P, float max_accel) const; + // parameters AP_Int8 axis_bitmask; // axes to be tuned AP_Float aggressiveness; // aircraft response aggressiveness to be tuned