From f6da3ce7641854b403c1845c1408a8122e36ab37 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Aug 2024 23:38:18 +0100 Subject: [PATCH] AC_AutoTune: add support for testing tune via aux function --- libraries/AC_AutoTune/AC_AutoTune.cpp | 63 +++++++++++++++++++++++++-- libraries/AC_AutoTune/AC_AutoTune.h | 22 ++++++++-- 2 files changed, 77 insertions(+), 8 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index a2b2f3418f..64dafeab8e 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -109,6 +109,51 @@ void AC_AutoTune::stop() // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch } +// Autotune aux function trigger +void AC_AutoTune::do_aux_function(const RC_Channel::AuxSwitchPos ch_flag) +{ + if (mode != TuneMode::SUCCESS) { + if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) { + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: must be complete to test gains"); + } + return; + } + + switch(ch_flag) { + case RC_Channel::AuxSwitchPos::LOW: + // load original gains + load_gains(GainType::GAIN_ORIGINAL); + update_gcs(AUTOTUNE_MESSAGE_TESTING_END); + break; + case RC_Channel::AuxSwitchPos::MIDDLE: + // Middle position is unused for now + break; + case RC_Channel::AuxSwitchPos::HIGH: + // Load tuned gains + load_gains(GainType::GAIN_TUNED); + update_gcs(AUTOTUNE_MESSAGE_TESTING); + break; + } + + have_pilot_testing_command = true; +} + +// Possibly save gains, called on disarm +void AC_AutoTune::disarmed(const bool in_autotune_mode) +{ + // True if pilot is testing tuned gains + const bool testing_tuned = have_pilot_testing_command && (loaded_gains == GainType::GAIN_TUNED); + + // True if in autotune mode and no pilot testing commands have been received + const bool tune_complete_no_testing = !have_pilot_testing_command && in_autotune_mode; + + if (tune_complete_no_testing || testing_tuned) { + save_tuning_gains(); + } else { + reset(); + } +} + // initialise position controller bool AC_AutoTune::init_position_controller(void) { @@ -524,6 +569,9 @@ void AC_AutoTune::control_attitude() update_gcs(AUTOTUNE_MESSAGE_SUCCESS); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SUCCESS); AP_Notify::events.autotune_complete = true; + + // Return to original gains for landing + load_gains(GainType::GAIN_ORIGINAL); } else { AP_Notify::events.autotune_next_axis = true; reset_update_gain_variables(); @@ -590,7 +638,12 @@ void AC_AutoTune::backup_gains_and_initialise() */ void AC_AutoTune::load_gains(enum GainType gain_type) { - // todo: add previous setting so gains are not loaded on each loop. + if (loaded_gains == gain_type) { + // Loaded gains are already of correct type + return; + } + loaded_gains = gain_type; + switch (gain_type) { case GAIN_ORIGINAL: load_orig_gains(); @@ -624,15 +677,17 @@ void AC_AutoTune::update_gcs(uint8_t message_id) const GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); break; case AUTOTUNE_MESSAGE_TESTING: - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Pilot Testing"); - break; case AUTOTUNE_MESSAGE_SAVED_GAINS: - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s%s", + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s gains for %s%s%s%s", + (message_id == AUTOTUNE_MESSAGE_SAVED_GAINS) ? "Saved" : "Pilot Testing", (axes_completed&AUTOTUNE_AXIS_BITMASK_ROLL)?"Roll ":"", (axes_completed&AUTOTUNE_AXIS_BITMASK_PITCH)?"Pitch ":"", (axes_completed&AUTOTUNE_AXIS_BITMASK_YAW)?"Yaw(E)":"", (axes_completed&AUTOTUNE_AXIS_BITMASK_YAW_D)?"Yaw(D)":""); break; + case AUTOTUNE_MESSAGE_TESTING_END: + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: original gains restored"); + break; } } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index b5a8f2cf25..167e4b2a52 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -25,6 +25,7 @@ #include #include #include +#include #include "AC_AutoTune_FreqResp.h" #define AUTOTUNE_AXIS_BITMASK_ROLL 1 @@ -41,6 +42,7 @@ #define AUTOTUNE_MESSAGE_FAILED 3 #define AUTOTUNE_MESSAGE_SAVED_GAINS 4 #define AUTOTUNE_MESSAGE_TESTING 5 +#define AUTOTUNE_MESSAGE_TESTING_END 6 #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 @@ -53,19 +55,27 @@ public: // main run loop virtual void run(); - // save gained, called on disarm - virtual void save_tuning_gains() = 0; + // Possibly save gains, called on disarm + void disarmed(const bool in_autotune_mode); // stop tune, reverting gains void stop(); + // Autotune aux function trigger + void do_aux_function(const RC_Channel::AuxSwitchPos ch_flag); + +protected: + + virtual void save_tuning_gains() = 0; + + // reset Autotune so that gains are not saved again and autotune can be run again. void reset() { mode = UNINITIALISED; axes_completed = 0; + have_pilot_testing_command = false; } -protected: // axis that can be tuned enum class AxisType { ROLL = 0, // roll axis is being tuned (either angle or rate) @@ -237,7 +247,7 @@ protected: GAIN_TEST = 1, GAIN_INTRA_TEST = 2, GAIN_TUNED = 3, - }; + } loaded_gains; void load_gains(enum GainType gain_type); // autotune modes (high level states) @@ -338,6 +348,10 @@ private: // time in ms of last pilot override warning uint32_t last_pilot_override_warning; + // True if we ever got a pilot testing command of tuned gains. + // If true then disarming will save if the tuned gains are currently active. + bool have_pilot_testing_command; + }; #endif // AC_AUTOTUNE_ENABLED