Copter: tradheli replaces rotor_runup_complete with spool state

ArduCopter/Copter.h

@@ -538,8 +538,7 @@ private:
     // Tradheli flags
     typedef struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
-        uint8_t init_targets_on_arming  : 1;    // 1   // true if we have been disarmed, and need to reset rate controller targets when we arm
-        uint8_t inverted_flight         : 1;    // 2   // true for inverted flight mode
+        uint8_t inverted_flight         : 1;    // 1   // true for inverted flight mode
     } heli_flags_t;
     heli_flags_t heli_flags;
 

ArduCopter/heli.cpp

@@ -23,7 +23,7 @@ void Copter::heli_init()
 // should be called at 50hz
 void Copter::check_dynamic_flight(void)
 {
-    if (!motors->armed() || !motors->rotor_runup_complete() ||
+    if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED ||
         control_mode == LAND || (control_mode==RTL && mode_rtl.state() == RTL_Land) || (control_mode == AUTO && mode_auto.mode() == Auto_Land)) {
         heli_dynamic_flight_counter = 0;
         heli_flags.dynamic_flight = false;

ArduCopter/land_detector.cpp

@@ -47,7 +47,7 @@ void Copter::update_land_detector()
     } else if (ap.land_complete) {
 #if FRAME_CONFIG == HELI_FRAME
         // if rotor speed and collective pitch are high then clear landing flag
-        if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->rotor_runup_complete()) {
+        if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
 #else
         // if throttle output is high then clear landing flag
         if (motors->get_throttle() > get_non_takeoff_throttle()) {

ArduCopter/mode.cpp

@@ -193,7 +193,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter a non-manual throttle mode if the
     // rotor runup is not complete
-    if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()){
+    if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_mode() == AP_Motors::SPOOL_UP || motors->get_spool_mode() == AP_Motors::SPOOL_DOWN)) {
         gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
         AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
         return false;
@@ -381,12 +381,11 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
         // can't takeoff unless we want to go up...
         return false;
     }
-#if FRAME_CONFIG == HELI_FRAME
-    if (!copter.motors->rotor_runup_complete()) {
-        // hold heli on the ground until rotor speed runup has finished
+    if (copter.motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED) {
+        // hold aircraft on the ground until rotor speed runup has finished
         return false;
     }
-#endif
+
     return true;
 }
 

ArduCopter/mode_acro_heli.cpp

@@ -29,21 +29,35 @@ void Copter::ModeAcro_Heli::run()
     float target_roll, target_pitch, target_yaw;
     float pilot_throttle_scaled;
 
-    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
-    // we may be in autorotation flight.  These should be reset only when transitioning from disarmed
-    // to armed, because the pilot will have placed the helicopter down on the landing pad.  This is so
-    // that the servos move in a realistic fashion while disarmed for operational checks.
-    // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
+    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
+    // we may be in autorotation flight.  This is so that the servos move in a realistic fashion while disarmed
+    // for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
+    // so the swash servos move.
+
+    if (!motors->armed()) {
+        // Motors should be Stopped
+        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+    } else {
+        // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
+        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+    }
 
-    if (motors->init_targets_on_arming()) {
+    if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
+        // Motors Stopped
+        attitude_control->set_attitude_target_to_current_attitude();
         attitude_control->reset_rate_controller_I_terms();
+    } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
+        // Landed
+        if (motors->init_targets_on_arming()) {
             attitude_control->set_attitude_target_to_current_attitude();
+            attitude_control->reset_rate_controller_I_terms();
         }
-
+    } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
         // clear landing flag above zero throttle
-    if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
+        if (!motors->limit.throttle_lower) {
             set_land_complete(false);
         }
+    }
 
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 

ArduCopter/mode_brake.cpp

@@ -61,7 +61,12 @@ void Copter::ModeBrake::run()
     // body-frame rate controller is run directly from 100hz loop
 
     // update altitude target and call position controller
+    // protects heli's from inflight motor interlock disable
+    if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {
+        pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
+    } else {
         pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
+    }
     pos_control->update_z_controller();
 
     if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {

ArduCopter/mode_circle.cpp

@@ -85,7 +85,12 @@ void Copter::ModeCircle::run()
     target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
 
     // update altitude target and call position controller
+    // protects heli's from inflight motor interlock disable
+    if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {
+        pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
+    } else {
         pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+    }
     pos_control->update_z_controller();
 }
 

ArduCopter/mode_flowhold.cpp

@@ -84,13 +84,6 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
 // flowhold_init - initialise flowhold controller
 bool Copter::ModeFlowHold::init(bool ignore_checks)
 {
-#if FRAME_CONFIG == HELI_FRAME
-    // do not allow helis to enter Flow Hold if the Rotor Runup is not complete
-    if (!ignore_checks && !motors->rotor_runup_complete()){
-        return false;
-    }
-#endif
-
     if (!copter.optflow.enabled() || !copter.optflow.healthy()) {
         return false;
     }

ArduCopter/mode_guided.cpp

@@ -395,8 +395,8 @@ void Copter::Mode::auto_takeoff_run()
     }
 
 #if FRAME_CONFIG == HELI_FRAME
-    // helicopters stay in landed state until rotor speed runup has finished
-    if (motors->rotor_runup_complete()) {
+    // aircraft stays in landed state until rotor speed runup has finished
+    if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
         set_land_complete(false);
     } else {
         // initialise wpnav targets

ArduCopter/mode_stabilize_heli.cpp

@@ -22,21 +22,6 @@ void Copter::ModeStabilize_Heli::run()
     float target_yaw_rate;
     float pilot_throttle_scaled;
 
-    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
-    // we may be in autorotation flight.  These should be reset only when transitioning from disarmed
-    // to armed, because the pilot will have placed the helicopter down on the landing pad.  This is so
-    // that the servos move in a realistic fashion while disarmed for operational checks.
-    // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
-    
-    if (motors->init_targets_on_arming()) {
-        attitude_control->reset_rate_controller_I_terms();
-        attitude_control->set_yaw_target_to_current_heading();
-    }
-
-    // clear landing flag above zero throttle
-    if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
-        set_land_complete(false);
-    }
     
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
@@ -52,6 +37,36 @@ void Copter::ModeStabilize_Heli::run()
     // get pilot's desired throttle
     pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
 
+    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because
+    // we may be in autorotation flight.  This is so that the servos move in a realistic fashion while disarmed
+    // for operational checks. Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero
+    // so the swash servos move.
+
+    if (!motors->armed()) {
+        // Motors should be Stopped
+        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+    } else {
+        // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
+        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+    }
+
+    if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
+        // Motors Stopped
+        attitude_control->set_yaw_target_to_current_heading();
+        attitude_control->reset_rate_controller_I_terms();
+    } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
+        // Landed
+        if (motors->init_targets_on_arming()) {
+            attitude_control->set_yaw_target_to_current_heading();
+            attitude_control->reset_rate_controller_I_terms();
+        }
+    } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
+        // clear landing flag above zero throttle
+        if (!motors->limit.throttle_lower) {
+            set_land_complete(false);
+        }
+    }
+
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
 

ArduCopter/motors.cpp

@@ -93,13 +93,11 @@ void Copter::auto_disarm_check()
         return;
     }
 
-#if FRAME_CONFIG == HELI_FRAME
     // if the rotor is still spinning, don't initiate auto disarm
-    if (motors->rotor_speed_above_critical()) {
+    if (motors->get_spool_mode() != AP_Motors::GROUND_IDLE) {
         auto_disarm_begin = tnow_ms;
         return;
     }
-#endif
 
     // always allow auto disarm if using interlock switch or motors are Emergency Stopped
     if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_emergency_stop()) {

ArduCopter/system.cpp

@@ -368,28 +368,26 @@ void Copter::update_auto_armed()
         if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) {
             set_auto_armed(false);
         }
-#if FRAME_CONFIG == HELI_FRAME
         // if helicopters are on the ground, and the motor is switched off, auto-armed should be false
         // so that rotor runup is checked again before attempting to take-off
-        if(ap.land_complete && !motors->rotor_runup_complete()) {
+        if(ap.land_complete && motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) {
             set_auto_armed(false);
         }
-#endif // HELI_FRAME
     }else{
         // arm checks
         
-#if FRAME_CONFIG == HELI_FRAME
         // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
-        if(motors->armed() && !ap.throttle_zero && motors->rotor_runup_complete()) {
+        if(motors->armed() && ap.using_interlock) {
+            if(!ap.throttle_zero && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
                 set_auto_armed(true);
             }
-#else
         // if motors are armed and throttle is above zero auto_armed should be true
         // if motors are armed and we are in throw mode, then auto_armed should be true
-        if(motors->armed() && (!ap.throttle_zero || control_mode == THROW)) {
+        } else if (motors->armed() && !ap.using_interlock) {
+            if(!ap.throttle_zero || control_mode == THROW) {
                 set_auto_armed(true);
             }
-#endif // HELI_FRAME
+        }
     }
 }
 

ArduCopter/takeoff.cpp

@@ -32,12 +32,10 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
         return false;
     }
 
-#if FRAME_CONFIG == HELI_FRAME
     // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
-    if (!copter.motors->rotor_runup_complete()) {
+    if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) {
         return false;
     }
-#endif
 
     if (!do_user_takeoff_start(takeoff_alt_cm)) {
         return false;