diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 959741eb6b..e70cd9df09 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -859,7 +859,7 @@ void Plane::update_flight_stage(void) if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); - } else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90 && + } else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 && landing.request_go_around()) { gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 08de06cfb1..de893f8da2 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -411,7 +411,7 @@ void Plane::stabilize() /* see if we should zero the attitude controller integrators. */ - if (channel_throttle->get_control_in() == 0 && + if (get_throttle_input() == 0 && fabsf(relative_altitude) < 5.0f && fabsf(barometer.get_climb_rate()) < 0.5f && gps.ground_speed() < 3) { @@ -507,7 +507,7 @@ void Plane::calc_nav_yaw_course(void) void Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && - channel_throttle->get_control_in() == 0 && + get_throttle_input() == 0 && flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { // manual rudder control while still @@ -583,80 +583,6 @@ void Plane::calc_nav_roll() update_load_factor(); } - -bool Plane::allow_reverse_thrust(void) -{ - // check if we should allow reverse thrust - bool allow = false; - - if (g.use_reverse_thrust == USE_REVERSE_THRUST_NEVER) { - return false; - } - - switch (control_mode) { - case AUTO: - { - uint16_t nav_cmd = mission.get_current_nav_cmd().id; - - // never allow reverse thrust during takeoff - if (nav_cmd == MAV_CMD_NAV_TAKEOFF) { - return false; - } - - // always allow regardless of mission item - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_ALWAYS); - - // landing - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LAND_APPROACH) && - (nav_cmd == MAV_CMD_NAV_LAND); - - // LOITER_TO_ALT - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_TO_ALT) && - (nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT); - - // any Loiter (including LOITER_TO_ALT) - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_ALL) && - (nav_cmd == MAV_CMD_NAV_LOITER_TIME || - nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT || - nav_cmd == MAV_CMD_NAV_LOITER_TURNS || - nav_cmd == MAV_CMD_NAV_LOITER_UNLIM); - - // waypoints - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_WAYPOINT) && - (nav_cmd == MAV_CMD_NAV_WAYPOINT || - nav_cmd == MAV_CMD_NAV_SPLINE_WAYPOINT); - } - break; - - case LOITER: - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_LOITER); - break; - case RTL: - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_RTL); - break; - case CIRCLE: - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE); - break; - case CRUISE: - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CRUISE); - break; - case FLY_BY_WIRE_B: - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB); - break; - case AVOID_ADSB: - case GUIDED: - allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED); - break; - default: - // all other control_modes are auto_throttle_mode=false. - // If we are not controlling throttle, don't limit it. - allow = true; - break; - } - - return allow; -} - /* adjust nav_pitch_cd for STAB_PITCH_DOWN_CD. This is used to make keeping up good airspeed in FBWA mode easier, as the plane will diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index cfe10d1228..696c9962be 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -993,7 +993,6 @@ private: void servos_auto_trim(void); void servos_twin_engine_mix(); void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); - bool allow_reverse_thrust(void); void update_is_flying_5Hz(void); void crash_detection_update(void); bool in_preLaunch_flight_stage(void); @@ -1066,6 +1065,10 @@ private: void read_aux_all(); + bool allow_reverse_thrust(void) const; + bool have_reverse_thrust(void) const; + int16_t get_throttle_input(bool no_deadzone=false) const; + // support for AP_Avoidance custom flight mode, AVOID_ADSB bool avoid_adsb_init(bool ignore_checks); void avoid_adsb_run(); diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 548e7e7b37..773cba53ca 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -26,7 +26,7 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX); - if (plane.aparm.throttle_min < 0) { + if (plane.have_reverse_thrust()) { // configured for reverse thrust, use TRIM SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); @@ -57,7 +57,7 @@ void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX); - if (plane.aparm.throttle_min < 0) { + if (plane.have_reverse_thrust()) { // configured for reverse thrust, use TRIM SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 7d7eb54448..6294f1c42b 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -57,7 +57,7 @@ void Plane::failsafe_check(void) int16_t roll = channel_roll->get_control_in_zero_dz(); int16_t pitch = channel_pitch->get_control_in_zero_dz(); - int16_t throttle = channel_throttle->get_control_in_zero_dz(); + int16_t throttle = get_throttle_input(true); int16_t rudder = channel_rudder->get_control_in_zero_dz(); if (!hal.util->get_soft_armed()) { diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index b8f3f60a05..24aaabc2b6 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -55,7 +55,7 @@ void QuadPlane::motor_test_output() break; case MOTOR_TEST_THROTTLE_PILOT: - pwm = thr_min_pwm + (thr_max_pwm - thr_min_pwm) * (float)plane.channel_throttle->get_control_in()*0.01f; + pwm = thr_min_pwm + (thr_max_pwm - thr_min_pwm) * (float)plane.get_throttle_input()*0.01f; break; default: diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 835c9a42d2..38e4709026 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -113,7 +113,7 @@ void Plane::calc_airspeed_errors() float control_min = 0.0f; float control_mid = 0.0f; const float control_max = channel_throttle->get_range(); - const float control_in = channel_throttle->get_control_in(); + const float control_in = get_throttle_input(); switch (channel_throttle->get_type()) { case RC_Channel::RC_CHANNEL_TYPE_ANGLE: control_min = -control_max; @@ -133,7 +133,7 @@ void Plane::calc_airspeed_errors() } } else { target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) * - channel_throttle->get_control_in()) + ((int32_t)aparm.airspeed_min * 100); + get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100); } } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp index 4a45dfe64f..a7049e866d 100644 --- a/ArduPlane/px4_mixer.cpp +++ b/ArduPlane/px4_mixer.cpp @@ -300,7 +300,7 @@ bool Plane::setup_failsafe_mixing(void) if (old_state == AP_HAL::Util::SAFETY_ARMED) { // make sure the throttle has a non-zero failsafe value before we // disable safety. This prevents sending zero PWM during switch over - SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } // we need to force safety on to allow us to load a mixer. We call diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 065f50f906..f584fbf3ee 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -680,7 +680,7 @@ void QuadPlane::run_esc_calibration(void) switch (esc_calibration) { case 1: // throttle based calibration - motors->set_throttle_passthrough_for_esc_calibration(plane.channel_throttle->get_control_in() * 0.01f); + motors->set_throttle_passthrough_for_esc_calibration(plane.get_throttle_input() * 0.01f); break; case 2: // full range calibration @@ -748,7 +748,7 @@ void QuadPlane::control_stabilize(void) } // normal QSTABILIZE mode - float pilot_throttle_scaled = plane.channel_throttle->get_control_in() / 100.0f; + float pilot_throttle_scaled = plane.get_throttle_input() / 100.0f; hold_stabilize(pilot_throttle_scaled); } @@ -943,7 +943,7 @@ bool QuadPlane::is_flying_vtol(void) const } if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER) { // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero - return plane.channel_throttle->get_control_in() > 0; + return plane.get_throttle_input() > 0; } if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) { // use landing detector @@ -1052,7 +1052,7 @@ void QuadPlane::control_loiter() */ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { - if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) { + if (plane.get_throttle_input() <= 0 && !plane.auto_throttle_mode) { // the user may be trying to disarm return 0; } @@ -1071,7 +1071,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void) // use bank angle to get desired yaw rate yaw_cds += desired_auto_yaw_rate_cds(); } - if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) { + if (plane.get_throttle_input() <= 0 && !plane.auto_throttle_mode) { // the user may be trying to disarm return 0; } @@ -1102,7 +1102,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const */ void QuadPlane::init_throttle_wait(void) { - if (plane.channel_throttle->get_control_in() >= 10 || + if (plane.get_throttle_input() >= 10 || plane.is_flying()) { throttle_wait = false; } else { @@ -1132,7 +1132,7 @@ float QuadPlane::assist_climb_rate_cms(void) const } else { // otherwise estimate from pilot input climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); - climb_rate *= plane.channel_throttle->get_control_in(); + climb_rate *= plane.get_throttle_input(); } climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up()); @@ -1256,7 +1256,7 @@ void QuadPlane::update_transition(void) !is_tailsitter() && hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) || - plane.channel_throttle->get_control_in()>0 || + plane.get_throttle_input()>0 || plane.is_flying())) { // the quad should provide some assistance to the plane if (transition_state != TRANSITION_AIRSPEED_WAIT) { @@ -1487,7 +1487,7 @@ void QuadPlane::update(void) // disable throttle_wait when throttle rises above 10% if (throttle_wait && - (plane.channel_throttle->get_control_in() > 10 || + (plane.get_throttle_input() > 10 || plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter>0)) { throttle_wait = false; @@ -1517,7 +1517,7 @@ void QuadPlane::check_throttle_suppression(void) } // if the users throttle is above zero then allow motors to run - if (plane.channel_throttle->get_control_in() != 0) { + if (plane.get_throttle_input() != 0) { return; } @@ -2374,7 +2374,7 @@ int8_t QuadPlane::forward_throttle_pct(void) vel_forward.integrator += fwd_vel_error * deltat * vel_forward.gain * 100; // inhibit reverse throttle and allow petrol engines with min > 0 - int8_t fwd_throttle_min = (plane.aparm.throttle_min <= 0) ? 0 : plane.aparm.throttle_min; + int8_t fwd_throttle_min = plane.have_reverse_thrust() ? 0 : plane.aparm.throttle_min; vel_forward.integrator = constrain_float(vel_forward.integrator, fwd_throttle_min, plane.aparm.throttle_max); // If we are below alt_cutoff then scale down the effect until it turns off at alt_cutoff and decay the integrator diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index b86665594c..766cb2ec92 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -23,7 +23,7 @@ void Plane::set_control_channels(void) channel_roll->set_angle(SERVO_MAX); channel_pitch->set_angle(SERVO_MAX); channel_rudder->set_angle(SERVO_MAX); - if (aparm.throttle_min >= 0) { + if (!have_reverse_thrust()) { // normal operation channel_throttle->set_range(100); } else { @@ -35,7 +35,7 @@ void Plane::set_control_channels(void) } if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { - SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } if (!quadplane.enable) { @@ -69,7 +69,7 @@ void Plane::init_rc_out_main() configuration error where the user sets CH3_TRIM incorrectly and the motor may start on power up */ - if (aparm.throttle_min >= 0) { + if (!have_reverse_thrust()) { SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle); } @@ -81,7 +81,7 @@ void Plane::init_rc_out_main() // setup PX4 to output the min throttle when safety off if arming // is setup for min on disarm if (arming.arming_required() == AP_Arming::YES_MIN_PWM) { - SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, aparm.throttle_min<0?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::SRV_CHANNEL_LIMIT_TRIM:SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } } @@ -114,7 +114,7 @@ void Plane::rudder_arm_disarm_check() } // if throttle is not down, then pilot cannot rudder arm/disarm - if (channel_throttle->get_control_in() != 0){ + if (get_throttle_input() != 0){ rudder_arm_timer = 0; return; } @@ -194,7 +194,7 @@ void Plane::read_radio() control_failsafe(); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input()); if (g.throttle_nudge && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 50 && geofence_stickmixing()) { float nudge = (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) - 50) * 0.02f; diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp new file mode 100644 index 0000000000..ba9a8ecdc6 --- /dev/null +++ b/ArduPlane/reverse_thrust.cpp @@ -0,0 +1,113 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + reverse thrust support functions + */ +#include "Plane.h" + +/* + see if reverse thrust should be allowed in the current flight state + */ +bool Plane::allow_reverse_thrust(void) const +{ + // check if we should allow reverse thrust + bool allow = false; + + if (g.use_reverse_thrust == USE_REVERSE_THRUST_NEVER || !have_reverse_thrust()) { + return false; + } + + switch (control_mode) { + case AUTO: + { + uint16_t nav_cmd = mission.get_current_nav_cmd().id; + + // never allow reverse thrust during takeoff + if (nav_cmd == MAV_CMD_NAV_TAKEOFF) { + return false; + } + + // always allow regardless of mission item + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_ALWAYS); + + // landing + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LAND_APPROACH) && + (nav_cmd == MAV_CMD_NAV_LAND); + + // LOITER_TO_ALT + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_TO_ALT) && + (nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT); + + // any Loiter (including LOITER_TO_ALT) + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_ALL) && + (nav_cmd == MAV_CMD_NAV_LOITER_TIME || + nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT || + nav_cmd == MAV_CMD_NAV_LOITER_TURNS || + nav_cmd == MAV_CMD_NAV_LOITER_UNLIM); + + // waypoints + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_WAYPOINT) && + (nav_cmd == MAV_CMD_NAV_WAYPOINT || + nav_cmd == MAV_CMD_NAV_SPLINE_WAYPOINT); + } + break; + + case LOITER: + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_LOITER); + break; + case RTL: + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_RTL); + break; + case CIRCLE: + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE); + break; + case CRUISE: + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CRUISE); + break; + case FLY_BY_WIRE_B: + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB); + break; + case AVOID_ADSB: + case GUIDED: + allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED); + break; + default: + // all other control_modes are auto_throttle_mode=false. + // If we are not controlling throttle, don't limit it. + allow = true; + break; + } + + return allow; +} + +/* + return true if we are configured to support reverse thrust + */ +bool Plane::have_reverse_thrust(void) const +{ + return aparm.throttle_min < 0; +} + +/* + return control in from the radio throttle channel. + */ +int16_t Plane::get_throttle_input(bool no_deadzone) const +{ + if (no_deadzone) { + return channel_throttle->get_control_in_zero_dz(); + } + return channel_throttle->get_control_in(); +} diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 5df686d08c..3b885cd96c 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -133,7 +133,7 @@ void Plane::update_sensor_status_flags(void) control_sensors_present |= MAV_SYS_STATUS_GEOFENCE; } - if (aparm.throttle_min < 0) { + if (have_reverse_thrust()) { control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR; } if (plane.DataFlash.logging_present()) { // primary logging only (usually File) @@ -295,7 +295,7 @@ void Plane::update_sensor_status_flags(void) } } - if (aparm.throttle_min < 0 && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) { + if (have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) { control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR; control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 84d431b9ed..277748fe4f 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -257,7 +257,7 @@ void Plane::set_servos_manual_passthrough(void) SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz()); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz()); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, channel_rudder->get_control_in_zero_dz()); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } /* @@ -360,7 +360,7 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } } else if (g.throttle_passthru_stabilize && (control_mode == STABILIZE || @@ -371,11 +371,11 @@ void Plane::set_servos_controlled(void) !failsafe.throttle_counter) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } else if (quadplane.in_vtol_mode()) { // ask quadplane code for forward throttle SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, @@ -542,7 +542,7 @@ void Plane::servos_twin_engine_mix(void) float throttle_left, throttle_right; - if (throttle < 0 && aparm.throttle_min < 0) { + if (throttle < 0 && have_reverse_thrust() && allow_reverse_thrust()) { // doing reverse thrust throttle_left = constrain_float(throttle + 50 * rudder, -100, 0); throttle_right = constrain_float(throttle - 50 * rudder, -100, 0); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 8a7a7dcc5b..b106137df7 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -733,7 +733,7 @@ int8_t Plane::throttle_percentage(void) return quadplane.throttle_percentage(); } float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - if (aparm.throttle_min >= 0) { + if (!have_reverse_thrust()) { return constrain_int16(throttle, 0, 100); } return constrain_int16(throttle, -100, 100); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 65c72d0fb2..8302eb7db1 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -127,7 +127,7 @@ void QuadPlane::tailsitter_output(void) SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.channel_throttle->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); } if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) { SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz());