diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 16944a7f25..94b08030c9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -91,7 +91,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Param: RSC_MODE // @DisplayName: Rotor Speed Control Mode // @Description: Controls the source of the desired rotor speed, either ch8 or RSC_SETPOINT - // @Values: 0:None, 1:Ch8 Input, 2:SetPoint + // @Values: 1:Ch8 Input, 2:SetPoint // @User: Standard AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), @@ -218,12 +218,6 @@ bool AP_MotorsHeli::parameter_check() const return true; } -// return true if the main rotor is up to speed -bool AP_MotorsHeli::rotor_runup_complete() const -{ - return _heliflags.rotor_runup_complete; -} - // reset_swash - free up swash for maximum movements. Used for set-up void AP_MotorsHeli::reset_swash() { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 21ff0c8ed2..c143d8f557 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -37,7 +37,7 @@ #define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0 // main rotor speed control types (ch8 out) -#define AP_MOTORS_HELI_RSC_MODE_NONE 0 // main rotor ESC is directly connected to receiver, pilot controls ESC speed through transmitter directly +#define AP_MOTORS_HELI_RSC_MODE_NONE 0 // not a valid RSC Mode #define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1 // main rotor ESC is connected to RC8 (out), pilot desired rotor speed provided by CH8 input #define AP_MOTORS_HELI_RSC_MODE_SETPOINT 2 // main rotor ESC is connected to RC8 (out), desired speed is held in RSC_SETPOINT parameter @@ -136,7 +136,7 @@ public: virtual int16_t get_estimated_rotor_speed() const = 0; // return true if the main rotor is up to speed - bool rotor_runup_complete() const; + bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; } // rotor_speed_above_critical - return true if rotor speed is above that critical for flight virtual bool rotor_speed_above_critical() const = 0; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index f499721ed2..e721333e2e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -195,7 +195,7 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm) bool AP_MotorsHeli_Single::allow_arming() const { // returns false if main rotor speed is not zero - if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) { + if (_main_rotor.get_estimated_speed() > 0) { return false; } @@ -220,18 +220,11 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed) // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters void AP_MotorsHeli_Single::recalc_scalers() { - if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { - _main_rotor.set_ramp_time(0); - _main_rotor.set_runup_time(0); - _main_rotor.set_critical_speed(0); - _main_rotor.set_idle_speed(0); - } else { - _main_rotor.set_ramp_time(_rsc_ramp_time); - _main_rotor.set_runup_time(_rsc_runup_time); - _main_rotor.set_critical_speed(_rsc_critical); - _main_rotor.set_idle_speed(_rsc_idle); - } - + + _main_rotor.set_ramp_time(_rsc_ramp_time); + _main_rotor.set_runup_time(_rsc_runup_time); + _main_rotor.set_critical_speed(_rsc_critical); + _main_rotor.set_idle_speed(_rsc_idle); _main_rotor.recalc_scalers(); if (_rsc_mode != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {