AP_MotorsHeli: RSC controller to use speed ramp as simple float scalar

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -200,7 +200,7 @@ bool AP_MotorsHeli::parameter_check() const
     }
 
     // returns false if RSC Mode is not set to a valid control mode
-    if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_NONE || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
+    if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_DISABLED || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
         return false;
     }
 

libraries/AP_Motors/AP_MotorsHeli.h

@@ -37,7 +37,7 @@
 #define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN      0
 
 // main rotor speed control types (ch8 out)
-#define AP_MOTORS_HELI_RSC_MODE_NONE            0       // not a valid RSC Mode
+#define AP_MOTORS_HELI_RSC_MODE_DISABLED        0       // not a valid RSC Mode
 #define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1       // main rotor ESC is connected to RC8 (out), pilot desired rotor speed provided by CH8 input
 #define AP_MOTORS_HELI_RSC_MODE_SETPOINT        2       // main rotor ESC is connected to RC8 (out), desired speed is held in RSC_SETPOINT parameter
 
@@ -120,7 +120,7 @@ public:
     // set_collective_for_landing - limits collective from going too low if we know we are landed
     void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
 
-    // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_NONE, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
+    // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
     uint8_t get_rsc_mode() const { return _rsc_mode; }
 
     // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1000)
@@ -132,8 +132,8 @@ public:
     // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
     virtual int16_t get_desired_rotor_speed() const = 0;
 
-    // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
-    virtual int16_t get_estimated_rotor_speed() const = 0;
+    // get_main_rotor_speed - gets estimated or measured main rotor speed
+    virtual int16_t get_main_rotor_speed() const = 0;
 
     // return true if the main rotor is up to speed
     bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; }

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -36,7 +36,7 @@ void AP_MotorsHeli_RSC::recalc_scalers()
         _ramp_time = 1;
     }
 
-    _ramp_increment = 1000.0f / (_ramp_time * _loop_rate);
+    _ramp_increment = 1.0f / (_ramp_time * _loop_rate);
 
     // recalculate rotor runup increment
     if (_runup_time <= 0 ) {
@@ -47,7 +47,7 @@ void AP_MotorsHeli_RSC::recalc_scalers()
         _runup_time = _ramp_time;
     }
 
-    _runup_increment = 1000.0f / (_runup_time * _loop_rate);
+    _runup_increment = 1.0f / (_runup_time * _loop_rate);
 }
 
 // output - update value to send to ESC/Servo
@@ -55,89 +55,113 @@ 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
+            // set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
+            update_rotor_ramp(0.0f);
+
+            // control output forced to zero
+            _control_speed = 0;
             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
-            }
+            // set rotor ramp to decrease speed to zero
+            update_rotor_ramp(0.0f);
+
+            // set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
+            _control_speed = _idle_speed;
             break;
 
         case ROTOR_CONTROL_ACTIVE:
-            _control_speed = _desired_speed;                // set control speed to desired speed
+            // set main rotor ramp to increase to full speed
+            update_rotor_ramp(1.0f);
+
+            if ((_control_mode == ROTOR_CONTROL_MODE_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SETPOINT)) {
+                // set control rotor speed to ramp slewed value between idle and desired speed
+                _control_speed = _idle_speed + (_rotor_ramp_output * (_desired_speed - _idle_speed));
+            }
             break;
     }
 
-    // run speed ramp function to slew output smoothly
-    speed_ramp(_control_speed);
-
-    // update rotor speed estimate
-    update_speed_estimate();
+    // update rotor speed run-up estimate
+    update_rotor_runup();
 
     // output to rsc servo
-    write_rsc(_control_out);
+    write_rsc(_control_speed);
 }
 
-// speed_ramp - ramps speed towards target, result put in _control_out 
-void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
+// update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
+void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
 {
-    // range check speed_target
-    speed_target = constrain_int16(speed_target,0,1000);
-
     // ramp output upwards towards target
-    if (_control_out < speed_target) {
+    if (_rotor_ramp_output < rotor_ramp_input) {
         // allow control output to jump to estimated speed
-        if (_control_out < _estimated_speed) {
-            _control_out = _estimated_speed;
+        if (_rotor_ramp_output < _rotor_runup_output) {
+            _rotor_ramp_output = _rotor_runup_output;
         }
         // ramp up slowly to target
-        _control_out += _ramp_increment;
-        if (_control_out > speed_target) {
-            _control_out = speed_target;
+        _rotor_ramp_output += _ramp_increment;
+        if (_rotor_ramp_output > rotor_ramp_input) {
+            _rotor_ramp_output = rotor_ramp_input;
         }
     }else{
         // ramping down happens instantly
-        _control_out = speed_target;
+        _rotor_ramp_output = rotor_ramp_input;
     }
-
-
 }
 
-// update_speed_estimate - function to estimate speed
-void AP_MotorsHeli_RSC::update_speed_estimate()
+// update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
+void AP_MotorsHeli_RSC::update_rotor_runup()
 {
     // 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 (_rotor_runup_output < _rotor_ramp_output) {
+        _rotor_runup_output += _runup_increment;
+        if (_rotor_runup_output > _rotor_ramp_output) {
+            _rotor_runup_output = _rotor_ramp_output;
         }
     }else{
-        _estimated_speed -= _runup_increment;
-        if (_estimated_speed < _control_out) {
-            _estimated_speed = _control_out;
+        _rotor_runup_output -= _runup_increment;
+        if (_rotor_runup_output < _rotor_ramp_output) {
+            _rotor_runup_output = _rotor_ramp_output;
         }
     }
 
     // update run-up complete flag
-    if (!_runup_complete && _control_out > _idle_speed && _estimated_speed >= _control_out) {
+
+    // if control mode is disabled, then run-up complete always returns true
+    if ( _control_mode == ROTOR_CONTROL_MODE_DISABLED ){
         _runup_complete = true;
+        return;
     }
-    if (_runup_complete && _estimated_speed <= _critical_speed) {
+
+    // if rotor ramp and runup are both at full speed, then run-up has been completed
+    if (!_runup_complete && (_rotor_ramp_output >= 1.0f) && (_rotor_runup_output >= 1.0f)) {
+        _runup_complete = true;
+    }
+    // if rotor speed is less than critical speed, then run-up is not complete
+    // this will prevent the case where the target rotor speed is less than critical speed
+    if (_runup_complete && (get_rotor_speed() <= _critical_speed)) {
         _runup_complete = false;
     }
 }
 
+// get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value
+int16_t AP_MotorsHeli_RSC::get_rotor_speed() const
+{
+    // if no actual measured rotor speed is available, estimate speed based on rotor runup scalar.
+    return (_rotor_runup_output * _max_speed);
+}
+
 // write_rsc - outputs pwm onto output rsc channel
 // servo_out parameter is of the range 0 ~ 1000
 void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out)
 {
-    _servo_output.servo_out = servo_out;
-    _servo_output.calc_pwm();
-
-    hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
+    if (_control_mode == ROTOR_CONTROL_MODE_DISABLED){
+        // do not do servo output to avoid conflicting with other output on the channel
+        // ToDo: We should probably use RC_Channel_Aux to avoid this problem
+        return;
+    } else {
+        _servo_output.servo_out = servo_out;
+        _servo_output.calc_pwm();
+
+        hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
+    }
 }

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -12,6 +12,11 @@
 #define ROTOR_CONTROL_IDLE                      1
 #define ROTOR_CONTROL_ACTIVE                    2
 
+// rotor control modes
+#define ROTOR_CONTROL_MODE_DISABLED             0
+#define ROTOR_CONTROL_MODE_PASSTHROUGH          1
+#define ROTOR_CONTROL_MODE_SETPOINT             2
+
 class AP_MotorsHeli_RSC {
 public:
         AP_MotorsHeli_RSC(RC_Channel&   servo_output,
@@ -25,6 +30,9 @@ public:
     // init_servo - servo initialization on start-up
     void        init_servo();
 
+    // set_control_mode - sets control mode
+    void        set_control_mode(int8_t mode) { _control_mode = mode; }
+
     // set_critical_speed
     void        set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; }
     
@@ -43,8 +51,8 @@ public:
     // get_control_speed
     int16_t     get_control_speed() const { return _control_speed; }
 
-    // get_estimated_speed
-    int16_t     get_estimated_speed() const { return _estimated_speed; }
+    // get_rotor_speed - return estimated or measured rotor speed
+    int16_t     get_rotor_speed() const;
 
     // is_runup_complete
     bool        is_runup_complete() const { return _runup_complete; }
@@ -69,23 +77,25 @@ private:
     float           _loop_rate;                 // main loop rate
 
     // internal variables
+    int8_t          _control_mode = 0;          // motor control mode, Passthrough or Setpoint
     int16_t         _critical_speed = 0;        // rotor speed below which flight is not possible
     int16_t         _idle_speed = 0;            // motor output idle speed
+    int16_t         _max_speed = 1000;          // rotor maximum speed. Placeholder value until we have measured speed input (ToDo)
     int16_t         _desired_speed = 0;         // latest desired rotor speed from pilot
     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
+    float           _rotor_ramp_output = 0;     // scalar used to ramp rotor speed between _rsc_idle and full speed (0.0-1.0f)
+    float           _rotor_runup_output = 0;    // scalar used to store status of rotor run-up time (0.0-1.0f)
+    float           _ramp_increment = 0;        // the amount to increase/decrease the rotor ramp scalar during each iteration
     int8_t          _ramp_time = 0;             // time in seconds for the output to the main rotor's ESC to reach full speed
     int8_t          _runup_time = 0;            // time in seconds for the main rotor to reach full speed.  Must be longer than _rsc_ramp_time
-    float           _runup_increment = 0;       // the amount we can increase the rotor's estimated speed during each 100hz iteration
+    float           _runup_increment = 0;       // the amount to increase/decrease the rotor run-up scalar during each 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);
+    // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
+    void            update_rotor_ramp(float rotor_ramp_input);
 
-    // update_speed_estimate - function to estimate speed
-    void            update_speed_estimate();
+    // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
+    void            update_rotor_runup();
 
     // 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

@@ -195,7 +195,7 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm)
 bool AP_MotorsHeli_Single::allow_arming() const
 {
     // returns false if main rotor speed is not zero
-    if (_main_rotor.get_estimated_speed() > 0) {
+    if (_main_rotor.get_rotor_speed() > 0) {
         return false;
     }
 
@@ -209,40 +209,39 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed)
 {
     _main_rotor.set_desired_speed(desired_speed);
 
-    if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
+    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
         _tail_rotor.set_desired_speed(_direct_drive_tailspeed);
     } else {
         _tail_rotor.set_desired_speed(0);
     }
 }
 
-
-// recalc_scalers - recalculates various scalers used.  Should be called at about 1hz to allow users to see effect of changing parameters
+// recalc_scalers - recalculates various scalers used.
 void AP_MotorsHeli_Single::recalc_scalers()
 {
-    
+    _main_rotor.set_control_mode(_rsc_mode);    
     _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();
 
-    if (_rsc_mode != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
-        _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 {
+    if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
+        _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT);
         _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);
+    } else {
+        _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_DISABLED);
+        _tail_rotor.set_ramp_time(0);
+        _tail_rotor.set_runup_time(0);
+        _tail_rotor.set_critical_speed(0);
+        _tail_rotor.set_idle_speed(0);
     }
-
     _tail_rotor.recalc_scalers();
 }
 
-
 // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
 //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
 uint16_t AP_MotorsHeli_Single::get_motor_mask()

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -87,16 +87,16 @@ public:
     // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
     void set_desired_rotor_speed(int16_t desired_speed);
 
-    // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
-    int16_t get_estimated_rotor_speed() const { return _main_rotor.get_estimated_speed(); }
+    // get_main_rotor_speed - gets estimated or measured main rotor speed
+    int16_t get_main_rotor_speed() const { return _main_rotor.get_rotor_speed(); }
 
     // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
     int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
 
     // rotor_speed_above_critical - return true if rotor speed is above that critical for flight
-    bool rotor_speed_above_critical() const { return _main_rotor.get_estimated_speed() > _main_rotor.get_critical_speed(); }
+    bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
 
-    // recalc_scalers - recalculates various scalers used.  Should be called at about 1hz to allow users to see effect of changing parameters
+    // recalc_scalers - recalculates various scalers used.
     void recalc_scalers();
 
     // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)