Copter: FlightMode - convert AUTOTUNE flight mode

ArduCopter/Copter.h

@@ -709,8 +709,6 @@ private:
     void send_pid_tuning(mavlink_channel_t chan);
     void gcs_data_stream_send(void);
     void gcs_check_input(void);
-    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 Log_Write_Current();
     void Log_Write_Optflow();
     void Log_Write_Nav_Tuning();
@@ -776,37 +774,7 @@ private:
     void set_auto_yaw_rate(float turn_rate_cds);
     float get_auto_heading(void);
     float get_auto_yaw_rate_cds();
-    bool autotune_init(bool ignore_checks);
-    void autotune_stop();
-    bool autotune_start(bool ignore_checks);
-    void autotune_run();
-    bool autotune_currently_level();
-    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_save_tuning_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 avoidance_adsb_update(void);
-    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;
 
 #if ADVANCED_FAILSAFE == ENABLED
     void afs_fs_check(void);
@@ -1055,6 +1023,10 @@ private:
 
     Copter::FlightMode_AUTO flightmode_auto{*this, mission, circle_nav};
 
+#if AUTOTUNE_ENABLED == ENABLED
+    Copter::FlightMode_AUTOTUNE flightmode_autotune{*this};
+#endif
+
     Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav};
 
     Copter::FlightMode_DRIFT flightmode_drift{*this};

ArduCopter/FlightMode.h

@@ -663,3 +663,69 @@ private:
     Vector3f flip_orig_attitude;         // original vehicle attitude before flip
 
 };
+
+
+
+#if AUTOTUNE_ENABLED == ENABLED
+class FlightMode_AUTOTUNE : public FlightMode {
+
+public:
+
+    FlightMode_AUTOTUNE(Copter &copter) :
+        Copter::FlightMode(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    bool requires_GPS() const override { return false; }
+    bool has_manual_throttle() const override { return false; }
+    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 autotune_stop();
+    void autotune_save_tuning_gains();
+
+protected:
+
+    const char *name() const override { return "AUTOTUNE"; }
+    const char *name4() const override { return "ATUN"; }
+
+private:
+
+    bool autotune_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 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();
+};
+#endif

ArduCopter/Log.cpp

@@ -21,7 +21,7 @@ struct PACKED log_AutoTune {
 };
 
 // Write an Autotune data packet
-void Copter::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),
@@ -36,7 +36,7 @@ void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_targ
         new_gain_sp : new_gain_sp,
         new_ddt     : new_ddt
     };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+    _copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
 struct PACKED log_AutoTuneDetails {
@@ -47,7 +47,7 @@ struct PACKED log_AutoTuneDetails {
 };
 
 // Write an Autotune data packet
-void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
+void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
 {
     struct log_AutoTuneDetails pkt = {
         LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
@@ -55,7 +55,7 @@ void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
         angle_cd    : angle_cd,
         rate_cds    : rate_cds
     };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+    _copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 #endif
 

ArduCopter/control_autotune.cpp

@@ -175,7 +175,7 @@ static struct {
 } autotune_level_problem;
 
 // autotune_init - should be called when autotune mode is selected
-bool Copter::autotune_init(bool ignore_checks)
+bool Copter::FlightMode_AUTOTUNE::init(bool ignore_checks)
 {
     bool success = true;
 
@@ -220,14 +220,14 @@ bool Copter::autotune_init(bool ignore_checks)
     }
 
     // only do position hold if starting autotune from LOITER or POSHOLD
-    autotune_state.use_poshold = (control_mode == LOITER || control_mode == POSHOLD);
+    autotune_state.use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD);
     autotune_state.have_position = false;
 
     return success;
 }
 
 // autotune_stop - should be called when the ch7/ch8 switch is switched OFF
-void Copter::autotune_stop()
+void Copter::FlightMode_AUTOTUNE::autotune_stop()
 {
     // set gains to their original values
     autotune_load_orig_gains();
@@ -244,11 +244,11 @@ void Copter::autotune_stop()
 }
 
 // autotune_start - Initialize autotune flight mode
-bool Copter::autotune_start(bool ignore_checks)
+bool Copter::FlightMode_AUTOTUNE::autotune_start(bool ignore_checks)
 {
     // only allow flip from Stabilize, AltHold,  PosHold or Loiter modes
-    if (control_mode != STABILIZE && control_mode != ALT_HOLD &&
+    if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD &&
-        control_mode != LOITER && control_mode != POSHOLD) {
+        _copter.control_mode != LOITER && _copter.control_mode != POSHOLD) {
         return false;
     }
 
@@ -275,7 +275,7 @@ bool Copter::autotune_start(bool ignore_checks)
     return true;
 }
 
-const char *Copter::autotune_level_issue_string() const
+const char *Copter::FlightMode_AUTOTUNE::autotune_level_issue_string() const
 {
     switch (autotune_level_problem.issue) {
     case Copter::AUTOTUNE_LEVEL_ISSUE_NONE:
@@ -296,7 +296,7 @@ const char *Copter::autotune_level_issue_string() const
     return "Bug";
 }
 
-void Copter::autotune_send_step_string()
+void Copter::FlightMode_AUTOTUNE::autotune_send_step_string()
 {
     if (autotune_state.pilot_override) {
         gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
@@ -316,7 +316,7 @@ void Copter::autotune_send_step_string()
     gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step");
 }
 
-const char *Copter::autotune_type_string() const
+const char *Copter::FlightMode_AUTOTUNE::autotune_type_string() const
 {
     switch (autotune_state.tune_type) {
     case AUTOTUNE_TYPE_RD_UP:
@@ -333,7 +333,7 @@ const char *Copter::autotune_type_string() const
     return "Bug";
 }
 
-void Copter::autotune_do_gcs_announcements()
+void Copter::FlightMode_AUTOTUNE::autotune_do_gcs_announcements()
 {
     const uint32_t now = millis();
     if (now - autotune_announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
@@ -394,7 +394,7 @@ void Copter::autotune_do_gcs_announcements()
 
 // autotune_run - runs the autotune flight mode
 // should be called at 100hz or more
-void Copter::autotune_run()
+void Copter::FlightMode_AUTOTUNE::run()
 {
     float target_roll, target_pitch;
     float target_yaw_rate;
@@ -420,7 +420,7 @@ void Copter::autotune_run()
     update_simple_mode();
 
     // get pilot desired lean angles
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
@@ -501,7 +501,7 @@ void Copter::autotune_run()
     }
 }
 
-bool Copter::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const
+bool Copter::FlightMode_AUTOTUNE::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, const float current, const float maximum) const
 {
     if (current > maximum) {
         autotune_level_problem.current = current;
@@ -512,7 +512,7 @@ bool Copter::autotune_check_level(const Copter::AUTOTUNE_LEVEL_ISSUE issue, cons
     return true;
 }
 
-bool Copter::autotune_currently_level()
+bool Copter::FlightMode_AUTOTUNE::autotune_currently_level()
 {
     if (!autotune_check_level(Copter::AUTOTUNE_LEVEL_ISSUE_ANGLE_ROLL,
                               labs(ahrs.roll_sensor - autotune_roll_cd),
@@ -549,7 +549,7 @@ bool Copter::autotune_currently_level()
 }
 
 // autotune_attitude_controller - sets attitude control targets during tuning
-void Copter::autotune_attitude_control()
+void Copter::FlightMode_AUTOTUNE::autotune_attitude_control()
 {
     rotation_rate = 0.0f;        // rotation rate in radians/second
     lean_angle = 0.0f;
@@ -733,7 +733,7 @@ void Copter::autotune_attitude_control()
 
         // log this iterations lean angle and rotation rate
         Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
-        DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
+        _copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
         break;
 
     case AUTOTUNE_STEP_UPDATE_GAINS:
@@ -953,7 +953,7 @@ void Copter::autotune_attitude_control()
 
 // autotune_backup_gains_and_initialise - store current gains as originals
 //  called before tuning starts to backup original gains
-void Copter::autotune_backup_gains_and_initialise()
+void Copter::FlightMode_AUTOTUNE::autotune_backup_gains_and_initialise()
 {
     // initialise state because this is our first time
     if (autotune_roll_enabled()) {
@@ -1011,7 +1011,7 @@ void Copter::autotune_backup_gains_and_initialise()
 
 // autotune_load_orig_gains - set gains to their original values
 //  called by autotune_stop and autotune_failed functions
-void Copter::autotune_load_orig_gains()
+void Copter::FlightMode_AUTOTUNE::autotune_load_orig_gains()
 {
     attitude_control->bf_feedforward(orig_bf_feedforward);
     if (autotune_roll_enabled()) {
@@ -1045,7 +1045,7 @@ void Copter::autotune_load_orig_gains()
 }
 
 // autotune_load_tuned_gains - load tuned gains
-void Copter::autotune_load_tuned_gains()
+void Copter::FlightMode_AUTOTUNE::autotune_load_tuned_gains()
 {
     if (!attitude_control->get_bf_feedforward()) {
         attitude_control->bf_feedforward(true);
@@ -1084,7 +1084,7 @@ void Copter::autotune_load_tuned_gains()
 
 // autotune_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::autotune_load_intra_test_gains()
+void Copter::FlightMode_AUTOTUNE::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
@@ -1112,7 +1112,7 @@ void Copter::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::autotune_load_twitch_gains()
+void Copter::FlightMode_AUTOTUNE::autotune_load_twitch_gains()
 {
     switch (autotune_state.axis) {
         case AUTOTUNE_AXIS_ROLL:
@@ -1139,7 +1139,7 @@ void Copter::autotune_load_twitch_gains()
 
 // autotune_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::autotune_save_tuning_gains()
+void Copter::FlightMode_AUTOTUNE::autotune_save_tuning_gains()
 {
     // if we successfully completed tuning
     if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) {
@@ -1227,7 +1227,7 @@ void Copter::autotune_save_tuning_gains()
 }
 
 // autotune_update_gcs - send message to ground station
-void Copter::autotune_update_gcs(uint8_t message_id)
+void Copter::FlightMode_AUTOTUNE::autotune_update_gcs(uint8_t message_id)
 {
     switch (message_id) {
         case AUTOTUNE_MESSAGE_STARTED:
@@ -1249,21 +1249,21 @@ void Copter::autotune_update_gcs(uint8_t message_id)
 }
 
 // axis helper functions
-inline bool Copter::autotune_roll_enabled() {
+inline bool Copter::FlightMode_AUTOTUNE::autotune_roll_enabled() {
     return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL;
 }
 
-inline bool Copter::autotune_pitch_enabled() {
+inline bool Copter::FlightMode_AUTOTUNE::autotune_pitch_enabled() {
     return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH;
 }
 
-inline bool Copter::autotune_yaw_enabled() {
+inline bool Copter::FlightMode_AUTOTUNE::autotune_yaw_enabled() {
     return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
 }
 
 // autotune_twitching_test - twitching tests
 // update min and max and test for end conditions
-void Copter::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max)
+void Copter::FlightMode_AUTOTUNE::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max)
 {
     // capture maximum measurement
     if (measurement > measurement_max) {
@@ -1303,7 +1303,7 @@ void Copter::autotune_twitching_test(float measurement, float target, float &mea
 
 // autotune_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::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::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
@@ -1358,7 +1358,7 @@ void Copter::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_
 
 // autotune_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::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::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
@@ -1413,7 +1413,7 @@ void Copter::autotune_updating_d_down(float &tune_d, float tune_d_min, float tun
 
 // autotune_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::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::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) {
@@ -1442,7 +1442,7 @@ void Copter::autotune_updating_p_down(float &tune_p, float tune_p_min, float tun
 
 // autotune_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::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::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
@@ -1471,7 +1471,7 @@ void Copter::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_
 
 // autotune_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::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::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
@@ -1520,7 +1520,7 @@ void Copter::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, floa
 }
 
 // autotune_twitching_measure_acceleration - measure rate of change of measurement
-void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
+void Copter::FlightMode_AUTOTUNE::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;
@@ -1529,7 +1529,7 @@ void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, floa
 }
 
 // get attitude for slow position hold in autotune mode
-void Copter::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd)
+void Copter::FlightMode_AUTOTUNE::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd)
 {
     roll_cd = pitch_cd = 0;
 
@@ -1539,7 +1539,7 @@ void Copter::autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, floa
     }
 
     // do we know where we are?
-    if (!position_ok()) {
+    if (!_copter.position_ok()) {
         return;
     }
 

ArduCopter/flight_mode.cpp

@@ -125,7 +125,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
 
 #if AUTOTUNE_ENABLED == ENABLED
         case AUTOTUNE:
-            success = autotune_init(ignore_checks);
+            success = flightmode_autotune.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_autotune;
+            }
             break;
 #endif
 
@@ -221,12 +224,6 @@ void Copter::update_flight_mode()
 
     switch (control_mode) {
 
-#if AUTOTUNE_ENABLED == ENABLED
-        case AUTOTUNE:
-            autotune_run();
-            break;
-#endif
-
 #if POSHOLD_ENABLED == ENABLED
         case POSHOLD:
             poshold_run();
@@ -264,7 +261,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
 {
 #if AUTOTUNE_ENABLED == ENABLED
     if (old_control_mode == AUTOTUNE) {
-        autotune_stop();
+        flightmode_autotune.autotune_stop();
     }
 #endif
 
@@ -381,9 +378,6 @@ void Copter::notify_flight_mode()
 
     // set flight mode string
     switch (control_mode) {
-        case AUTOTUNE:
-            notify.set_flight_mode_str("ATUN");
-            break;
         case POSHOLD:
             notify.set_flight_mode_str("PHLD");
             break;

ArduCopter/motors.cpp

@@ -246,7 +246,7 @@ void Copter::init_disarm_motors()
 
 #if AUTOTUNE_ENABLED == ENABLED
     // save auto tuned parameters
-    autotune_save_tuning_gains();
+    flightmode_autotune.autotune_save_tuning_gains();
 #endif
 
     // we are not in the air