From 6aefd1c2b7f17c8ee8583b39362749fc93eff380 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Aug 2024 22:43:35 +0100 Subject: [PATCH 1/5] RC_Channel: add autotune switch --- libraries/RC_Channel/RC_Channel.cpp | 1 + libraries/RC_Channel/RC_Channel.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 88ba857515bb4..04508c3f7b53c 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -239,6 +239,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 176:Quadplane Fwd Throttle Override enable // @Values{Copter, Rover, Plane, Blimp}: 177:Mount LRF enable // @Values{Copter}: 178:FlightMode Pause/Resume + // @Values{Copter, Plane}: 180:Test autotuned gains after tune is complete // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 97dddce75b0d8..496a3187d5433 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -119,7 +119,7 @@ class RC_Channel { ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited SPRAYER = 15, // enable/disable the crop sprayer AUTO = 16, // change to auto flight mode - AUTOTUNE = 17, // auto tune + AUTOTUNE_MODE = 17, // auto tune LAND = 18, // change to LAND flight mode GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on PARACHUTE_ENABLE = 21, // Parachute enable/disable @@ -252,6 +252,7 @@ class RC_Channel { VFWD_THR_OVERRIDE = 176, // force enabled VTOL forward throttle method MOUNT_LRF_ENABLE = 177, // mount LRF enable/disable FLIGHTMODE_PAUSE = 178, // e.g. pause movement towards waypoint + AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains // inputs from 200 will eventually used to replace RCMAP From 83e4dea8269a1b12b645a0641bcff601fdc02af7 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Aug 2024 23:36:11 +0100 Subject: [PATCH 2/5] AC_AutoTune: Multi and Heli: only load tuned gains if a axis has completed successfully --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 10 ++-- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 63 ++++++++++----------- 2 files changed, 33 insertions(+), 40 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index d1d9972d8b295..27e9406039b45 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -487,16 +487,14 @@ void AC_AutoTune_Heli::load_tuned_gains() attitude_control->set_accel_roll_max_cdss(0.0f); attitude_control->set_accel_pitch_max_cdss(0.0f); } - if (roll_enabled()) { + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) { load_gain_set(AxisType::ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate); } - if (pitch_enabled()) { + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) { load_gain_set(AxisType::PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate); } - if (yaw_enabled()) { - if (!is_zero(tune_yaw_rp)) { - load_gain_set(AxisType::YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax, orig_yaw_rate); - } + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) { + load_gain_set(AxisType::YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax, orig_yaw_rate); } } diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 7d0d6153ef65d..213fc41a2b416 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -260,43 +260,38 @@ void AC_AutoTune_Multi::load_tuned_gains() attitude_control->set_accel_roll_max_cdss(0.0); attitude_control->set_accel_pitch_max_cdss(0.0); } - if (roll_enabled()) { - if (!is_zero(tune_roll_rp)) { - attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff); - attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff); - attitude_control->get_angle_roll_p().set_kP(tune_roll_sp); - attitude_control->set_accel_roll_max_cdss(tune_roll_accel); - } + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) { + attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd); + attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff); + attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff); + attitude_control->get_angle_roll_p().set_kP(tune_roll_sp); + attitude_control->set_accel_roll_max_cdss(tune_roll_accel); } - if (pitch_enabled()) { - if (!is_zero(tune_pitch_rp)) { - attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff); - attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff); - attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp); - attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel); - } + if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) { + attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd); + attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff); + attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff); + attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp); + attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel); } - if (yaw_enabled() || yaw_d_enabled()) { - if (!is_zero(tune_yaw_rp)) { - attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - if (yaw_d_enabled()) { - attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd); - } - if (yaw_enabled()) { - attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF); - } - attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff); - attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp); - attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel); + if ((((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled()) + || ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW_D) && yaw_d_enabled())) && !is_zero(tune_yaw_rp)) { + attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + if (yaw_d_enabled()) { + attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd); } + if (yaw_enabled()) { + attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF); + } + attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff); + attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff); + attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp); + attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel); } } From 40cac3be0be03f1e7bbb7945317d740b5186b9fa Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Aug 2024 23:37:09 +0100 Subject: [PATCH 3/5] Copter: autotune: allow access to autotune object and call through for disarm and aux function --- ArduCopter/AP_Arming.cpp | 8 ++------ ArduCopter/RC_Channel.cpp | 8 ++++++-- ArduCopter/mode.h | 8 +------- ArduCopter/mode_autotune.cpp | 10 ---------- 4 files changed, 9 insertions(+), 25 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 119971f059c31..eecef0394c166 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -809,12 +809,8 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che } #if AUTOTUNE_ENABLED == ENABLED - // save auto tuned parameters - if (copter.flightmode == &copter.mode_autotune) { - copter.mode_autotune.save_tuning_gains(); - } else { - copter.mode_autotune.reset(); - } + // Possibly save auto tuned parameters + copter.mode_autotune.autotune.disarmed(copter.flightmode == &copter.mode_autotune); #endif // we are not in the air diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index e0cf6905d3710..0ae33562a80df 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -76,7 +76,8 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi // the following functions do not need to be initialised: case AUX_FUNC::ALTHOLD: case AUX_FUNC::AUTO: - case AUX_FUNC::AUTOTUNE: + case AUX_FUNC::AUTOTUNE_MODE: + case AUX_FUNC::AUTOTUNE_TEST_GAINS: case AUX_FUNC::BRAKE: case AUX_FUNC::CIRCLE: case AUX_FUNC::DRIFT: @@ -291,9 +292,12 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc #endif #if AUTOTUNE_ENABLED == ENABLED - case AUX_FUNC::AUTOTUNE: + case AUX_FUNC::AUTOTUNE_MODE: do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag); break; + case AUX_FUNC::AUTOTUNE_TEST_GAINS: + copter.mode_autotune.autotune.do_aux_function(ch_flag); + break; #endif case AUX_FUNC::LAND: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 11b2a64410c4c..0c8bc6b058962 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -796,18 +796,12 @@ class ModeAutoTune : public Mode { bool allows_arming(AP_Arming::Method method) const override { return false; } bool is_autopilot() const override { return false; } - void save_tuning_gains(); - void reset(); + AutoTune autotune; protected: const char *name() const override { return "AUTOTUNE"; } const char *name4() const override { return "ATUN"; } - -private: - - AutoTune autotune; - }; #endif diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index d7d0d2e470095..04a2b88bda8ef 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -118,19 +118,9 @@ void ModeAutoTune::run() autotune.run(); } -void ModeAutoTune::save_tuning_gains() -{ - autotune.save_tuning_gains(); -} - void ModeAutoTune::exit() { autotune.stop(); } -void ModeAutoTune::reset() -{ - autotune.reset(); -} - #endif // AUTOTUNE_ENABLED == ENABLED From 18c5e1b08f7b82e91e56b0e1894b8808d9abc3ad Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Aug 2024 23:38:18 +0100 Subject: [PATCH 4/5] 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 a2b2f3418f9cf..64dafeab8e671 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 b5a8f2cf253f9..167e4b2a5288d 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 @@ class AC_AutoTune // 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 @@ class AC_AutoTune 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 @@ class AC_AutoTune // 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 From caaf27ec9ce3d89ee1e9c450145a346278ab4dba Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 5 Aug 2024 11:54:15 +0100 Subject: [PATCH 5/5] Plane: Qautotune: use new disarmed call and add new aux function --- ArduPlane/AP_Arming.cpp | 8 ++------ ArduPlane/RC_Channel.cpp | 9 +++++++++ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index a774b778f5f59..643f93a6c17cc 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -370,12 +370,8 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec change_arm_state(); #if QAUTOTUNE_ENABLED - //save qautotune gains if enabled and success - if (plane.control_mode == &plane.mode_qautotune) { - plane.quadplane.qautotune.save_tuning_gains(); - } else { - plane.quadplane.qautotune.reset(); - } + // Possibly save auto tuned parameters + plane.quadplane.qautotune.disarmed(plane.control_mode == &plane.mode_qautotune); #endif // re-initialize speed variable used in AUTO and GUIDED for diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 573b7ae0411c7..424c861c7ffc9 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -169,6 +169,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, case AUX_FUNC::FW_AUTOTUNE: case AUX_FUNC::VFWD_THR_OVERRIDE: case AUX_FUNC::PRECISION_LOITER: +#if QAUTOTUNE_ENABLED + case AUX_FUNC::AUTOTUNE_TEST_GAINS: +#endif break; case AUX_FUNC::SOARING: @@ -441,6 +444,12 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch // handled by lua scripting, just ignore here break; +#if QAUTOTUNE_ENABLED + case AUX_FUNC::AUTOTUNE_TEST_GAINS: + plane.quadplane.qautotune.do_aux_function(ch_flag); + break; +#endif + default: return RC_Channel::do_aux_function(ch_option, ch_flag); }