Plane: added AUTOTUNE flight mode

this is just like FBWA, but with automatic roll/pitch tuning

ArduPlane/ArduPlane.pde

@@ -1230,6 +1230,7 @@ static void update_flight_mode(void)
         break;
     }
 
+    case AUTOTUNE:
     case FLY_BY_WIRE_A: {
         // set nav_roll and nav_pitch using sticks
         nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
@@ -1342,6 +1343,7 @@ static void update_navigation()
     case INITIALISING:
     case ACRO:
     case FLY_BY_WIRE_A:
+    case AUTOTUNE:
     case FLY_BY_WIRE_B:
     case CIRCLE:
         // nothing to do

ArduPlane/Attitude.pde

@@ -130,6 +130,7 @@ static void stabilize_stick_mixing_direct()
     if (!stick_mixing_enabled() ||
         control_mode == ACRO ||
         control_mode == FLY_BY_WIRE_A ||
+        control_mode == AUTOTUNE ||
         control_mode == FLY_BY_WIRE_B ||
         control_mode == CRUISE ||
         control_mode == TRAINING) {
@@ -148,6 +149,7 @@ static void stabilize_stick_mixing_fbw()
     if (!stick_mixing_enabled() ||
         control_mode == ACRO ||
         control_mode == FLY_BY_WIRE_A ||
+        control_mode == AUTOTUNE ||
         control_mode == FLY_BY_WIRE_B ||
         control_mode == CRUISE ||
         control_mode == TRAINING ||
@@ -842,7 +844,8 @@ static void set_servos(void)
                    (control_mode == STABILIZE || 
                     control_mode == TRAINING ||
                     control_mode == ACRO ||
-                    control_mode == FLY_BY_WIRE_A)) {
+                    control_mode == FLY_BY_WIRE_A ||
+                    control_mode == AUTOTUNE)) {
             // manual pass through of throttle while in FBWA or
             // STABILIZE mode with THR_PASS_STAB set
             channel_throttle->radio_out = channel_throttle->radio_in;

ArduPlane/GCS_Mavlink.pde

@@ -51,6 +51,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
         break;
     case STABILIZE:
     case FLY_BY_WIRE_A:
+    case AUTOTUNE:
     case FLY_BY_WIRE_B:
     case CRUISE:
         base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
@@ -174,6 +175,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
 
     case STABILIZE:
     case FLY_BY_WIRE_A:
+    case AUTOTUNE:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
         break;
@@ -1339,6 +1341,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         case TRAINING:
         case ACRO:
         case FLY_BY_WIRE_A:
+        case AUTOTUNE:
         case FLY_BY_WIRE_B:
         case CRUISE:
         case AUTO:

ArduPlane/Parameters.pde

@@ -471,7 +471,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: FLTMODE1
     // @DisplayName: FlightMode1
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
     // @User: Standard
     // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
     GSCALAR(flight_mode1,           "FLTMODE1",       FLIGHT_MODE_1),
@@ -479,35 +479,35 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FLTMODE2
     // @DisplayName: FlightMode2
     // @Description: Flight mode for switch position 2 (1231 to 1360)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
     // @User: Standard
     GSCALAR(flight_mode2,           "FLTMODE2",       FLIGHT_MODE_2),
 
     // @Param: FLTMODE3
     // @DisplayName: FlightMode3
     // @Description: Flight mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
     // @User: Standard
     GSCALAR(flight_mode3,           "FLTMODE3",       FLIGHT_MODE_3),
 
     // @Param: FLTMODE4
     // @DisplayName: FlightMode4
     // @Description: Flight mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
     // @User: Standard
     GSCALAR(flight_mode4,           "FLTMODE4",       FLIGHT_MODE_4),
 
     // @Param: FLTMODE5
     // @DisplayName: FlightMode5
     // @Description: Flight mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
     // @User: Standard
     GSCALAR(flight_mode5,           "FLTMODE5",       FLIGHT_MODE_5),
 
     // @Param: FLTMODE6
     // @DisplayName: FlightMode6
     // @Description: Flight mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,10:Auto,11:RTL,12:Loiter,15:Guided
+    // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,15:Guided
     // @User: Standard
     GSCALAR(flight_mode6,           "FLTMODE6",       FLIGHT_MODE_6),
 

ArduPlane/control_modes.pde

@@ -79,3 +79,20 @@ static void reset_control_switch()
     read_control_switch();
 }
 
+/*
+  called when entering autotune
+ */
+static void autotune_start(void)
+{
+    rollController.autotune_start();
+    pitchController.autotune_start();
+}
+
+/*
+  called when exiting autotune
+ */
+static void autotune_restore(void)
+{
+    rollController.autotune_restore();
+    pitchController.autotune_restore();
+}

ArduPlane/defines.h

@@ -62,6 +62,7 @@ enum FlightMode {
     FLY_BY_WIRE_A = 5,
     FLY_BY_WIRE_B = 6,
     CRUISE        = 7,
+    AUTOTUNE      = 8,
     AUTO          = 10,
     RTL           = 11,
     LOITER        = 12,

ArduPlane/events.pde

@@ -13,6 +13,7 @@ static void failsafe_short_on_event(enum failsafe_state fstype)
     case STABILIZE:
     case ACRO:
     case FLY_BY_WIRE_A:
+    case AUTOTUNE:
     case FLY_BY_WIRE_B:
     case CRUISE:
     case TRAINING:
@@ -60,6 +61,7 @@ static void failsafe_long_on_event(enum failsafe_state fstype)
     case STABILIZE:
     case ACRO:
     case FLY_BY_WIRE_A:
+    case AUTOTUNE:
     case FLY_BY_WIRE_B:
     case CRUISE:
     case TRAINING:

ArduPlane/setup.pde

@@ -310,6 +310,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
                 mode != TRAINING &&
                 mode != ACRO &&
                 mode != FLY_BY_WIRE_A &&
+                mode != AUTOTUNE &&
                 mode != FLY_BY_WIRE_B &&
                 mode != CRUISE &&
                 mode != AUTO &&

ArduPlane/system.pde

@@ -292,36 +292,55 @@ static void set_mode(enum FlightMode mode)
     previous_mode = control_mode;
     control_mode = mode;
 
+    if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
+        // restore last gains
+        autotune_restore();
+    }
+
     switch(control_mode)
     {
     case INITIALISING:
+        auto_throttle_mode = true;
+        break;
+
     case MANUAL:
     case STABILIZE:
     case TRAINING:
     case FLY_BY_WIRE_A:
+        auto_throttle_mode = false;
+        break;
+
+    case AUTOTUNE:
+        auto_throttle_mode = false;
+        autotune_start();
         break;
 
     case ACRO:
+        auto_throttle_mode = false;
         acro_state.locked_roll = false;
         acro_state.locked_pitch = false;
         break;
 
     case CRUISE:
+        auto_throttle_mode = true;
         cruise_state.locked_heading = false;
         cruise_state.lock_timer_ms = 0;
         target_altitude_cm = current_loc.alt;
         break;
 
     case FLY_BY_WIRE_B:
+        auto_throttle_mode = true;
         target_altitude_cm = current_loc.alt;
         break;
 
     case CIRCLE:
         // the altitude to circle at is taken from the current altitude
+        auto_throttle_mode = true;
         next_WP_loc.alt = current_loc.alt;
         break;
 
     case AUTO:
+        auto_throttle_mode = true;
         prev_WP_loc = current_loc;
         // start the mission. Note that we use resume(), not start(),
         // as the correct behaviour for plane when entering auto is to
@@ -332,34 +351,25 @@ static void set_mode(enum FlightMode mode)
         break;
 
     case RTL:
+        auto_throttle_mode = true;
         prev_WP_loc = current_loc;
         do_RTL();
         break;
 
     case LOITER:
+        auto_throttle_mode = true;
         do_loiter_at_location();
         break;
 
     case GUIDED:
+        auto_throttle_mode = true;
         guided_throttle_passthru = false;
         set_guided_WP();
         break;
-
-    default:
-        prev_WP_loc = current_loc;
-        do_RTL();
-        break;
     }
 
-    // if in an auto-throttle mode, start with throttle suppressed for
+    // start with throttle suppressed in auto_throttle modes
-    // safety. suppress_throttle() will unsupress it when appropriate
+    throttle_suppressed = auto_throttle_mode;
-    if (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) {
-        auto_throttle_mode = true;
-        throttle_suppressed = true;
-    } else {
-        auto_throttle_mode = false;        
-        throttle_suppressed = false;
-    }
 
     if (should_log(MASK_LOG_MODE))
         Log_Write_Mode(control_mode);
@@ -565,6 +575,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case FLY_BY_WIRE_A:
         port->print_P(PSTR("FBW_A"));
         break;
+    case AUTOTUNE:
+        port->print_P(PSTR("AUTOTUNE"));
+        break;
     case FLY_BY_WIRE_B:
         port->print_P(PSTR("FBW_B"));
         break;