diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 8669a78f96..546ba3db08 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1137,15 +1137,19 @@ static void update_current_flight_mode(void) // Thanks to Yury MonZon for the altitude limit code! nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd; - altitude_error_cm = g.channel_pitch.norm_input() * g.pitch_limit_min_cd; + + float elevator_input; + elevator_input = g.channel_pitch.norm_input(); + if (g.flybywire_elev_reverse) { + elevator_input = -elevator_input; + } if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) { - altitude_error_cm = g.channel_pitch.norm_input() * g.pitch_limit_min_cd; + altitude_error_cm = elevator_input * g.pitch_limit_min_cd; } else { - if (g.channel_pitch.norm_input()<0) { - altitude_error_cm =( (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min_cd; - } else { - altitude_error_cm =( (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt); + altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt; + if (elevator_input < 0) { + altitude_error_cm += elevator_input * g.pitch_limit_min_cd; } } calc_throttle(); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c720c26ee0..0f92432d4b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -87,6 +87,7 @@ public: k_param_flybywire_airspeed_min = 120, k_param_flybywire_airspeed_max, k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm) + k_param_flybywire_elev_reverse, // // 130: Sensor parameters @@ -253,6 +254,7 @@ public: // AP_Int16 flybywire_airspeed_min; AP_Int16 flybywire_airspeed_max; + AP_Int8 flybywire_elev_reverse; // Throttle // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 8eab1265d3..52976b3dca 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -199,6 +199,13 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX), + // @Param: FBWB_ELEV_REV + // @DisplayName: Fly By Wire elevator reverse + // @Description: Reverse sense of elevator in FBWB. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude. + // @Values: 0:Disabled,1:Enabled + // @User: Standard + GSCALAR(flybywire_elev_reverse, "FBWB_ELEV_REV", 0), + // @Param: THR_MIN // @DisplayName: Minimum Throttle // @Description: The minimum throttle setting to which the autopilot will apply.