diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 825fbeaa68..a249885c21 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -501,6 +501,8 @@ private: // governor. Even a single "off" frame can cause the rotor to slow dramatically and take a long time to restart. ModeFilterInt16_Size5 rotor_speed_deglitch_filter {4}; + int16_t rsc_control_deglitched; + // Tradheli flags struct { uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 6ddee2c534..b2b2a818f3 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -137,24 +137,30 @@ void Copter::heli_update_rotor_speed_targets() // get rotor control method uint8_t rsc_control_mode = motors.get_rsc_mode(); - int16_t rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in); - switch (rsc_control_mode) { - case AP_MOTORS_HELI_RSC_MODE_NONE: - // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if - // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time - case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH: - // pass through pilot desired rotor speed - motors.set_desired_rotor_speed(rsc_control_deglitched); - break; - case AP_MOTORS_HELI_RSC_MODE_SETPOINT: - // pass setpoint through as desired rotor speed - if (rsc_control_deglitched > 0) { - motors.set_desired_rotor_speed(motors.get_rsc_setpoint()); - }else{ - motors.set_desired_rotor_speed(0); - } - break; + rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in); + + if (motors.armed()) { + switch (rsc_control_mode) { + case AP_MOTORS_HELI_RSC_MODE_NONE: + // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if + // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time + case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH: + // pass through pilot desired rotor speed + motors.set_desired_rotor_speed(rsc_control_deglitched); + break; + case AP_MOTORS_HELI_RSC_MODE_SETPOINT: + // pass setpoint through as desired rotor speed + if (rsc_control_deglitched > 0) { + motors.set_desired_rotor_speed(motors.get_rsc_setpoint()); + }else{ + motors.set_desired_rotor_speed(0); + } + break; + } + } else { + // if disarmed, force desired_rotor_speed to Zero + motors.set_desired_rotor_speed(0); } // when rotor_runup_complete changes to true, log event diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 264ae69a4e..99e879c46f 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -702,15 +702,15 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) } // always check if rotor is spinning on heli - #if FRAME_CONFIG == HELI_FRAME +#if FRAME_CONFIG == HELI_FRAME // heli specific arming check - if (!motors.allow_arming()){ + if ((rsc_control_deglitched > 0) || !motors.allow_arming()){ if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor Control Engaged")); } return false; } - #endif // HELI_FRAME +#endif // HELI_FRAME // succeed if arming checks are disabled if (g.arming_check == ARMING_CHECK_NONE) {