AP_MotorsHeli: Fully detail tradheli output functions and move manual servo handling

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -201,21 +201,6 @@ bool AP_MotorsHeli::rotor_runup_complete() const
     return _heliflags.rotor_runup_complete;
 }
 
-void AP_MotorsHeli::output_armed_not_stabilizing()
-{
-    // stabilizing servos always operate for helicopters
-    output_armed_stabilizing();
-}
-
-// output_armed_zero_throttle - sends commands to the motors
-void AP_MotorsHeli::output_armed_zero_throttle()
-{
-    // stabilizing servos always operate for helicopters
-    // ToDo: Bring RSC Master On/Off into this function
-    output_armed_stabilizing();
-}
-
-
 // reset_swash - free up swash for maximum movements. Used for set-up
 void AP_MotorsHeli::reset_swash()
 {

libraries/AP_Motors/AP_MotorsHeli.h

@@ -168,8 +168,8 @@ protected:
 
     // output - sends commands to the motors
     virtual void        output_armed_stabilizing() = 0;
-    void                output_armed_not_stabilizing();
-    void                output_armed_zero_throttle();
+    virtual void        output_armed_not_stabilizing() = 0;
+    virtual void        output_armed_zero_throttle() = 0;
     virtual void        output_disarmed() = 0;
 
     // update the throttle input filter

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -257,6 +257,63 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask()
 // sends commands to the motors
 void AP_MotorsHeli_Single::output_armed_stabilizing()
 {
+    // if manual override active after arming, deactivate it.
+    if (_servo_manual == 1) {
+        reset_radio_passthrough();
+        _servo_manual = 0;
+    }
+
+    move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
+
+    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
+        _tail_rotor.output_armed();
+
+        if (!_tail_rotor.is_runup_complete())
+        {
+            _heliflags.rotor_runup_complete = false;
+            return;
+        }
+    }
+
+    _main_rotor.output_armed();
+
+    _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
+}
+
+void AP_MotorsHeli_Single::output_armed_not_stabilizing()
+{
+    // if manual override active after arming, deactivate it.
+    if (_servo_manual == 1) {
+        reset_radio_passthrough();
+        _servo_manual = 0;
+    }
+
+    move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
+
+    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
+        _tail_rotor.output_armed();
+
+        if (!_tail_rotor.is_runup_complete())
+        {
+            _heliflags.rotor_runup_complete = false;
+            return;
+        }
+    }
+
+    _main_rotor.output_armed();
+
+    _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
+}
+
+// output_armed_zero_throttle - sends commands to the motors
+void AP_MotorsHeli_Single::output_armed_zero_throttle()
+{
+    // if manual override active after arming, deactivate it.
+    if (_servo_manual == 1) {
+        reset_radio_passthrough();
+        _servo_manual = 0;
+    }
+
     move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
 
     if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
@@ -274,9 +331,18 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
     _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
 }
 
+
 // output_disarmed - sends commands to the motors
 void AP_MotorsHeli_Single::output_disarmed()
 {
+    // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
+    if (_servo_manual == 1) {
+        _roll_control_input = _roll_radio_passthrough;
+        _pitch_control_input = _pitch_radio_passthrough;
+        _throttle_control_input = _throttle_radio_passthrough;
+        _yaw_control_input = _yaw_radio_passthrough;
+    }
+
     move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
 
     if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
@@ -355,14 +421,6 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
 //
 void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
 {
-    // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
-    if (_servo_manual == 1) {
-        _roll_control_input = _roll_radio_passthrough;
-        _pitch_control_input = _pitch_radio_passthrough;
-        _throttle_control_input = _throttle_radio_passthrough;
-        _yaw_control_input = _yaw_radio_passthrough;
-    }
-
     int16_t yaw_offset = 0;
     int16_t coll_out_scaled;
 

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -126,6 +126,8 @@ protected:
 
     // output - sends commands to the motors
     void output_armed_stabilizing();
+    void output_armed_not_stabilizing();
+    void output_armed_zero_throttle();
     void output_disarmed();
 
     // reset_servos - free up the swash servos for maximum movement