diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0996a50b725ab..6a8687966cad6 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -110,6 +110,7 @@ bool Plane::stick_mixing_enabled(void) */ void Plane::stabilize_roll() { +#if AP_INVERTED_FLIGHT_ENABLED 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 @@ -119,6 +120,7 @@ void Plane::stabilize_roll() nav_roll_cd += 18000; if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; } +#endif const float roll_out = stabilize_roll_get_roll_out(); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out); @@ -306,9 +308,11 @@ void Plane::stabilize_stick_mixing_fbw() } else if (pitch_input < -0.5f) { pitch_input = (3*pitch_input + 1); } +#if AP_INVERTED_FLIGHT_ENABLED if (fly_inverted()) { pitch_input = -pitch_input; } +#endif if (pitch_input > 0) { nav_pitch_cd += pitch_input * aparm.pitch_limit_max*100; } else { @@ -641,10 +645,12 @@ void Plane::update_load_factor(void) // stall prevention is disabled return; } +#if AP_INVERTED_FLIGHT_ENABLED if (fly_inverted()) { // no roll limits when inverted return; } +#endif #if HAL_QUADPLANE_ENABLED if (quadplane.tailsitter.active()) { // no limits while hovering diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 71b450256c4d9..c119b3a02b316 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -314,8 +314,10 @@ class Plane : public AP_Vehicle { // This is set to 254 when we need to re-read the switch uint8_t oldSwitchPosition = 254; +#if AP_INVERTED_FLIGHT_ENABLED // This is used to enable the inverted flight feature bool inverted_flight; +#endif // last time we ran roll/pitch stabilization uint32_t last_stabilize_ms; @@ -489,8 +491,10 @@ class Plane : public AP_Vehicle { // are we headed to the land approach waypoint? Works for any nav type bool wp_is_land_approach; +#if AP_INVERTED_FLIGHT_ENABLED // should we fly inverted? bool inverted_flight; +#endif // should we enable cross-tracking for the next waypoint? bool next_wp_crosstrack; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 1fc5b0ad222a1..90fba4d4d37ab 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -152,7 +152,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, case AUX_FUNC::TRAINING: case AUX_FUNC::FLAP: case AUX_FUNC::GUIDED: +#if AP_INVERTED_FLIGHT_ENABLED case AUX_FUNC::INVERTED: +#endif case AUX_FUNC::LOITER: case AUX_FUNC::MANUAL: case AUX_FUNC::RTL: @@ -216,9 +218,11 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag) { switch(ch_option) { +#if AP_INVERTED_FLIGHT_ENABLED case AUX_FUNC::INVERTED: plane.inverted_flight = (ch_flag == AuxSwitchPos::HIGH); break; +#endif case AUX_FUNC::REVERSE_THROTTLE: plane.reversed_throttle = (ch_flag == AuxSwitchPos::HIGH); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 215b4cf05e20d..0b02169a41411 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -134,12 +134,14 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) do_set_home(cmd); break; +#if AP_INVERTED_FLIGHT_ENABLED case MAV_CMD_DO_INVERTED_FLIGHT: if (cmd.p1 == 0 || cmd.p1 == 1) { auto_state.inverted_flight = (bool)cmd.p1; gcs().send_text(MAV_SEVERITY_INFO, "Set inverted %u", cmd.p1); } break; +#endif case MAV_CMD_DO_LAND_START: break; @@ -319,7 +321,9 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: +#if AP_INVERTED_FLIGHT_ENABLED case MAV_CMD_DO_INVERTED_FLIGHT: +#endif case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_FENCE_ENABLE: case MAV_CMD_DO_AUTOTUNE_ENABLE: diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 7f15e0e2663f9..9664e028ab225 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -169,6 +169,7 @@ void Plane::autotune_enable(bool enable) } } +#if AP_INVERTED_FLIGHT_ENABLED /* are we flying inverted? */ @@ -186,3 +187,4 @@ bool Plane::fly_inverted(void) } return false; } +#endif diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 30ec5fc5b0fb1..401b008af9933 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -30,9 +30,11 @@ bool Mode::enter() plane.nav_scripting.enabled = false; #endif +#if AP_INVERTED_FLIGHT_ENABLED // cancel inverted flight plane.auto_state.inverted_flight = false; - +#endif + // cancel waiting for rudder neutral plane.takeoff_state.waiting_for_rudder_neutral = false; diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index ff53a835ee49a..e2ad66134cddb 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -14,9 +14,11 @@ void ModeFBWA::update() } plane.adjust_nav_pitch_throttle(); plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); +#if AP_INVERTED_FLIGHT_ENABLED if (plane.fly_inverted()) { plane.nav_pitch_cd = -plane.nav_pitch_cd; } +#endif if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) { // FBWA failsafe glide plane.nav_roll_cd = 0; diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index eb7040ce6fade..0063e0f008927 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -28,9 +28,11 @@ void ModeTraining::update() plane.training_manual_pitch = true; plane.nav_pitch_cd = 0; } +#if AP_INVERTED_FLIGHT_ENABLED if (plane.fly_inverted()) { plane.nav_pitch_cd = -plane.nav_pitch_cd; } +#endif } /* diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 4e28d534440a3..0e64f9da9a055 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -557,9 +557,11 @@ bool Tailsitter::transition_vtol_complete(void) const return true; } int32_t roll_cd = labs(plane.ahrs.roll_sensor); +#if AP_INVERTED_FLIGHT_ENABLED if (plane.fly_inverted()) { roll_cd = 18000 - roll_cd; } +#endif if (roll_cd > MAX(4500, plane.roll_limit_cd + 500)) { gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, roll error"); return true; @@ -845,7 +847,11 @@ void Tailsitter_Transition::update() quadplane.assisted_flight = true; uint32_t dt = now - fw_transition_start_ms; // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees +#if AP_INVERTED_FLIGHT_ENABLED plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500); +#else + plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f, -8500, 8500); +#endif plane.nav_roll_cd = 0; quadplane.disable_yaw_rate_time_constant(); quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index faa220412fd91..dc39ca95e1c20 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -107,7 +107,11 @@ bool Plane::auto_takeoff_check(void) if (do_takeoff_attitude_check) { // Check aircraft attitude for bad launch if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 || - (!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) { + ( +#if AP_INVERTED_FLIGHT_ENABLED + !fly_inverted() && +#endif + labs(ahrs.roll_sensor) > 3000)) { gcs().send_text(MAV_SEVERITY_WARNING, "Bad launch AUTO"); takeoff_state.accel_event_counter = 0; goto no_launch;