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

@@ -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;
 

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
 

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,

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
 

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