diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 02999245c6..e7c3f3da30 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -737,6 +737,14 @@ void Plane::set_servos_idle(void) channel_throttle->output_trim(); } +/* + return minimum throttle, taking account of throttle reversal + */ +uint16_t Plane::throttle_min(void) const +{ + return channel_throttle->get_reverse() ? channel_throttle->radio_max : channel_throttle->radio_min; +}; + /***************************************** * Set the flight control servos based on the current calculated values @@ -1004,7 +1012,7 @@ void Plane::set_servos(void) case AP_Arming::YES_MIN_PWM: default: - channel_throttle->radio_out = channel_throttle->radio_min; + channel_throttle->radio_out = throttle_min(); break; } } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 37b42417a5..3e3b5f22a6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -956,7 +956,8 @@ private: uint32_t micros() const; void init_capabilities(void); void dataflash_periodic(void); - + uint16_t throttle_min(void) const; + public: void mavlink_delay_cb(); void failsafe_check(void); diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp index 3fe4576320..b821e480bf 100644 --- a/ArduPlane/px4_mixer.cpp +++ b/ArduPlane/px4_mixer.cpp @@ -259,7 +259,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 - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_min); + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); } // we need to force safety on to allow us to load a mixer. We call diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 779033bf91..f17b6ad124 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -28,7 +28,7 @@ void Plane::set_control_channels(void) channel_throttle->set_range(0, 100); if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_min); + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); } // setup correct scaling for ESCs like the UAVCAN PX4ESC which @@ -63,7 +63,7 @@ void Plane::init_rc_out() configuration error where the user sets CH3_TRIM incorrectly and the motor may start on power up */ - channel_throttle->radio_trim = channel_throttle->get_reverse() ? channel_throttle->radio_max : channel_throttle->radio_min; + channel_throttle->radio_trim = throttle_min(); if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) { channel_throttle->enable_out(); @@ -80,7 +80,7 @@ void Plane::init_rc_out() // 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) { - hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_min); + hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min()); } }