From 355df4000a81dbc502d4ab863182a9d761970e52 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 17 Jul 2023 18:15:19 +1000 Subject: [PATCH] ArduPlane: add compile-time option to remove inverted-flight capability --- ArduPlane/Attitude.cpp | 6 ++++++ ArduPlane/Plane.h | 4 ++++ ArduPlane/RC_Channel.cpp | 4 ++++ ArduPlane/commands_logic.cpp | 4 ++++ ArduPlane/control_modes.cpp | 2 ++ ArduPlane/mode.cpp | 4 +++- ArduPlane/mode_fbwa.cpp | 2 ++ ArduPlane/mode_training.cpp | 2 ++ ArduPlane/tailsitter.cpp | 6 ++++++ ArduPlane/takeoff.cpp | 6 +++++- 10 files changed, 38 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0996a50b725ab4..6a8687966cad67 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 189189e9dcb844..20c86bcb8fec81 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 1fc5b0ad222a19..90fba4d4d37ab8 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 215b4cf05e20d1..0b02169a41411e 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 7f15e0e2663f90..9664e028ab2257 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 30ec5fc5b0fb1b..401b008af9933e 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 ff53a835ee49ac..e2ad66134cddba 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 eb7040ce6fadea..0063e0f008927c 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 4e28d534440a3e..0e64f9da9a055e 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 faa220412fd914..dc39ca95e1c20c 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;