diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 05cf9c28d1..a58dbe1747 100644 --- a/ArduPlane/ArduPlane.pde +++ b/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) { diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 8bdb7b4461..6303d2c47a 100644 --- a/ArduPlane/Attitude.pde +++ b/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) { diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 8e6dadab8f..4aa381d8c4 100644 --- a/ArduPlane/commands_logic.pde +++ b/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: diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index d26a544bc9..4a5fe9e210 100644 --- a/ArduPlane/control_modes.pde +++ b/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; +} diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 54a5397d25..cdb172bc97 100644 --- a/ArduPlane/system.pde +++ b/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;