AP_MotorsHeli: Change RSC output() function into a state machine.

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -43,54 +43,64 @@ void AP_MotorsHeli_RSC::recalc_scalers()
     _runup_increment = 1000.0f / (_runup_time * _loop_rate);
 }
 
-void AP_MotorsHeli_RSC::output_armed()
+void AP_MotorsHeli_RSC::output(uint8_t state)
 {
-    // ramp rotor esc output towards target
+    switch (state){
-    if (_speed_out < _desired_speed) {
-        // allow rotor out to jump to rotor's current speed
-        if (_speed_out < _estimated_speed) {
-            _speed_out = _estimated_speed;
-        }
 
-        // ramp up slowly to target 
+    case ROTOR_CONTROL_STOP:
-        _speed_out += _ramp_increment;
+
-        if (_speed_out > _desired_speed) {
+        _estimated_speed = 0;
+        _speed_out = 0;
+        write_rsc(0);
+        break;
+
+    case ROTOR_CONTROL_IDLE:
+    case ROTOR_CONTROL_ACTIVE:
+    
+        // ramp rotor esc output towards target
+        if (_speed_out < _desired_speed) {
+            // allow rotor out to jump to rotor's current speed
+            if (_speed_out < _estimated_speed) {
+                _speed_out = _estimated_speed;
+            }
+
+            // ramp up slowly to target 
+            _speed_out += _ramp_increment;
+            if (_speed_out > _desired_speed) {
+                _speed_out = _desired_speed;
+            }
+        } else {
+            // ramping down happens instantly
             _speed_out = _desired_speed;
         }
-    } else {
-        // ramping down happens instantly
-        _speed_out = _desired_speed;
-    }
 
-    // ramp rotor speed estimate towards speed out
+        // ramp rotor speed estimate towards speed out
-    if (_estimated_speed < _speed_out) {
-        _estimated_speed += _runup_increment;
-        if (_estimated_speed > _speed_out) {
-            _estimated_speed = _speed_out;
-        }
-    } else {
-        _estimated_speed -= _runup_increment;
         if (_estimated_speed < _speed_out) {
-            _estimated_speed = _speed_out;
+            _estimated_speed += _runup_increment;
+            if (_estimated_speed > _speed_out) {
+                _estimated_speed = _speed_out;
+            }
+        } else {
+            _estimated_speed -= _runup_increment;
+            if (_estimated_speed < _speed_out) {
+                _estimated_speed = _speed_out;
+            }
         }
+
+        // set runup complete flag
+        if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
+            _runup_complete = true;
+        }
+
+        if (_runup_complete && _estimated_speed <= _critical_speed) {
+            _runup_complete = false;
+        }
+
+        // output to rsc servo
+        write_rsc(_speed_out);
+
+        break;
     }
-
-    // set runup complete flag
-    if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
-        _runup_complete = true;
-    }
-
-    if (_runup_complete && _estimated_speed <= _critical_speed) {
-        _runup_complete = false;
-    }
-
-    // output to rsc servo
-    write_rsc(_speed_out);
-}
-
-void AP_MotorsHeli_RSC::output_disarmed()
-{
-    write_rsc(0);
 }
 
 // write_rsc - outputs pwm onto output rsc channel

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -7,6 +7,11 @@
 #include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <RC_Channel/RC_Channel.h>      // RC Channel Library
 
+// rotor controller states
+#define ROTOR_CONTROL_STOP                      0
+#define ROTOR_CONTROL_IDLE                      1
+#define ROTOR_CONTROL_ACTIVE                    2
+
 class AP_MotorsHeli_RSC {
 public:
         AP_MotorsHeli_RSC(RC_Channel&   servo_output,
@@ -45,10 +50,7 @@ public:
     void        recalc_scalers();
 
     // output_armed
-    void        output_armed();
+    void        output(uint8_t state);
-
-    // output_disarmed
-    void        output_disarmed();
 
 private:
 

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -266,7 +266,7 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
     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();
+        _tail_rotor.output(ROTOR_CONTROL_ACTIVE);
 
         if (!_tail_rotor.is_runup_complete())
         {
@@ -275,7 +275,7 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
         }
     }
 
-    _main_rotor.output_armed();
+    _main_rotor.output(ROTOR_CONTROL_ACTIVE);
 
     _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
 }
@@ -291,7 +291,7 @@ void AP_MotorsHeli_Single::output_armed_not_stabilizing()
     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();
+        _tail_rotor.output(ROTOR_CONTROL_ACTIVE);
 
         if (!_tail_rotor.is_runup_complete())
         {
@@ -300,7 +300,7 @@ void AP_MotorsHeli_Single::output_armed_not_stabilizing()
         }
     }
 
-    _main_rotor.output_armed();
+    _main_rotor.output(ROTOR_CONTROL_ACTIVE);
 
     _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
 }
@@ -317,7 +317,7 @@ void AP_MotorsHeli_Single::output_armed_zero_throttle()
     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();
+        _tail_rotor.output(ROTOR_CONTROL_IDLE);
 
         if (!_tail_rotor.is_runup_complete())
         {
@@ -326,7 +326,7 @@ void AP_MotorsHeli_Single::output_armed_zero_throttle()
         }
     }
 
-    _main_rotor.output_armed();
+    _main_rotor.output(ROTOR_CONTROL_IDLE);
 
     _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
 }
@@ -346,10 +346,10 @@ void AP_MotorsHeli_Single::output_disarmed()
     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_disarmed();
+        _tail_rotor.output(ROTOR_CONTROL_STOP);
     }
 
-    _main_rotor.output_disarmed();
+    _main_rotor.output(ROTOR_CONTROL_STOP);
 
     _heliflags.rotor_runup_complete = false;
 }