Copter: integrate control_autotune

ArduCopter/ArduCopter.pde

@@ -1397,17 +1397,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
         case ROLL_PITCH_SPORT:
             roll_pitch_initialised = true;
             break;
-
-#if AUTOTUNE == ENABLED
-        case ROLL_PITCH_AUTOTUNE:
-            // only enter autotune mode from stabilized roll-pitch mode when armed and flying
-            if (roll_pitch_mode == ROLL_PITCH_STABLE && motors.armed() && !ap.land_complete) {
                 reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in);
-                // auto_tune_start returns true if it wants the roll-pitch mode changed to autotune
-                roll_pitch_initialised = auto_tune_start();
-            }
-            break;
-#endif
             }
 
     // if initialisation has been successful update the yaw mode
@@ -1423,11 +1413,6 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
 // exit_roll_pitch_mode - peforms any code required when exiting the current roll-pitch mode
 void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode)
 {
-#if AUTOTUNE == ENABLED
-    if (old_roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
-        auto_tune_stop();
-    }
-#endif
 }
 
 // update_flight_mode - calls the appropriate attitude controllers based on flight mode
@@ -1494,6 +1479,12 @@ static void update_flight_mode()
         case FLIP:
             flip_run();
             break;
+
+#if AUTOTUNE_ENABLED == ENABLED
+        case AUTOTUNE:
+            autotune_run();
+            break;
+#endif
     }
 }
 
@@ -1579,25 +1570,6 @@ void update_roll_pitch_mode(void)
         get_roll_rate_stabilized_ef(g.rc_1.control_in);
         get_pitch_rate_stabilized_ef(g.rc_2.control_in);
         break;
-
-#if AUTOTUNE == ENABLED
-    case ROLL_PITCH_AUTOTUNE:
-        // apply SIMPLE mode transform
-        if(ap.simple_mode && ap.new_radio_frame) {
-            update_simple_mode();
-        }
-
-        // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
-
-        // pass desired roll, pitch to stabilize attitude controllers
-        get_stabilize_roll(control_roll);
-        get_stabilize_pitch(control_pitch);
-
-        // copy user input for reporting purposes
-        get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in);
-        break;
-#endif
         }
 
 	#if FRAME_CONFIG != HELI_FRAME

ArduCopter/Log.pde

@@ -157,7 +157,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
     return 0;
 }
 
-#if AUTOTUNE == ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
 struct PACKED log_AutoTune {
     LOG_PACKET_HEADER;
     uint8_t axis;           // roll or pitch
@@ -774,7 +774,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
 static void Log_Write_Mode(uint8_t mode) {}
 static void Log_Write_IMU() {}
 static void Log_Write_GPS() {}
-#if AUTOTUNE == ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
 static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) {}
 static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) {}
 #endif

ArduCopter/config.h

@@ -130,7 +130,7 @@
   # define HELI_STAB_COLLECTIVE_MIN_DEFAULT     0
   # define HELI_STAB_COLLECTIVE_MAX_DEFAULT     1000
   # define THR_MIN_DEFAULT                      0
-  # define AUTOTUNE                             DISABLED
+  # define AUTOTUNE_ENABLED                     DISABLED
   
   # ifndef HELI_CC_COMP
     #define HELI_CC_COMP DISABLED
@@ -411,8 +411,8 @@
 
 //////////////////////////////////////////////////////////////////////////////
 //  Auto Tuning
-#ifndef AUTOTUNE
+#ifndef AUTOTUNE_ENABLED
- # define AUTOTUNE  ENABLED
+ # define AUTOTUNE_ENABLED  ENABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/control_modes.pde

@@ -340,21 +340,22 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             }
             break;
 
-#if AUTOTUNE == ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
         case AUX_SWITCH_AUTOTUNE:
             // turn on auto tuner
             switch(ch_flag) {
                 case AUX_SWITCH_LOW:
                 case AUX_SWITCH_MIDDLE:
-                    // turn off tuning and return to standard pids
+                    // stop the autotune and return to original gains
-                    if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
+                    autotune_stop();
-                        set_roll_pitch_mode(ROLL_PITCH_STABLE);
+                    // restore flight mode based on flight mode switch position
+                    if (control_mode == AUTOTUNE) {
+                        reset_control_switch();
                     }
                     break;
                 case AUX_SWITCH_HIGH:
-                    // start an auto tuning session
+                    // start an autotuning session
-                    // set roll-pitch mode to our special auto tuning stabilize roll-pitch mode
+                    autotune_start();
-                    set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE);
                     break;
             }
             break;

ArduCopter/defines.h

@@ -128,7 +128,8 @@
 #define DRIFT 11                        // DRIFT mode (Note: 12 is no longer used)
 #define SPORT 13                        // earth frame rate control
 #define FLIP        14                  // flip the vehicle on the roll axis
-#define NUM_MODES 15
+#define AUTOTUNE    15                  // autotune the vehicle's roll and pitch gains
+#define NUM_MODES   16
 
 
 // CH_6 Tuning
@@ -313,10 +314,10 @@ enum FlipState {
 #define DATA_AUTOTUNE_INITIALISED       30
 #define DATA_AUTOTUNE_OFF               31
 #define DATA_AUTOTUNE_RESTART           32
-#define DATA_AUTOTUNE_COMPLETE          33
+#define DATA_AUTOTUNE_SUCCESS           33
-#define DATA_AUTOTUNE_ABANDONED         34
+#define DATA_AUTOTUNE_FAILED            34
 #define DATA_AUTOTUNE_REACHED_LIMIT     35
-#define DATA_AUTOTUNE_TESTING           36
+#define DATA_AUTOTUNE_PILOT_TESTING     36
 #define DATA_AUTOTUNE_SAVEDGAINS        37
 #define DATA_SAVE_TRIM                  38
 #define DATA_SAVEWP_ADD_WP              39
@@ -383,6 +384,7 @@ enum FlipState {
 #define ERROR_SUBSYSTEM_GPS                 11
 #define ERROR_SUBSYSTEM_CRASH_CHECK         12
 #define ERROR_SUBSYSTEM_FLIP                13
+#define ERROR_SUBSYSTEM_AUTOTUNE            14
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
@@ -401,6 +403,8 @@ enum FlipState {
 #define ERROR_CODE_CRASH_CHECK_CRASH        1
 // subsystem specific error codes -- flip
 #define ERROR_CODE_FLIP_ABANDONED           2
+// subsystem specific error codes -- autotune
+#define ERROR_CODE_AUTOTUNE_BAD_GAINS       2
 
 // Arming Check Enable/Disable bits
 #define ARMING_CHECK_NONE                   0x00

ArduCopter/motors.pde

@@ -514,9 +514,9 @@ static void init_disarm_motors()
 
     g.throttle_cruise.save();
 
-#if AUTOTUNE == ENABLED
+#if AUTOTUNE_ENABLED == ENABLED
     // save auto tuned parameters
-    auto_tune_save_tuning_gains_and_reset();
+    autotune_save_tuning_gains();
 #endif
 
     // we are not in the air

ArduCopter/system.pde

@@ -450,6 +450,12 @@ static bool set_mode(uint8_t mode)
             success = flip_init(ignore_checks);
             break;
 
+#if AUTOTUNE_ENABLED == ENABLED
+        case AUTOTUNE:
+            success = autotune_init(ignore_checks);
+            break;
+#endif
+
         default:
             success = false;
             break;
@@ -457,6 +463,8 @@ static bool set_mode(uint8_t mode)
 
     // update flight mode
     if (success) {
+        // perform any cleanup required by previous flight mode
+        exit_mode(control_mode);
         control_mode = mode;
         Log_Write_Mode(control_mode);
     }else{
@@ -468,6 +476,16 @@ static bool set_mode(uint8_t mode)
     return success;
 }
 
+// exit_mode - high level call to organise cleanup as a flight mode is exited
+static void exit_mode(uint8_t old_control_mode)
+{
+#if AUTOTUNE_ENABLED == ENABLED
+    if (old_control_mode == AUTOTUNE) {
+        autotune_stop();
+    }
+#endif
+}
+
 // update_auto_armed - update status of auto_armed flag
 static void update_auto_armed()
 {
@@ -588,6 +606,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case FLIP:
         port->print_P(PSTR("FLIP"));
         break;
+    case AUTOTUNE:
+        port->print_P(PSTR("AUTOTUNE"));
+        break;
     default:
         port->printf_P(PSTR("Mode(%u)"), (unsigned)mode);
         break;