AP_MotorsHeli: rework RSC output() function to implement idle speed function

Also, split out speed_ramp function

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -43,63 +43,76 @@ void AP_MotorsHeli_RSC::recalc_scalers()
     _runup_increment = 1000.0f / (_runup_time * _loop_rate);
 }
 
+// output - update value to send to ESC/Servo
 void AP_MotorsHeli_RSC::output(uint8_t state)
 {
     switch (state){
+        case ROTOR_CONTROL_STOP:
+            _control_speed = 0;                             // ramp input to zero
+            _control_out = 0;                               // force ramp output to zero
+            _estimated_speed = 0;                           // force speed estimate to zero
+            break;
+
+        case ROTOR_CONTROL_IDLE:
+            _control_speed = _idle_speed;                   // set control speed to idle speed
+            if (_control_out < _idle_speed){
+                _control_out = _idle_speed;                 // if control output is less than idle speed, force ramp function to jump to idle speed
+            }
+            break;
 
-    case ROTOR_CONTROL_STOP:
-
-        _estimated_speed = 0;
-        _speed_out = 0;
-        write_rsc(0);
-        break;
+        case ROTOR_CONTROL_ACTIVE:
+            _control_speed = _desired_speed;                // set control speed to desired speed
+            break;
+    }
 
-    case ROTOR_CONTROL_IDLE:
-    case ROTOR_CONTROL_ACTIVE:
+    // run speed ramp function to slew output smoothly, also updates estimated speed
+    speed_ramp(_control_speed);
 
-        // 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;
-            }
+    // output to rsc servo
+    write_rsc(_control_out);
+}
 
-            // 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;
+// speed_ramp - ramps speed towards target, result put in _control_out 
+void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
+{
+    // range check speed_target
+    speed_target = constrain_int16(speed_target,0,1000);
+
+    // ramp output upwards towards target
+    if (_control_out < speed_target) {
+        // allow control output to jump to estimated speed
+        if (_control_out < _estimated_speed) {
+            _control_out = _estimated_speed;
         }
-
-        // 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;
-            }
+        // ramp up slowly to target
+        _control_out += _ramp_increment;
+        if (_control_out > speed_target) {
+            _control_out = speed_target;
         }
+    }else{
+        // ramping down happens instantly
+        _control_out = speed_target;
+    }
 
-        // set runup complete flag
-        if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
-            _runup_complete = true;
+    // ramp speed estimate towards control out
+    if (_estimated_speed < _control_out) {
+        _estimated_speed += _runup_increment;
+        if (_estimated_speed > _control_out) {
+            _estimated_speed = _control_out;
         }
-
-        if (_runup_complete && _estimated_speed <= _critical_speed) {
-            _runup_complete = false;
+    }else{
+        _estimated_speed -= _runup_increment;
+        if (_estimated_speed < _control_out) {
+            _estimated_speed = _control_out;
         }
+    }
 
-        // output to rsc servo
-        write_rsc(_speed_out);
-
-        break;
+    // set runup complete flag
+    if (!_runup_complete && speed_target > 0 && _estimated_speed >= speed_target) {
+        _runup_complete = true;
+    }
+    if (_runup_complete && _estimated_speed <= _critical_speed) {
+        _runup_complete = false;
     }
 }
 

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -28,12 +28,18 @@ public:
     // get_critical_speed
     int16_t     get_critical_speed() const { return _critical_speed; }
 
+    // set_idle_speed
+    void        set_idle_speed(int16_t idle_speed) { _idle_speed = idle_speed; }
+
     // get_desired_speed
     int16_t     get_desired_speed() const { return _desired_speed; }
 
     // set_desired_speed
     void        set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; }
 
+    // get_control_speed
+    int16_t     get_control_speed() const { return _control_speed; }
+
     // get_estimated_speed
     int16_t     get_estimated_speed() const { return _estimated_speed; }
 
@@ -49,7 +55,7 @@ public:
     // recalc_scalers
     void        recalc_scalers();
 
-    // output_armed
+    // output - update value to send to ESC/Servo
     void        output(uint8_t state);
 
 private:
@@ -61,8 +67,10 @@ private:
 
     // internal variables
     int16_t         _critical_speed = 0;        // rotor speed below which flight is not possible
+    int16_t         _idle_speed = 0;            // motor output idle speed
     int16_t         _desired_speed = 0;         // latest desired rotor speed from pilot
-    float           _speed_out = 0;             // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
+    int16_t         _control_speed = 0;         // latest logic controlled rotor speed
+    float           _control_out = 0;           // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
     float           _estimated_speed = 0;       // estimated speed of the main rotor (0~1000)
     float           _ramp_increment = 0;        // the amount we can increase the rotor output during each 100hz iteration
     int8_t          _ramp_time = 0;             // time in seconds for the output to the main rotor's ESC to reach full speed
@@ -70,6 +78,9 @@ private:
     float           _runup_increment = 0;       // the amount we can increase the rotor's estimated speed during each 100hz iteration
     bool            _runup_complete = false;    // flag for determining if runup is complete
 
+    // speed_ramp - ramps speed towards target, result put in _control_out
+    void            speed_ramp(int16_t rotor_target);
+
     // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000
     void            write_rsc(int16_t servo_out);
 };

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -224,10 +224,12 @@ void AP_MotorsHeli_Single::recalc_scalers()
         _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.recalc_scalers();
@@ -236,10 +238,12 @@ void AP_MotorsHeli_Single::recalc_scalers()
         _tail_rotor.set_ramp_time(0);
         _tail_rotor.set_runup_time(0);
         _tail_rotor.set_critical_speed(0);
+        _tail_rotor.set_idle_speed(0);
     } else {
         _tail_rotor.set_ramp_time(_rsc_ramp_time);
         _tail_rotor.set_runup_time(_rsc_runup_time);
         _tail_rotor.set_critical_speed(_rsc_critical);
+        _tail_rotor.set_idle_speed(_rsc_idle);
     }
 
     _tail_rotor.recalc_scalers();
@@ -494,7 +498,7 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16
         // rudder feed forward based on collective
         // the feed-forward is not required when the motor is shut down and not creating torque
         // also not required if we are using external gyro
-        if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
+        if ((_main_rotor.get_control_speed() > 0) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
             // sanity check collective_yaw_effect
             _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
             yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);