AP_MotorsHeli_RSC: rotor speeds in 0 to 1 range

Also move recalc_scalers functionality into update_rotor_ramp and update_rotor_runup

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -28,28 +28,6 @@ void AP_MotorsHeli_RSC::init_servo()
     RC_Channel_aux::set_aux_channel_default(_aux_fn, _default_channel);
 }
 
-// recalc_scalers - recalculates various scalers used.  Should be called at about 1hz to allow users to see effect of changing parameters
-void AP_MotorsHeli_RSC::recalc_scalers()
-{
-    // recalculate rotor ramp up increment
-    if (_ramp_time <= 0) {
-        _ramp_time = 1;
-    }
-
-    _ramp_increment = 1.0f / (_ramp_time * _loop_rate);
-
-    // recalculate rotor runup increment
-    if (_runup_time <= 0 ) {
-        _runup_time = 1;
-    }
-    
-    if (_runup_time < _ramp_time) {
-        _runup_time = _ramp_time;
-    }
-
-    _runup_increment = 1.0f / (_runup_time * _loop_rate);
-}
-
 // set_power_output_range
 void AP_MotorsHeli_RSC::set_power_output_range(uint16_t power_low, uint16_t power_high)
 {
@@ -67,7 +45,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
             update_rotor_ramp(0.0f);
 
             // control output forced to zero
-            _control_output = 0;
+            _control_output = 0.0f;
             break;
 
         case ROTOR_CONTROL_IDLE:
@@ -102,6 +80,11 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
 // 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)
 {
+    // sanity check ramp time
+    if (_ramp_time <= 0) {
+        _ramp_time = 1;
+    }
+
     // ramp output upwards towards target
     if (_rotor_ramp_output < rotor_ramp_input) {
         // allow control output to jump to estimated speed
@@ -109,7 +92,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
             _rotor_ramp_output = _rotor_runup_output;
         }
         // ramp up slowly to target
-        _rotor_ramp_output += _ramp_increment;
+        _rotor_ramp_output += (1.0f / (_ramp_time * _loop_rate));
         if (_rotor_ramp_output > rotor_ramp_input) {
             _rotor_ramp_output = rotor_ramp_input;
         }
@@ -122,14 +105,23 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
 // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
 void AP_MotorsHeli_RSC::update_rotor_runup()
 {
+    // sanity check runup time
+    if (_runup_time < _ramp_time) {
+        _runup_time = _ramp_time;
+    }
+    if (_runup_time <= 0 ) {
+        _runup_time = 1;
+    }
+
     // ramp speed estimate towards control out
+    float runup_increment = 1.0f / (_runup_time * _loop_rate);
     if (_rotor_runup_output < _rotor_ramp_output) {
-        _rotor_runup_output += _runup_increment;
+        _rotor_runup_output += runup_increment;
         if (_rotor_runup_output > _rotor_ramp_output) {
             _rotor_runup_output = _rotor_ramp_output;
         }
     }else{
-        _rotor_runup_output -= _runup_increment;
+        _rotor_runup_output -= runup_increment;
         if (_rotor_runup_output < _rotor_ramp_output) {
             _rotor_runup_output = _rotor_ramp_output;
         }
@@ -155,21 +147,21 @@ void AP_MotorsHeli_RSC::update_rotor_runup()
 }
 
 // get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value
-int16_t AP_MotorsHeli_RSC::get_rotor_speed() const
+float 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);
+    return _rotor_runup_output;
 }
 
 // write_rsc - outputs pwm onto output rsc channel
-// servo_out parameter is of the range 0 ~ 1000
+// servo_out parameter is of the range 0 ~ 1
-void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out)
+void AP_MotorsHeli_RSC::write_rsc(float servo_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 {
-        RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out);
+        RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out * 1000.0f);
     }
 }

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -38,26 +38,26 @@ public:
     void        set_control_mode(RotorControlMode mode) { _control_mode = mode; }
 
     // set_critical_speed
-    void        set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; }
+    void        set_critical_speed(float critical_speed) { _critical_speed = critical_speed; }
     
     // get_critical_speed
-    int16_t     get_critical_speed() const { return _critical_speed; }
+    float       get_critical_speed() const { return _critical_speed; }
 
     // set_idle_output
-    void        set_idle_output(int16_t idle_output) { _idle_output = idle_output; }
     float       get_idle_output() { return _idle_output; }
+    void        set_idle_output(float idle_output) { _idle_output = idle_output; }
 
     // get_desired_speed
-    int16_t     get_desired_speed() const { return _desired_speed; }
+    float       get_desired_speed() const { return _desired_speed; }
 
     // set_desired_speed
-    void        set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; }
+    void        set_desired_speed(float desired_speed) { _desired_speed = desired_speed; }
 
     // get_control_speed
-    int16_t     get_control_output() const { return _control_output; }
+    float       get_control_output() const { return _control_output; }
 
     // get_rotor_speed - return estimated or measured rotor speed
-    int16_t     get_rotor_speed() const;
+    float       get_rotor_speed() const;
 
     // is_runup_complete
     bool        is_runup_complete() const { return _runup_complete; }
@@ -74,9 +74,6 @@ public:
     // set_motor_load
     void        set_motor_load(float load) { _load_feedforward = load; }
 
-    // recalc_scalers
-    void        recalc_scalers();
-
     // output - update value to send to ESC/Servo
     void        output(RotorControlState state);
 
@@ -91,22 +88,19 @@ private:
     
     // internal variables
     RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED;   // motor control mode, Passthrough or Setpoint
-    int16_t         _critical_speed = 0;        // rotor speed below which flight is not possible
+    float           _critical_speed = 0.0f;     // rotor speed below which flight is not possible
-    int16_t         _idle_output = 0;           // motor output idle speed
+    float           _idle_output = 0.0f;        // motor output idle speed
-    int16_t         _max_speed = 1000;          // rotor maximum speed. Placeholder value until we have measured speed input (ToDo)
+    float           _desired_speed = 0.0f;      // latest desired rotor speed from pilot
-    int16_t         _desired_speed = 0;         // latest desired rotor speed from pilot
+    float           _control_output = 0.0f;     // latest logic controlled output
-    int16_t         _control_output = 0;        // latest logic controlled output
+    float           _rotor_ramp_output = 0.0f;  // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f)
-    float           _rotor_ramp_output = 0;     // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f)
+    float           _rotor_runup_output = 0.0f; // scalar used to store status of rotor run-up time (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 to increase/decrease the rotor run-up scalar during each iteration
     bool            _runup_complete = false;    // flag for determining if runup is complete
-    uint16_t        _power_output_low = 0;      // setpoint for power output at minimum rotor power
+    float           _power_output_low = 0.0f;   // setpoint for power output at minimum rotor power
-    uint16_t        _power_output_high = 0;     // setpoint for power output at maximum rotor power
+    float           _power_output_high = 0.0f;  // setpoint for power output at maximum rotor power
-    uint16_t        _power_output_range = 0;    // maximum range of output power
+    float           _power_output_range = 0.0f; // maximum range of output power
-    float           _load_feedforward = 0;      // estimate of motor load, range 0-1.0f
+    float           _load_feedforward = 0.0f;   // estimate of motor load, range 0-1.0f
 
     // 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);
@@ -114,6 +108,6 @@ private:
     // 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
+    // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1
-    void            write_rsc(int16_t servo_out);
+    void            write_rsc(float servo_out);
 };