Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Autotune: Add switch to test gains after tune is complete #27754

Merged
merged 5 commits into from
Aug 12, 2024
Merged

Autotune: Add switch to test gains after tune is complete #27754

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6aefd1c
RC_Channel: add autotune switch
IamPete1 Aug 4, 2024
83e4dea
AC_AutoTune: Multi and Heli: only load tuned gains if a axis has comp…
IamPete1 Aug 4, 2024
40cac3b
Copter: autotune: allow access to autotune object and call through fo…
IamPete1 Aug 4, 2024
18c5e1b
AC_AutoTune: add support for testing tune via aux function
IamPete1 Aug 4, 2024
caaf27e
Plane: Qautotune: use new disarmed call and add new aux function
IamPete1 Aug 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 2 additions & 6 deletions ArduCopter/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 6 additions & 2 deletions ArduCopter/RC_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down
8 changes: 1 addition & 7 deletions ArduCopter/mode.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
10 changes: 0 additions & 10 deletions ArduCopter/mode_autotune.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
8 changes: 2 additions & 6 deletions ArduPlane/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions ArduPlane/RC_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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);
}
Expand Down
63 changes: 59 additions & 4 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is where we could change to mode AutoTune if the tune has not been completed.

********* For Discussion *********

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();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do you intend to save_tuning_gains() if switch is in lower position so we currently have the old gains loaded?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you have used the switch to test the gains and you have the switch in the "original gains" position then it should not save the tuned gains.

If you have not used the switch to test the gains and are in AutoTune mode then it will save the gains. The is should be the same as the current behaviour.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sounds confusing.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm open to better suggestions but I could not think of anything else while preserving the current behaviour, we can't make it a requirement that everyone must setup the new switch to run autotune.

Now we can send aux functions from the gcs we can't really check if the user has setup a switch until they first trigger it.

Copy link
Contributor

@timtuxworth timtuxworth Aug 7, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How about reverse the switch?

  • low = new gains (test/save on disarm),
  • high = original gains (discard)

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure how that is less confusing, the pattern (there are one or two exceptions) for functions is that high is to take some action. In this case the action is the thing we have added namely the ability to test gains in any mode. Original gains and discarding in other modes is the existing behavior, it would be abit odd to require the user to flip a switch to high for that.

Don't forget that aux functions are edge triggered, it does not matter where the switch is until autotune has completed, only changes after that (and before you disarm) will do anything.

} else {
reset();
}
}

// initialise position controller
bool AC_AutoTune::init_position_controller(void)
{
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}
}

Expand Down
22 changes: 18 additions & 4 deletions libraries/AC_AutoTune/AC_AutoTune.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <AC_AttitudeControl/AC_AttitudeControl.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include "AC_AutoTune_FreqResp.h"

#define AUTOTUNE_AXIS_BITMASK_ROLL 1
Expand All @@ -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

Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
10 changes: 4 additions & 6 deletions libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down
63 changes: 29 additions & 34 deletions libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down
1 change: 1 addition & 0 deletions libraries/RC_Channel/RC_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion libraries/RC_Channel/RC_Channel.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
Loading