Plane: added MAV_CMD_DO_INVERTED_FLIGHT support

allows for mission control over inverted flight

ArduPlane/ArduPlane.pde

@@ -536,6 +536,9 @@ static struct {
 
     // turn angle for next leg of mission
     float next_turn_angle;
+
+    // should we fly inverted?
+    bool inverted_flight;
 } auto_state = {
     takeoff_complete : true,
     land_complete : false,
@@ -543,7 +546,8 @@ static struct {
     takeoff_pitch_cd : 0,
     highest_airspeed : 0,
     initial_pitch_cd : 0,
-    next_turn_angle  : 90.0f
+    next_turn_angle  : 90.0f,
+    inverted_flight  : false
 };
 
 // true if we are in an auto-throttle mode, which means
@@ -1218,7 +1222,7 @@ static void update_flight_mode(void)
             training_manual_pitch = true;
             nav_pitch_cd = 0;
         }
-        if (inverted_flight) {
+        if (fly_inverted()) {
             nav_pitch_cd = -nav_pitch_cd;
         }
         break;
@@ -1251,7 +1255,7 @@ static void update_flight_mode(void)
             nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
         }
         nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
-        if (inverted_flight) {
+        if (fly_inverted()) {
             nav_pitch_cd = -nav_pitch_cd;
         }
         if (failsafe.ch3_failsafe && g.short_fs_action == 2) {

ArduPlane/Attitude.pde

@@ -71,7 +71,7 @@ static bool stick_mixing_enabled(void)
  */
 static void stabilize_roll(float speed_scaler)
 {
-    if (inverted_flight) {
+    if (fly_inverted()) {
         // we want to fly upside down. We need to cope with wrap of
         // the roll_sensor interfering with wrap of nav_roll, which
         // would really confuse the PID code. The easiest way to
@@ -185,7 +185,7 @@ static void stabilize_stick_mixing_fbw()
     if (fabsf(pitch_input) > 0.5f) {
         pitch_input = (3*pitch_input - 1);
     }
-    if (inverted_flight) {
+    if (fly_inverted()) {
         pitch_input = -pitch_input;
     }
     if (pitch_input > 0) {

ArduPlane/commands_logic.pde

@@ -114,6 +114,13 @@ start_command(const AP_Mission::Mission_Command& cmd)
                                          cmd.content.repeat_relay.cycle_time * 1000.0f);
         break;
 
+    case MAV_CMD_DO_INVERTED_FLIGHT:
+        if (cmd.p1 == 0 || cmd.p1 == 1) {
+            auto_state.inverted_flight = (bool)cmd.p1;
+            gcs_send_text_fmt(PSTR("Set inverted %u"), cmd.p1);
+        }
+        break;
+
 #if CAMERA == ENABLED
     case MAV_CMD_DO_CONTROL_VIDEO:                      // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
         break;
@@ -219,6 +226,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_NAV_ROI:
     case MAV_CMD_DO_MOUNT_CONFIGURE:
     case MAV_CMD_DO_MOUNT_CONTROL:
+    case MAV_CMD_DO_INVERTED_FLIGHT:
         return true;
 
     default:

ArduPlane/control_modes.pde

@@ -96,3 +96,18 @@ static void autotune_restore(void)
     rollController.autotune_restore();
     pitchController.autotune_restore();
 }
+
+/*
+  are we flying inverted?
+ */
+static bool fly_inverted(void)
+{
+    if (g.inverted_flight_ch != 0 && inverted_flight) {
+        // controlled with INVERTED_FLIGHT_CH
+        return true;
+    }
+    if (control_mode == AUTO && auto_state.inverted_flight) {
+        return true;
+    }
+    return false;
+}

ArduPlane/system.pde

@@ -282,6 +282,9 @@ static void set_mode(enum FlightMode mode)
     // perform any cleanup required for prev flight mode
     exit_mode(control_mode);
 
+    // cancel inverted flight
+    auto_state.inverted_flight = false;
+
     // set mode
     previous_mode = control_mode;
     control_mode = mode;