From 4ae2be55aaa01febb1303eeeec069f5845cacd3e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Dec 2017 16:39:18 +1100 Subject: [PATCH] Copter: fix up autotune namespacing This moves static variables into the autotune flightmode object. It also adjusts namespacing on everything to take advantage of having everything encapsulated in the AutoTune object --- ArduCopter/Copter.h | 10 - ArduCopter/FlightMode.h | 148 +++-- ArduCopter/Log.cpp | 4 +- ArduCopter/control_autotune.cpp | 918 +++++++++++++++----------------- ArduCopter/flight_mode.cpp | 2 +- ArduCopter/motors.cpp | 2 +- 6 files changed, 540 insertions(+), 544 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 74e71925f3..c31adf890c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -156,16 +156,6 @@ public: void setup() override; void loop() override; - enum AUTOTUNE_LEVEL_ISSUE { - AUTOTUNE_LEVEL_ISSUE_NONE, - AUTOTUNE_LEVEL_ISSUE_ANGLE_ROLL, - AUTOTUNE_LEVEL_ISSUE_ANGLE_PITCH, - AUTOTUNE_LEVEL_ISSUE_ANGLE_YAW, - AUTOTUNE_LEVEL_ISSUE_RATE_ROLL, - AUTOTUNE_LEVEL_ISSUE_RATE_PITCH, - AUTOTUNE_LEVEL_ISSUE_RATE_YAW, - }; - private: static const AP_FWVersion fwver; diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 2a1bf04677..c57e3e50b6 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -691,12 +691,9 @@ public: bool allows_arming(bool from_gcs) const override { return false; }; bool is_autopilot() const override { return false; } - float get_autotune_descent_speed(); - bool autotuneing_with_GPS(); - void do_not_use_GPS(); + void save_tuning_gains(); - void autotune_stop(); - void autotune_save_tuning_gains(); + void stop(); protected: @@ -705,36 +702,127 @@ protected: private: - bool autotune_start(bool ignore_checks); + bool start(bool ignore_checks); + void autotune_attitude_control(); - void autotune_backup_gains_and_initialise(); - void autotune_load_orig_gains(); - void autotune_load_tuned_gains(); - void autotune_load_intra_test_gains(); - void autotune_load_twitch_gains(); - void autotune_update_gcs(uint8_t message_id); - bool autotune_roll_enabled(); - bool autotune_pitch_enabled(); - bool autotune_yaw_enabled(); - void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max); - void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); - void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); - void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max); - void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max); - void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); - void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); - void autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); + void backup_gains_and_initialise(); + void load_orig_gains(); + void load_tuned_gains(); + void load_intra_test_gains(); + void load_twitch_gains(); + void update_gcs(uint8_t message_id); + bool roll_enabled(); + bool pitch_enabled(); + bool yaw_enabled(); + void twitching_test(float measurement, float target, float &measurement_min, float &measurement_max); + void updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max); + void updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max); + void updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); + void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd); void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); - void autotune_send_step_string(); - const char *autotune_level_issue_string() const; - const char * autotune_type_string() const; - void autotune_announce_state_to_gcs(); - void autotune_do_gcs_announcements(); - bool autotune_check_level(const enum AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const; - bool autotune_currently_level(); + void send_step_string(); + const char *level_issue_string() const; + const char * type_string() const; + void announce_state_to_gcs(); + void do_gcs_announcements(); + + enum LEVEL_ISSUE { + LEVEL_ISSUE_NONE, + LEVEL_ISSUE_ANGLE_ROLL, + LEVEL_ISSUE_ANGLE_PITCH, + LEVEL_ISSUE_ANGLE_YAW, + LEVEL_ISSUE_RATE_ROLL, + LEVEL_ISSUE_RATE_PITCH, + LEVEL_ISSUE_RATE_YAW, + }; + bool check_level(const enum LEVEL_ISSUE issue, const float current, const float maximum); + bool currently_level(); + + // autotune modes (high level states) + enum TuneMode { + UNINITIALISED = 0, // autotune has never been run + TUNING = 1, // autotune is testing gains + SUCCESS = 2, // tuning has completed, user is flight testing the new gains + FAILED = 3, // tuning has failed, user is flying on original gains + }; + + // steps performed while in the tuning mode + enum StepType { + WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch + TWITCHING = 1, // autotune has begun a twitch and is watching the resulting vehicle movement + UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results + }; + + // things that can be tuned + enum AxisType { + ROLL = 0, // roll axis is being tuned (either angle or rate) + PITCH = 1, // pitch axis is being tuned (either angle or rate) + YAW = 2, // pitch axis is being tuned (either angle or rate) + }; + + // mini steps performed while in Tuning mode, Testing step + enum TuneType { + RD_UP = 0, // rate D is being tuned up + RD_DOWN = 1, // rate D is being tuned down + RP_UP = 2, // rate P is being tuned up + SP_DOWN = 3, // angle P is being tuned down + SP_UP = 4 // angle P is being tuned up + }; + + TuneMode mode : 2; // see TuneMode for what modes are allowed + bool pilot_override : 1; // true = pilot is overriding controls so we suspend tuning temporarily + AxisType axis : 2; // see AxisType for which things can be tuned + bool positive_direction : 1; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) + StepType step : 2; // see StepType for what steps are performed + TuneType tune_type : 3; // see TuneType + bool ignore_next : 1; // true = ignore the next test + bool twitch_first_iter : 1; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) + bool use_poshold : 1; // true = enable position hold + bool have_position : 1; // true = start_position is value + Vector3f start_position; + +// variables + uint32_t override_time; // the last time the pilot overrode the controls + float test_min; // the minimum angular rate achieved during TESTING_RATE step + float test_max; // the maximum angular rate achieved during TESTING_RATE step + uint32_t step_start_time; // start time of current tuning step (used for timeout checks) + uint32_t step_stop_time; // start time of current tuning step (used for timeout checks) + int8_t counter; // counter for tuning gains + float target_rate, start_rate; // target and start rate + float target_angle, start_angle; // target and start angles + float desired_yaw; // yaw heading during tune + float rate_max, test_accel_max; // maximum acceleration variables + + LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second + +// backup of currently being tuned parameter values + float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel; + float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel; + float orig_yaw_rp = 0, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel; + bool orig_bf_feedforward; + +// currently being tuned parameter values + float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; + float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; + float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; + + uint32_t announce_time; + float lean_angle; + float rotation_rate; + float roll_cd, pitch_cd; + + struct { + LEVEL_ISSUE issue{LEVEL_ISSUE_NONE}; + float maximum; + float current; + } level_problem; + }; #endif diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 3059596390..abcb9fda84 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -21,12 +21,12 @@ struct PACKED log_AutoTune { }; // Write an Autotune data packet -void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) +void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) { struct log_AutoTune pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG), time_us : AP_HAL::micros64(), - axis : axis, + axis : _axis, tune_step : tune_step, meas_target : meas_target, meas_min : meas_min, diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 7ce2540051..939a59135b 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -92,159 +92,77 @@ #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 -// autotune modes (high level states) -enum AutoTuneTuneMode { - AUTOTUNE_MODE_UNINITIALISED = 0, // autotune has never been run - AUTOTUNE_MODE_TUNING = 1, // autotune is testing gains - AUTOTUNE_MODE_SUCCESS = 2, // tuning has completed, user is flight testing the new gains - AUTOTUNE_MODE_FAILED = 3, // tuning has failed, user is flying on original gains -}; - -// steps performed while in the tuning mode -enum AutoTuneStepType { - AUTOTUNE_STEP_WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch - AUTOTUNE_STEP_TWITCHING = 1, // autotune has begun a twitch and is watching the resulting vehicle movement - AUTOTUNE_STEP_UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results -}; - -// things that can be tuned -enum AutoTuneAxisType { - AUTOTUNE_AXIS_ROLL = 0, // roll axis is being tuned (either angle or rate) - AUTOTUNE_AXIS_PITCH = 1, // pitch axis is being tuned (either angle or rate) - AUTOTUNE_AXIS_YAW = 2, // pitch axis is being tuned (either angle or rate) -}; - -// mini steps performed while in Tuning mode, Testing step -enum AutoTuneTuneType { - AUTOTUNE_TYPE_RD_UP = 0, // rate D is being tuned up - AUTOTUNE_TYPE_RD_DOWN = 1, // rate D is being tuned down - AUTOTUNE_TYPE_RP_UP = 2, // rate P is being tuned up - AUTOTUNE_TYPE_SP_DOWN = 3, // angle P is being tuned down - AUTOTUNE_TYPE_SP_UP = 4 // angle P is being tuned up -}; - -// autotune_state_struct - hold state flags -static struct autotune_state_struct { - AutoTuneTuneMode mode : 2; // see AutoTuneTuneMode for what modes are allowed - uint8_t pilot_override : 1; // true = pilot is overriding controls so we suspend tuning temporarily - AutoTuneAxisType axis : 2; // see AutoTuneAxisType for which things can be tuned - uint8_t positive_direction : 1; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) - AutoTuneStepType step : 2; // see AutoTuneStepType for what steps are performed - AutoTuneTuneType tune_type : 3; // see AutoTuneTuneType - uint8_t ignore_next : 1; // true = ignore the next test - uint8_t twitch_first_iter : 1; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) - bool use_poshold : 1; // true = enable position hold - bool have_position : 1; // true = start_position is value - Vector3f start_position; -} autotune_state; - -// variables -static uint32_t autotune_override_time; // the last time the pilot overrode the controls -static float autotune_test_min; // the minimum angular rate achieved during TESTING_RATE step -static float autotune_test_max; // the maximum angular rate achieved during TESTING_RATE step -static uint32_t autotune_step_start_time; // start time of current tuning step (used for timeout checks) -static uint32_t autotune_step_stop_time; // start time of current tuning step (used for timeout checks) -static int8_t autotune_counter; // counter for tuning gains -static float autotune_target_rate, autotune_start_rate; // target and start rate -static float autotune_target_angle, autotune_start_angle; // target and start angles -static float autotune_desired_yaw; // yaw heading during tune -static float rate_max, autotune_test_accel_max; // maximum acceleration variables - -LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second - -// backup of currently being tuned parameter values -static float orig_roll_rp = 0, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel; -static float orig_pitch_rp = 0, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel; -static float orig_yaw_rp = 0, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel; -static bool orig_bf_feedforward; - -// currently being tuned parameter values -static float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; -static float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; -static float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; - -static uint32_t autotune_announce_time; -static float lean_angle; -static float rotation_rate; -static float autotune_roll_cd, autotune_pitch_cd; - -static struct { - Copter::AUTOTUNE_LEVEL_ISSUE issue{Copter::AUTOTUNE_LEVEL_ISSUE_NONE}; - float maximum; - float current; -} autotune_level_problem; - // autotune_init - should be called when autotune mode is selected bool Copter::FlightMode_AUTOTUNE::init(bool ignore_checks) { bool success = true; - switch (autotune_state.mode) { - case AUTOTUNE_MODE_FAILED: + switch (mode) { + case FAILED: // autotune has been run but failed so reset state to uninitialized - autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED; + mode = UNINITIALISED; // fall through to restart the tuning FALLTHROUGH; - case AUTOTUNE_MODE_UNINITIALISED: + case UNINITIALISED: // autotune has never been run - success = autotune_start(false); + success = start(false); if (success) { // so store current gains as original gains - autotune_backup_gains_and_initialise(); + backup_gains_and_initialise(); // advance mode to tuning - autotune_state.mode = AUTOTUNE_MODE_TUNING; + mode = TUNING; // send message to ground station that we've started tuning - autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); + update_gcs(AUTOTUNE_MESSAGE_STARTED); } break; - case AUTOTUNE_MODE_TUNING: + case TUNING: // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off - success = autotune_start(false); + success = start(false); if (success) { // reset gains to tuning-start gains (i.e. low I term) - autotune_load_intra_test_gains(); + load_intra_test_gains(); // write dataflash log even and send message to ground station Log_Write_Event(DATA_AUTOTUNE_RESTART); - autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); + update_gcs(AUTOTUNE_MESSAGE_STARTED); } break; - case AUTOTUNE_MODE_SUCCESS: + case SUCCESS: // we have completed a tune and the pilot wishes to test the new gains in the current flight mode // so simply apply tuning gains (i.e. do not change flight mode) - autotune_load_tuned_gains(); + load_tuned_gains(); Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING); break; } // only do position hold if starting autotune from LOITER or POSHOLD - autotune_state.use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD); - autotune_state.have_position = false; + use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD); + have_position = false; return success; } -// autotune_stop - should be called when the ch7/ch8 switch is switched OFF -void Copter::FlightMode_AUTOTUNE::autotune_stop() +// stop - should be called when the ch7/ch8 switch is switched OFF +void Copter::FlightMode_AUTOTUNE::stop() { // set gains to their original values - autotune_load_orig_gains(); + load_orig_gains(); // re-enable angle-to-rate request limits attitude_control->use_ff_and_input_shaping(true); // log off event and send message to ground station - autotune_update_gcs(AUTOTUNE_MESSAGE_STOPPED); + update_gcs(AUTOTUNE_MESSAGE_STOPPED); Log_Write_Event(DATA_AUTOTUNE_OFF); - // Note: we leave the autotune_state.mode as it was so that we know how the autotune ended + // Note: we leave the mode as it was so that we know how the autotune ended // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch } -// autotune_start - Initialize autotune flight mode -bool Copter::FlightMode_AUTOTUNE::autotune_start(bool ignore_checks) +// start - Initialize autotune flight mode +bool Copter::FlightMode_AUTOTUNE::start(bool ignore_checks) { // only allow flip from Stabilize, AltHold, PosHold or Loiter modes if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD && @@ -275,124 +193,124 @@ bool Copter::FlightMode_AUTOTUNE::autotune_start(bool ignore_checks) return true; } -const char *Copter::FlightMode_AUTOTUNE::autotune_level_issue_string() const +const char *Copter::FlightMode_AUTOTUNE::level_issue_string() const { - switch (autotune_level_problem.issue) { - case Copter::AUTOTUNE_LEVEL_ISSUE_NONE: + switch (level_problem.issue) { + case LEVEL_ISSUE_NONE: return "None"; - case Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_ROLL: + case LEVEL_ISSUE_ANGLE_ROLL: return "Angle(R)"; - case Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_PITCH: + case LEVEL_ISSUE_ANGLE_PITCH: return "Angle(P)"; - case Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_YAW: + case LEVEL_ISSUE_ANGLE_YAW: return "Angle(Y)"; - case Copter::AUTOTUNE_LEVEL_ISSUE_RATE_ROLL: + case LEVEL_ISSUE_RATE_ROLL: return "Rate(R)"; - case Copter::AUTOTUNE_LEVEL_ISSUE_RATE_PITCH: + case LEVEL_ISSUE_RATE_PITCH: return "Rate(P)"; - case Copter::AUTOTUNE_LEVEL_ISSUE_RATE_YAW: + case LEVEL_ISSUE_RATE_YAW: return "Rate(Y)"; } return "Bug"; } -void Copter::FlightMode_AUTOTUNE::autotune_send_step_string() +void Copter::FlightMode_AUTOTUNE::send_step_string() { - if (autotune_state.pilot_override) { + if (pilot_override) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active"); return; } - switch (autotune_state.step) { - case AUTOTUNE_STEP_WAITING_FOR_LEVEL: - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: WFL (%s) (%f > %f)", autotune_level_issue_string(), (double)(autotune_level_problem.current*0.01f), (double)(autotune_level_problem.maximum*0.01f)); + switch (step) { + case WAITING_FOR_LEVEL: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: WFL (%s) (%f > %f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f)); return; - case AUTOTUNE_STEP_UPDATE_GAINS: + case UPDATE_GAINS: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: UPDATING_GAINS"); return; - case AUTOTUNE_STEP_TWITCHING: + case TWITCHING: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: TWITCHING"); return; } gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step"); } -const char *Copter::FlightMode_AUTOTUNE::autotune_type_string() const +const char *Copter::FlightMode_AUTOTUNE::type_string() const { - switch (autotune_state.tune_type) { - case AUTOTUNE_TYPE_RD_UP: + switch (tune_type) { + case RD_UP: return "Rate D Up"; - case AUTOTUNE_TYPE_RD_DOWN: + case RD_DOWN: return "Rate D Down"; - case AUTOTUNE_TYPE_RP_UP: + case RP_UP: return "Rate P Up"; - case AUTOTUNE_TYPE_SP_DOWN: + case SP_DOWN: return "Angle P Down"; - case AUTOTUNE_TYPE_SP_UP: + case SP_UP: return "Angle P Up"; } return "Bug"; } -void Copter::FlightMode_AUTOTUNE::autotune_do_gcs_announcements() +void Copter::FlightMode_AUTOTUNE::do_gcs_announcements() { const uint32_t now = millis(); - if (now - autotune_announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { + if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { return; } float tune_rp = 0.0f; float tune_rd = 0.0f; float tune_sp = 0.0f; float tune_accel = 0.0f; - char axis = '?'; - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + char axis_char = '?'; + switch (axis) { + case ROLL: tune_rp = tune_roll_rp; tune_rd = tune_roll_rd; tune_sp = tune_roll_sp; tune_accel = tune_roll_accel; - axis = 'R'; + axis_char = 'R'; break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: tune_rp = tune_pitch_rp; tune_rd = tune_pitch_rd; tune_sp = tune_pitch_sp; tune_accel = tune_pitch_accel; - axis = 'P'; + axis_char = 'P'; break; - case AUTOTUNE_AXIS_YAW: + case YAW: tune_rp = tune_yaw_rp; tune_rd = tune_yaw_rLPF; tune_sp = tune_yaw_sp; tune_accel = tune_yaw_accel; - axis = 'Y'; + axis_char = 'Y'; break; } - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis, autotune_type_string()); - autotune_send_step_string(); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis_char, type_string()); + send_step_string(); if (!is_zero(lean_angle)) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", (double)lean_angle, (double)autotune_target_angle); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", (double)lean_angle, (double)target_angle); } if (!is_zero(rotation_rate)) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", (double)(rotation_rate*0.01f), (double)(autotune_target_rate*0.01f)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", (double)(rotation_rate*0.01f), (double)(target_rate*0.01f)); } - switch (autotune_state.tune_type) { - case AUTOTUNE_TYPE_RD_UP: - case AUTOTUNE_TYPE_RD_DOWN: - case AUTOTUNE_TYPE_RP_UP: + switch (tune_type) { + case RD_UP: + case RD_DOWN: + case RP_UP: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd); break; - case AUTOTUNE_TYPE_SP_DOWN: - case AUTOTUNE_TYPE_SP_UP: + case SP_DOWN: + case SP_UP: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel); break; } - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", autotune_counter, AUTOTUNE_SUCCESS_COUNT); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT); - autotune_announce_time = now; + announce_time = now; } -// autotune_run - runs the autotune flight mode +// run - runs the autotune flight mode // should be called at 100hz or more void Copter::FlightMode_AUTOTUNE::run() { @@ -401,14 +319,14 @@ void Copter::FlightMode_AUTOTUNE::run() int16_t target_climb_rate; // tell the user what's going on - autotune_do_gcs_announcements(); + do_gcs_announcements(); // initialize vertical speeds and acceleration pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - // this should not actually be possible because of the autotune_init() checks + // this should not actually be possible because of the init() checks if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); @@ -431,7 +349,7 @@ void Copter::FlightMode_AUTOTUNE::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // check for pilot requested take-off - this should not actually be possible because of autotune_init() checks + // check for pilot requested take-off - this should not actually be possible because of init() checks if (ap.land_complete && target_climb_rate > 0) { // indicate we are taking off set_land_complete(false); @@ -456,39 +374,39 @@ void Copter::FlightMode_AUTOTUNE::run() // check if pilot is overriding the controls bool zero_rp_input = is_zero(target_roll) && is_zero(target_pitch); if (!zero_rp_input || !is_zero(target_yaw_rate) || target_climb_rate != 0) { - if (!autotune_state.pilot_override) { - autotune_state.pilot_override = true; + if (!pilot_override) { + pilot_override = true; // set gains to their original values - autotune_load_orig_gains(); + load_orig_gains(); attitude_control->use_ff_and_input_shaping(true); } // reset pilot override time - autotune_override_time = millis(); + override_time = millis(); if (!zero_rp_input) { // only reset position on roll or pitch input - autotune_state.have_position = false; + have_position = false; } - }else if (autotune_state.pilot_override) { + }else if (pilot_override) { // check if we should resume tuning after pilot's override - if (millis() - autotune_override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { - autotune_state.pilot_override = false; // turn off pilot override + if (millis() - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { + pilot_override = false; // turn off pilot override // set gains to their intra-test values (which are very close to the original gains) - // autotune_load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly - autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; // set tuning step back from beginning - autotune_desired_yaw = ahrs.yaw_sensor; + // load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly + step = WAITING_FOR_LEVEL; // set tuning step back from beginning + desired_yaw = ahrs.yaw_sensor; } } if (zero_rp_input) { // pilot input on throttle and yaw will still use position hold if enabled - autotune_get_poshold_attitude(target_roll, target_pitch, autotune_desired_yaw); + get_poshold_attitude(target_roll, target_pitch, desired_yaw); } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // if pilot override call attitude controller - if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) { + if (pilot_override || mode != TUNING) { attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); }else{ // somehow get attitude requests from autotuning @@ -501,128 +419,128 @@ void Copter::FlightMode_AUTOTUNE::run() } } -bool Copter::FlightMode_AUTOTUNE::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const +bool Copter::FlightMode_AUTOTUNE::check_level(const LEVEL_ISSUE issue, const float current, const float maximum) { if (current > maximum) { - autotune_level_problem.current = current; - autotune_level_problem.maximum = maximum; - autotune_level_problem.issue = issue; + level_problem.current = current; + level_problem.maximum = maximum; + level_problem.issue = issue; return false; } return true; } -bool Copter::FlightMode_AUTOTUNE::autotune_currently_level() +bool Copter::FlightMode_AUTOTUNE::currently_level() { - if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_ROLL, - labs(ahrs.roll_sensor - autotune_roll_cd), - AUTOTUNE_LEVEL_ANGLE_CD)) { + if (!check_level(LEVEL_ISSUE_ANGLE_ROLL, + labs(ahrs.roll_sensor - roll_cd), + AUTOTUNE_LEVEL_ANGLE_CD)) { return false; } - if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_PITCH, - labs(ahrs.pitch_sensor - autotune_pitch_cd), - AUTOTUNE_LEVEL_ANGLE_CD)) { + if (!check_level(LEVEL_ISSUE_ANGLE_PITCH, + labs(ahrs.pitch_sensor - pitch_cd), + AUTOTUNE_LEVEL_ANGLE_CD)) { return false; } - if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_YAW, - labs(wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_desired_yaw)), - AUTOTUNE_LEVEL_ANGLE_CD)) { + if (!check_level(LEVEL_ISSUE_ANGLE_YAW, + labs(wrap_180_cd(ahrs.yaw_sensor-(int32_t)desired_yaw)), + AUTOTUNE_LEVEL_ANGLE_CD)) { return false; } - if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_RATE_ROLL, - (ToDeg(ahrs.get_gyro().x) * 100.0f), - AUTOTUNE_LEVEL_RATE_RP_CD)) { + if (!check_level(LEVEL_ISSUE_RATE_ROLL, + (ToDeg(ahrs.get_gyro().x) * 100.0f), + AUTOTUNE_LEVEL_RATE_RP_CD)) { return false; } - if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_RATE_PITCH, - (ToDeg(ahrs.get_gyro().y) * 100.0f), - AUTOTUNE_LEVEL_RATE_RP_CD)) { + if (!check_level(LEVEL_ISSUE_RATE_PITCH, + (ToDeg(ahrs.get_gyro().y) * 100.0f), + AUTOTUNE_LEVEL_RATE_RP_CD)) { return false; } - if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_RATE_YAW, - (ToDeg(ahrs.get_gyro().z) * 100.0f), - AUTOTUNE_LEVEL_RATE_Y_CD)) { + if (!check_level(LEVEL_ISSUE_RATE_YAW, + (ToDeg(ahrs.get_gyro().z) * 100.0f), + AUTOTUNE_LEVEL_RATE_Y_CD)) { return false; } return true; } -// autotune_attitude_controller - sets attitude control targets during tuning +// attitude_controller - sets attitude control targets during tuning void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() { rotation_rate = 0.0f; // rotation rate in radians/second lean_angle = 0.0f; - const float direction_sign = autotune_state.positive_direction ? 1.0f : -1.0f; + const float direction_sign = positive_direction ? 1.0f : -1.0f; // check tuning step - switch (autotune_state.step) { + switch (step) { - case AUTOTUNE_STEP_WAITING_FOR_LEVEL: + case WAITING_FOR_LEVEL: // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) // re-enable rate limits attitude_control->use_ff_and_input_shaping(true); - autotune_get_poshold_attitude(autotune_roll_cd, autotune_pitch_cd, autotune_desired_yaw); + get_poshold_attitude(roll_cd, pitch_cd, desired_yaw); // hold level attitude - attitude_control->input_euler_angle_roll_pitch_yaw(autotune_roll_cd, autotune_pitch_cd, autotune_desired_yaw, true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true, get_smoothing_gain()); // hold the copter level for 0.5 seconds before we begin a twitch // reset counter if we are no longer level - if (!autotune_currently_level()) { - autotune_step_start_time = millis(); + if (!currently_level()) { + step_start_time = millis(); } // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step - if (millis() - autotune_step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { + if (millis() - step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch"); // initiate variables for next step - autotune_state.step = AUTOTUNE_STEP_TWITCHING; - autotune_step_start_time = millis(); - autotune_step_stop_time = autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; - autotune_state.twitch_first_iter = true; - autotune_test_max = 0.0f; - autotune_test_min = 0.0f; + step = TWITCHING; + step_start_time = millis(); + step_stop_time = step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; + twitch_first_iter = true; + test_max = 0.0f; + test_min = 0.0f; rotation_rate_filt.reset(0.0f); rate_max = 0.0f; // set gains to their to-be-tested values - autotune_load_twitch_gains(); + load_twitch_gains(); } - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - autotune_target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - autotune_target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); - autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f; - autotune_start_angle = ahrs.roll_sensor; + switch (axis) { + case ROLL: + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); + start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f; + start_angle = ahrs.roll_sensor; rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f); - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - rotation_rate_filt.reset(autotune_start_rate); + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + rotation_rate_filt.reset(start_rate); } else { rotation_rate_filt.reset(0); } break; - case AUTOTUNE_AXIS_PITCH: - autotune_target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); - autotune_target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); - autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f; - autotune_start_angle = ahrs.pitch_sensor; + case PITCH: + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); + start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f; + start_angle = ahrs.pitch_sensor; rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f); - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - rotation_rate_filt.reset(autotune_start_rate); + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + rotation_rate_filt.reset(start_rate); } else { rotation_rate_filt.reset(0); } break; - case AUTOTUNE_AXIS_YAW: - autotune_target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS); - autotune_target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); - autotune_start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f; - autotune_start_angle = ahrs.yaw_sensor; + case YAW: + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); + start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f; + start_angle = ahrs.yaw_sensor; rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ); - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - rotation_rate_filt.reset(autotune_start_rate); + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + rotation_rate_filt.reset(start_rate); } else { rotation_rate_filt.reset(0); } @@ -630,7 +548,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() } break; - case AUTOTUNE_STEP_TWITCHING: + case TWITCHING: // Run the twitching step // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) @@ -639,23 +557,23 @@ void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() // hold current attitude attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { // step angle targets on first iteration - if (autotune_state.twitch_first_iter) { - autotune_state.twitch_first_iter = false; + if (twitch_first_iter) { + twitch_first_iter = false; // Testing increasing stabilize P gain so will set lean angle target - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + switch (axis) { + case ROLL: // request roll to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw(direction_sign * autotune_target_angle, 0.0f, 0.0f); + attitude_control->input_angle_step_bf_roll_pitch_yaw(direction_sign * target_angle, 0.0f, 0.0f); break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: // request pitch to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, direction_sign * autotune_target_angle, 0.0f); + attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, direction_sign * target_angle, 0.0f); break; - case AUTOTUNE_AXIS_YAW: + case YAW: // request pitch to 20deg - attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, direction_sign * autotune_target_angle); + attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, direction_sign * target_angle); break; } } @@ -663,71 +581,71 @@ void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() // Testing rate P and D gains so will set body-frame rate targets. // Rate controller will use existing body-frame rates and convert to motor outputs // for all axes except the one we override here. - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + switch (axis) { + case ROLL: // override body-frame roll rate - attitude_control->rate_bf_roll_target(direction_sign * autotune_target_rate + autotune_start_rate); + attitude_control->rate_bf_roll_target(direction_sign * target_rate + start_rate); break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: // override body-frame pitch rate - attitude_control->rate_bf_pitch_target(direction_sign * autotune_target_rate + autotune_start_rate); + attitude_control->rate_bf_pitch_target(direction_sign * target_rate + start_rate); break; - case AUTOTUNE_AXIS_YAW: + case YAW: // override body-frame yaw rate - attitude_control->rate_bf_yaw_target(direction_sign * autotune_target_rate + autotune_start_rate); + attitude_control->rate_bf_yaw_target(direction_sign * target_rate + start_rate); break; } } // capture this iterations rotation rate and lean angle // Add filter to measurements - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + switch (axis) { + case ROLL: + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), MAIN_LOOP_SECONDS); } else { - rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), MAIN_LOOP_SECONDS); } - lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)autotune_start_angle); + lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)start_angle); break; - case AUTOTUNE_AXIS_PITCH: - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + case PITCH: + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), MAIN_LOOP_SECONDS); } else { - rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), MAIN_LOOP_SECONDS); } - lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)autotune_start_angle); + lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)start_angle); break; - case AUTOTUNE_AXIS_YAW: - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { + case YAW: + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), MAIN_LOOP_SECONDS); } else { - rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - autotune_start_rate), MAIN_LOOP_SECONDS); + rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), MAIN_LOOP_SECONDS); } - lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)autotune_start_angle); + lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)start_angle); break; } - switch (autotune_state.tune_type) { - case AUTOTUNE_TYPE_RD_UP: - case AUTOTUNE_TYPE_RD_DOWN: - autotune_twitching_test(rotation_rate, autotune_target_rate, autotune_test_min, autotune_test_max); - autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max); - if (lean_angle >= autotune_target_angle) { - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + switch (tune_type) { + case RD_UP: + case RD_DOWN: + twitching_test(rotation_rate, target_rate, test_min, test_max); + twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); + if (lean_angle >= target_angle) { + step = UPDATE_GAINS; } break; - case AUTOTUNE_TYPE_RP_UP: - autotune_twitching_test(rotation_rate, autotune_target_rate*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max); - autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate, rate_max); - if (lean_angle >= autotune_target_angle) { - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + case RP_UP: + twitching_test(rotation_rate, target_rate*(1+0.5f*g.autotune_aggressiveness), test_min, test_max); + twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); + if (lean_angle >= target_angle) { + step = UPDATE_GAINS; } break; - case AUTOTUNE_TYPE_SP_DOWN: - case AUTOTUNE_TYPE_SP_UP: - autotune_twitching_test(lean_angle, autotune_target_angle*(1+0.5f*g.autotune_aggressiveness), autotune_test_min, autotune_test_max); - autotune_twitching_measure_acceleration(autotune_test_accel_max, rotation_rate - direction_sign * autotune_start_rate, rate_max); + case SP_DOWN: + case SP_UP: + twitching_test(lean_angle, target_angle*(1+0.5f*g.autotune_aggressiveness), test_min, test_max); + twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max); break; } @@ -736,195 +654,195 @@ void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() _copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control); break; - case AUTOTUNE_STEP_UPDATE_GAINS: + case UPDATE_GAINS: // re-enable rate limits attitude_control->use_ff_and_input_shaping(true); // log the latest gains - if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) { - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, autotune_test_accel_max); + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + switch (axis) { + case ROLL: + Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); break; - case AUTOTUNE_AXIS_PITCH: - Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, autotune_test_accel_max); + case PITCH: + Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); break; - case AUTOTUNE_AXIS_YAW: - Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_angle, autotune_test_min, autotune_test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, autotune_test_accel_max); + case YAW: + Log_Write_AutoTune(axis, tune_type, target_angle, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); break; } } else { - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, autotune_test_accel_max); + switch (axis) { + case ROLL: + Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); break; - case AUTOTUNE_AXIS_PITCH: - Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, autotune_test_accel_max); + case PITCH: + Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); break; - case AUTOTUNE_AXIS_YAW: - Log_Write_AutoTune(autotune_state.axis, autotune_state.tune_type, autotune_target_rate, autotune_test_min, autotune_test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, autotune_test_accel_max); + case YAW: + Log_Write_AutoTune(axis, tune_type, target_rate, test_min, test_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); break; } } // Check results after mini-step to increase rate D gain - switch (autotune_state.tune_type) { - case AUTOTUNE_TYPE_RD_UP: - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - autotune_updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + switch (tune_type) { + case RD_UP: + switch (axis) { + case ROLL: + updating_d_up(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; - case AUTOTUNE_AXIS_PITCH: - autotune_updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case PITCH: + updating_d_up(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; - case AUTOTUNE_AXIS_YAW: - autotune_updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case YAW: + updating_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; } break; // Check results after mini-step to decrease rate D gain - case AUTOTUNE_TYPE_RD_DOWN: - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - autotune_updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case RD_DOWN: + switch (axis) { + case ROLL: + updating_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; - case AUTOTUNE_AXIS_PITCH: - autotune_updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case PITCH: + updating_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; - case AUTOTUNE_AXIS_YAW: - autotune_updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case YAW: + updating_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; } break; // Check results after mini-step to increase rate P gain - case AUTOTUNE_TYPE_RP_UP: - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - autotune_updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case RP_UP: + switch (axis) { + case ROLL: + updating_p_up_d_down(tune_roll_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; - case AUTOTUNE_AXIS_PITCH: - autotune_updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case PITCH: + updating_p_up_d_down(tune_pitch_rd, g.autotune_min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; - case AUTOTUNE_AXIS_YAW: - autotune_updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, autotune_target_rate, autotune_test_min, autotune_test_max); + case YAW: + updating_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_min, test_max); break; } break; // Check results after mini-step to increase stabilize P gain - case AUTOTUNE_TYPE_SP_DOWN: - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - autotune_updating_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + case SP_DOWN: + switch (axis) { + case ROLL: + updating_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max); break; - case AUTOTUNE_AXIS_PITCH: - autotune_updating_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + case PITCH: + updating_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max); break; - case AUTOTUNE_AXIS_YAW: - autotune_updating_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + case YAW: + updating_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_max); break; } break; // Check results after mini-step to increase stabilize P gain - case AUTOTUNE_TYPE_SP_UP: - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: - autotune_updating_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + case SP_UP: + switch (axis) { + case ROLL: + updating_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max); break; - case AUTOTUNE_AXIS_PITCH: - autotune_updating_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + case PITCH: + updating_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max); break; - case AUTOTUNE_AXIS_YAW: - autotune_updating_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, autotune_target_angle, autotune_test_max); + case YAW: + updating_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_max); break; } break; } // we've complete this step, finalize pids and move to next step - if (autotune_counter >= AUTOTUNE_SUCCESS_COUNT) { + if (counter >= AUTOTUNE_SUCCESS_COUNT) { // reset counter - autotune_counter = 0; + counter = 0; // move to the next tuning type - switch (autotune_state.tune_type) { - case AUTOTUNE_TYPE_RD_UP: - autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); + switch (tune_type) { + case RD_UP: + tune_type = TuneType(tune_type + 1); break; - case AUTOTUNE_TYPE_RD_DOWN: - autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + case RD_DOWN: + tune_type = TuneType(tune_type + 1); + switch (axis) { + case ROLL: tune_roll_rd = MAX(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF); break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: tune_pitch_rd = MAX(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF); break; - case AUTOTUNE_AXIS_YAW: + case YAW: tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF); tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); break; } break; - case AUTOTUNE_TYPE_RP_UP: - autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + case RP_UP: + tune_type = TuneType(tune_type + 1); + switch (axis) { + case ROLL: tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); break; - case AUTOTUNE_AXIS_YAW: + case YAW: tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); break; } break; - case AUTOTUNE_TYPE_SP_DOWN: - autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); + case SP_DOWN: + tune_type = TuneType(tune_type + 1); break; - case AUTOTUNE_TYPE_SP_UP: + case SP_UP: // we've reached the end of a D-up-down PI-up-down tune type cycle - autotune_state.tune_type = AUTOTUNE_TYPE_RD_UP; + tune_type = RD_UP; // advance to the next axis - bool autotune_complete = false; - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + bool complete = false; + switch (axis) { + case ROLL: tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); - tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); - if (autotune_pitch_enabled()) { - autotune_state.axis = AUTOTUNE_AXIS_PITCH; - } else if (autotune_yaw_enabled()) { - autotune_state.axis = AUTOTUNE_AXIS_YAW; + tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + if (pitch_enabled()) { + axis = PITCH; + } else if (yaw_enabled()) { + axis = YAW; } else { - autotune_complete = true; + complete = true; } break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); - tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); - if (autotune_yaw_enabled()) { - autotune_state.axis = AUTOTUNE_AXIS_YAW; + tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + if (yaw_enabled()) { + axis = YAW; } else { - autotune_complete = true; + complete = true; } break; - case AUTOTUNE_AXIS_YAW: + case YAW: tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); - tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); - autotune_complete = true; + tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); + complete = true; break; } // if we've just completed all axes we have successfully completed the autotune // change to TESTING mode to allow user to fly with new gains - if (autotune_complete) { - autotune_state.mode = AUTOTUNE_MODE_SUCCESS; - autotune_update_gcs(AUTOTUNE_MESSAGE_SUCCESS); + if (complete) { + mode = SUCCESS; + update_gcs(AUTOTUNE_MESSAGE_SUCCESS); Log_Write_Event(DATA_AUTOTUNE_SUCCESS); AP_Notify::events.autotune_complete = 1; } else { @@ -935,40 +853,40 @@ void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() } // reverse direction - autotune_state.positive_direction = !autotune_state.positive_direction; + positive_direction = !positive_direction; - if (autotune_state.axis == AUTOTUNE_AXIS_YAW) { + if (axis == YAW) { attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain()); } // set gains to their intra-test values (which are very close to the original gains) - autotune_load_intra_test_gains(); + load_intra_test_gains(); // reset testing step - autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; - autotune_step_start_time = millis(); + step = WAITING_FOR_LEVEL; + step_start_time = millis(); break; } } -// autotune_backup_gains_and_initialise - store current gains as originals +// backup_gains_and_initialise - store current gains as originals // called before tuning starts to backup original gains -void Copter::FlightMode_AUTOTUNE::autotune_backup_gains_and_initialise() +void Copter::FlightMode_AUTOTUNE::backup_gains_and_initialise() { // initialise state because this is our first time - if (autotune_roll_enabled()) { - autotune_state.axis = AUTOTUNE_AXIS_ROLL; - } else if (autotune_pitch_enabled()) { - autotune_state.axis = AUTOTUNE_AXIS_PITCH; - } else if (autotune_yaw_enabled()) { - autotune_state.axis = AUTOTUNE_AXIS_YAW; + if (roll_enabled()) { + axis = ROLL; + } else if (pitch_enabled()) { + axis = PITCH; + } else if (yaw_enabled()) { + axis = YAW; } - autotune_state.positive_direction = false; - autotune_state.step = AUTOTUNE_STEP_WAITING_FOR_LEVEL; - autotune_step_start_time = millis(); - autotune_state.tune_type = AUTOTUNE_TYPE_RD_UP; + positive_direction = false; + step = WAITING_FOR_LEVEL; + step_start_time = millis(); + tune_type = RD_UP; - autotune_desired_yaw = ahrs.yaw_sensor; + desired_yaw = ahrs.yaw_sensor; g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05f, 0.2f); @@ -1009,12 +927,12 @@ void Copter::FlightMode_AUTOTUNE::autotune_backup_gains_and_initialise() Log_Write_Event(DATA_AUTOTUNE_INITIALISED); } -// autotune_load_orig_gains - set gains to their original values -// called by autotune_stop and autotune_failed functions -void Copter::FlightMode_AUTOTUNE::autotune_load_orig_gains() +// load_orig_gains - set gains to their original values +// called by stop and failed functions +void Copter::FlightMode_AUTOTUNE::load_orig_gains() { attitude_control->bf_feedforward(orig_bf_feedforward); - if (autotune_roll_enabled()) { + if (roll_enabled()) { if (!is_zero(orig_roll_rp)) { attitude_control->get_rate_roll_pid().kP(orig_roll_rp); attitude_control->get_rate_roll_pid().kI(orig_roll_ri); @@ -1023,7 +941,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_orig_gains() attitude_control->set_accel_roll_max(orig_roll_accel); } } - if (autotune_pitch_enabled()) { + if (pitch_enabled()) { if (!is_zero(orig_pitch_rp)) { attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); @@ -1032,7 +950,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_orig_gains() attitude_control->set_accel_pitch_max(orig_pitch_accel); } } - if (autotune_yaw_enabled()) { + if (yaw_enabled()) { if (!is_zero(orig_yaw_rp)) { attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri); @@ -1044,15 +962,15 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_orig_gains() } } -// autotune_load_tuned_gains - load tuned gains -void Copter::FlightMode_AUTOTUNE::autotune_load_tuned_gains() +// load_tuned_gains - load tuned gains +void Copter::FlightMode_AUTOTUNE::load_tuned_gains() { if (!attitude_control->get_bf_feedforward()) { attitude_control->bf_feedforward(true); attitude_control->set_accel_roll_max(0.0f); attitude_control->set_accel_pitch_max(0.0f); } - if (autotune_roll_enabled()) { + if (roll_enabled()) { if (!is_zero(tune_roll_rp)) { attitude_control->get_rate_roll_pid().kP(tune_roll_rp); attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); @@ -1061,7 +979,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_tuned_gains() attitude_control->set_accel_roll_max(tune_roll_accel); } } - if (autotune_pitch_enabled()) { + if (pitch_enabled()) { if (!is_zero(tune_pitch_rp)) { attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); @@ -1070,7 +988,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_tuned_gains() attitude_control->set_accel_pitch_max(tune_pitch_accel); } } - if (autotune_yaw_enabled()) { + if (yaw_enabled()) { if (!is_zero(tune_yaw_rp)) { attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); @@ -1082,26 +1000,26 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_tuned_gains() } } -// autotune_load_intra_test_gains - gains used between tests +// load_intra_test_gains - gains used between tests // called during testing mode's update-gains step to set gains ahead of return-to-level step -void Copter::FlightMode_AUTOTUNE::autotune_load_intra_test_gains() +void Copter::FlightMode_AUTOTUNE::load_intra_test_gains() { // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) // sanity check the gains attitude_control->bf_feedforward(true); - if (autotune_roll_enabled()) { + if (roll_enabled()) { attitude_control->get_rate_roll_pid().kP(orig_roll_rp); attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); attitude_control->get_rate_roll_pid().kD(orig_roll_rd); attitude_control->get_angle_roll_p().kP(orig_roll_sp); } - if (autotune_pitch_enabled()) { + if (pitch_enabled()) { attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); } - if (autotune_yaw_enabled()) { + if (yaw_enabled()) { attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); @@ -1110,24 +1028,24 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_intra_test_gains() } } -// autotune_load_twitch_gains - load the to-be-tested gains for a single axis -// called by autotune_attitude_control() just before it beings testing a gain (i.e. just before it twitches) -void Copter::FlightMode_AUTOTUNE::autotune_load_twitch_gains() +// load_twitch_gains - load the to-be-tested gains for a single axis +// called by attitude_control() just before it beings testing a gain (i.e. just before it twitches) +void Copter::FlightMode_AUTOTUNE::load_twitch_gains() { - switch (autotune_state.axis) { - case AUTOTUNE_AXIS_ROLL: + switch (axis) { + case ROLL: attitude_control->get_rate_roll_pid().kP(tune_roll_rp); attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f); attitude_control->get_rate_roll_pid().kD(tune_roll_rd); attitude_control->get_angle_roll_p().kP(tune_roll_sp); break; - case AUTOTUNE_AXIS_PITCH: + case PITCH: attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f); attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); break; - case AUTOTUNE_AXIS_YAW: + case YAW: attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); attitude_control->get_rate_yaw_pid().kD(0.0f); @@ -1137,12 +1055,12 @@ void Copter::FlightMode_AUTOTUNE::autotune_load_twitch_gains() } } -// autotune_save_tuning_gains - save the final tuned gains for each axis +// save_tuning_gains - save the final tuned gains for each axis // save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position) -void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains() +void Copter::FlightMode_AUTOTUNE::save_tuning_gains() { // if we successfully completed tuning - if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) { + if (mode == SUCCESS) { if (!attitude_control->get_bf_feedforward()) { attitude_control->bf_feedforward_save(true); @@ -1151,7 +1069,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains() } // sanity check the rate P values - if (autotune_roll_enabled() && !is_zero(tune_roll_rp)) { + if (roll_enabled() && !is_zero(tune_roll_rp)) { // rate roll gains attitude_control->get_rate_roll_pid().kP(tune_roll_rp); attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); @@ -1173,7 +1091,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains() orig_roll_accel = attitude_control->get_accel_roll_max(); } - if (autotune_pitch_enabled() && !is_zero(tune_pitch_rp)) { + if (pitch_enabled() && !is_zero(tune_pitch_rp)) { // rate pitch gains attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); @@ -1195,7 +1113,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains() orig_pitch_accel = attitude_control->get_accel_pitch_max(); } - if (autotune_yaw_enabled() && !is_zero(tune_yaw_rp)) { + if (yaw_enabled() && !is_zero(tune_yaw_rp)) { // rate yaw gains attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); @@ -1219,15 +1137,15 @@ void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains() orig_yaw_accel = attitude_control->get_accel_pitch_max(); } // update GCS and log save gains event - autotune_update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); + update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS); // reset Autotune so that gains are not saved again and autotune can be run again. - autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED; + mode = UNINITIALISED; } } -// autotune_update_gcs - send message to ground station -void Copter::FlightMode_AUTOTUNE::autotune_update_gcs(uint8_t message_id) +// update_gcs - send message to ground station +void Copter::FlightMode_AUTOTUNE::update_gcs(uint8_t message_id) { switch (message_id) { case AUTOTUNE_MESSAGE_STARTED: @@ -1249,21 +1167,21 @@ void Copter::FlightMode_AUTOTUNE::autotune_update_gcs(uint8_t message_id) } // axis helper functions -inline bool Copter::FlightMode_AUTOTUNE::autotune_roll_enabled() { +inline bool Copter::FlightMode_AUTOTUNE::roll_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL; } -inline bool Copter::FlightMode_AUTOTUNE::autotune_pitch_enabled() { +inline bool Copter::FlightMode_AUTOTUNE::pitch_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH; } -inline bool Copter::FlightMode_AUTOTUNE::autotune_yaw_enabled() { +inline bool Copter::FlightMode_AUTOTUNE::yaw_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; } -// autotune_twitching_test - twitching tests +// twitching_test - twitching tests // update min and max and test for end conditions -void Copter::FlightMode_AUTOTUNE::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) +void Copter::FlightMode_AUTOTUNE::twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) { // capture maximum measurement if (measurement > measurement_max) { @@ -1281,29 +1199,29 @@ void Copter::FlightMode_AUTOTUNE::autotune_twitching_test(float measurement, flo // calculate early stopping time based on the time it takes to get to 90% if (measurement_max < target * 0.75f) { // the measurement not reached the 90% threshold yet - autotune_step_stop_time = autotune_step_start_time + (millis() - autotune_step_start_time) * 3.0f; - autotune_step_stop_time = MIN(autotune_step_stop_time, autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + step_stop_time = step_start_time + (millis() - step_start_time) * 3.0f; + step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); } if (measurement_max > target) { // the measurement has passed the target - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + step = UPDATE_GAINS; } if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { // the measurement has passed 50% of the target and bounce back is larger than the threshold - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + step = UPDATE_GAINS; } - if (millis() >= autotune_step_stop_time) { + if (millis() >= step_stop_time) { // we have passed the maximum stop time - autotune_state.step = AUTOTUNE_STEP_UPDATE_GAINS; + step = UPDATE_GAINS; } } -// autotune_updating_d_up - increase D and adjust P to optimize the D term for a little bounce back +// updating_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void Copter::FlightMode_AUTOTUNE::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::FlightMode_AUTOTUNE::updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // if maximum measurement was higher than target @@ -1316,7 +1234,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_updating_d_up(float &tune_d, float tu if (tune_d <= tune_d_min) { // We have reached minimum D gain so stop tuning tune_d = tune_d_min; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } @@ -1332,33 +1250,33 @@ void Copter::FlightMode_AUTOTUNE::autotune_updating_d_up(float &tune_d, float tu // we have a good measurement of bounce back if (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) { // ignore the next result unless it is the same as this one - autotune_state.ignore_next = true; + ignore_next = true; // bounce back is bigger than our threshold so increment the success counter - autotune_counter++; + counter++; }else{ - if (autotune_state.ignore_next == false) { + if (ignore_next == false) { // bounce back is smaller than our threshold so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; + if (counter > 0 ) { + counter--; } // increase D gain (which should increase bounce back) tune_d += tune_d*tune_d_step_ratio*2.0f; // stop tuning if we hit maximum D if (tune_d >= tune_d_max) { tune_d = tune_d_max; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } else { - autotune_state.ignore_next = false; + ignore_next = false; } } } } -// autotune_updating_d_down - decrease D and adjust P to optimize the D term for no bounce back +// updating_d_down - decrease D and adjust P to optimize the D term for no bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void Copter::FlightMode_AUTOTUNE::autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::FlightMode_AUTOTUNE::updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // if maximum measurement was higher than target @@ -1371,7 +1289,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_updating_d_down(float &tune_d, float if (tune_d <= tune_d_min) { // We have reached minimum D so stop tuning tune_d = tune_d_min; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } @@ -1386,102 +1304,102 @@ void Copter::FlightMode_AUTOTUNE::autotune_updating_d_down(float &tune_d, float }else{ // we have a good measurement of bounce back if (measurement_max-measurement_min < measurement_max*g.autotune_aggressiveness) { - if (autotune_state.ignore_next == false) { + if (ignore_next == false) { // bounce back is less than our threshold so increment the success counter - autotune_counter++; + counter++; } else { - autotune_state.ignore_next = false; + ignore_next = false; } }else{ // ignore the next result unless it is the same as this one - autotune_state.ignore_next = true; + ignore_next = true; // bounce back is larger than our threshold so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; + if (counter > 0 ) { + counter--; } // decrease D gain (which should decrease bounce back) tune_d -= tune_d*tune_d_step_ratio; // stop tuning if we hit minimum D if (tune_d <= tune_d_min) { tune_d = tune_d_min; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } } } -// autotune_updating_p_down - decrease P until we don't reach the target before time out +// updating_p_down - decrease P until we don't reach the target before time out // P is decreased to ensure we are not overshooting the target -void Copter::FlightMode_AUTOTUNE::autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) +void Copter::FlightMode_AUTOTUNE::updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) { - if (autotune_state.ignore_next == false) { + if (ignore_next == false) { // if maximum measurement was lower than target so increment the success counter - autotune_counter++; + counter++; } else { - autotune_state.ignore_next = false; + ignore_next = false; } }else{ // ignore the next result unless it is the same as this one - autotune_state.ignore_next = true; + ignore_next = true; // if maximum measurement was higher than target so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; + if (counter > 0 ) { + counter--; } // decrease P gain (which should decrease the maximum) tune_p -= tune_p*tune_p_step_ratio; // stop tuning if we hit maximum P if (tune_p <= tune_p_min) { tune_p = tune_p_min; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } } -// autotune_updating_p_up - increase P to ensure the target is reached +// updating_p_up - increase P to ensure the target is reached // P is increased until we achieve our target within a reasonable time -void Copter::FlightMode_AUTOTUNE::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) +void Copter::FlightMode_AUTOTUNE::updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { // ignore the next result unless it is the same as this one - autotune_state.ignore_next = 1; + ignore_next = 1; // if maximum measurement was greater than target so increment the success counter - autotune_counter++; + counter++; }else{ - if (autotune_state.ignore_next == false) { + if (ignore_next == false) { // if maximum measurement was lower than target so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; + if (counter > 0 ) { + counter--; } // increase P gain (which should increase the maximum) tune_p += tune_p*tune_p_step_ratio; // stop tuning if we hit maximum P if (tune_p >= tune_p_max) { tune_p = tune_p_max; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } else { - autotune_state.ignore_next = false; + ignore_next = false; } } } -// autotune_updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing +// updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing // P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold -void Copter::FlightMode_AUTOTUNE::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::FlightMode_AUTOTUNE::updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { // ignore the next result unless it is the same as this one - autotune_state.ignore_next = true; + ignore_next = true; // if maximum measurement was greater than target so increment the success counter - autotune_counter++; + counter++; } else if ((measurement_max < target) && (measurement_max > target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (measurement_max-measurement_min > measurement_max*g.autotune_aggressiveness) && (tune_d > tune_d_min)) { // if bounce back was larger than the threshold so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; + if (counter > 0 ) { + counter--; } // decrease D gain (which should decrease bounce back) tune_d -= tune_d*tune_d_step_ratio; @@ -1498,42 +1416,42 @@ void Copter::FlightMode_AUTOTUNE::autotune_updating_p_up_d_down(float &tune_d, f Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } // cancel change in direction - autotune_state.positive_direction = !autotune_state.positive_direction; + positive_direction = !positive_direction; }else{ - if (autotune_state.ignore_next == false) { + if (ignore_next == false) { // if maximum measurement was lower than target so decrement the success counter - if (autotune_counter > 0 ) { - autotune_counter--; + if (counter > 0 ) { + counter--; } // increase P gain (which should increase the maximum) tune_p += tune_p*tune_p_step_ratio; // stop tuning if we hit maximum P if (tune_p >= tune_p_max) { tune_p = tune_p_max; - autotune_counter = AUTOTUNE_SUCCESS_COUNT; + counter = AUTOTUNE_SUCCESS_COUNT; Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT); } } else { - autotune_state.ignore_next = false; + ignore_next = false; } } } -// autotune_twitching_measure_acceleration - measure rate of change of measurement -void Copter::FlightMode_AUTOTUNE::autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) +// twitching_measure_acceleration - measure rate of change of measurement +void Copter::FlightMode_AUTOTUNE::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) { if (rate_measurement_max < rate_measurement) { rate_measurement_max = rate_measurement; - rate_of_change = (1000.0f*rate_measurement_max)/(millis() - autotune_step_start_time); + rate_of_change = (1000.0f*rate_measurement_max)/(millis() - step_start_time); } } // get attitude for slow position hold in autotune mode -void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd) +void Copter::FlightMode_AUTOTUNE::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out) { - roll_cd = pitch_cd = 0; + roll_cd_out = pitch_cd_out = 0; - if (!autotune_state.use_poshold) { + if (!use_poshold) { // we are not trying to hold position return; } @@ -1543,9 +1461,9 @@ void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, return; } - if (!autotune_state.have_position) { - autotune_state.have_position = true; - autotune_state.start_position = inertial_nav.get_position(); + if (!have_position) { + have_position = true; + start_position = inertial_nav.get_position(); } // don't go past 10 degrees, as autotune result would deteriorate too much @@ -1558,7 +1476,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, // target position. That corresponds to a lean angle of 2.5 degrees const float yaw_dist_limit_cm = 500; - Vector3f pdiff = inertial_nav.get_position() - autotune_state.start_position; + Vector3f pdiff = inertial_nav.get_position() - start_position; pdiff.z = 0; float dist_cm = pdiff.length(); if (dist_cm < 10) { @@ -1574,8 +1492,8 @@ void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, angle_ne *= scaling / dist_cm; // rotate into body frame - pitch_cd = angle_ne.x * ahrs.cos_yaw() + angle_ne.y * ahrs.sin_yaw(); - roll_cd = angle_ne.x * ahrs.sin_yaw() - angle_ne.y * ahrs.cos_yaw(); + pitch_cd_out = angle_ne.x * ahrs.cos_yaw() + angle_ne.y * ahrs.sin_yaw(); + roll_cd_out = angle_ne.x * ahrs.sin_yaw() - angle_ne.y * ahrs.cos_yaw(); if (dist_cm < yaw_dist_limit_cm) { // no yaw adjustment @@ -1589,17 +1507,17 @@ void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, more than 2.5 degrees of attitude on the axis it is tuning */ float target_yaw_cd = degrees(atan2f(pdiff.y, pdiff.x)) * 100; - if (autotune_state.axis == AUTOTUNE_AXIS_PITCH) { + if (axis == PITCH) { // for roll and yaw tuning we point along the wind, for pitch // we point across the wind target_yaw_cd += 9000; } // go to the nearest 180 degree mark, with 5 degree slop to prevent oscillation - if (fabsf(yaw_cd - target_yaw_cd) > 9500) { + if (fabsf(yaw_cd_out - target_yaw_cd) > 9500) { target_yaw_cd += 18000; } - yaw_cd = target_yaw_cd; + yaw_cd_out = target_yaw_cd; } #endif // AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index e6aa142e43..ce0be59cd1 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -183,7 +183,7 @@ void Copter::exit_mode(Copter::FlightMode *&old_flightmode, { #if AUTOTUNE_ENABLED == ENABLED if (old_flightmode == &flightmode_autotune) { - flightmode_autotune.autotune_stop(); + flightmode_autotune.stop(); } #endif diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 83786998af..f096d52a0b 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -246,7 +246,7 @@ void Copter::init_disarm_motors() #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters - flightmode_autotune.autotune_save_tuning_gains(); + flightmode_autotune.save_tuning_gains(); #endif // we are not in the air