AP_Motors: allow setting of loop rate

this allows for SCHED_LOOP_RATE in copter

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -39,10 +39,21 @@ void AP_MotorsHeli_RSC::set_power_output_range(float power_low, float power_high
 // output - update value to send to ESC/Servo
 void AP_MotorsHeli_RSC::output(RotorControlState state)
 {
+    float dt;
+    uint64_t now = AP_HAL::micros64();
+    
+    if (_last_update_us == 0) {
+        _last_update_us = now;
+        dt = 0.001f;
+    } else {
+        dt = 1.0e-6f * (now - _last_update_us);
+        _last_update_us = now;
+    }
+    
     switch (state){
         case ROTOR_CONTROL_STOP:
             // set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
-            update_rotor_ramp(0.0f);
+            update_rotor_ramp(0.0f, dt);
 
             // control output forced to zero
             _control_output = 0.0f;
@@ -50,7 +61,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
 
         case ROTOR_CONTROL_IDLE:
             // set rotor ramp to decrease speed to zero
-            update_rotor_ramp(0.0f);
+            update_rotor_ramp(0.0f, dt);
 
             // set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
             _control_output = _idle_output;
@@ -58,7 +69,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
 
         case ROTOR_CONTROL_ACTIVE:
             // set main rotor ramp to increase to full speed
-            update_rotor_ramp(1.0f);
+            update_rotor_ramp(1.0f, dt);
 
             if ((_control_mode == ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SPEED_SETPOINT)) {
                 // set control rotor speed to ramp slewed value between idle and desired speed
@@ -71,14 +82,14 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
     }
 
     // update rotor speed run-up estimate
-    update_rotor_runup();
+    update_rotor_runup(dt);
 
     // output to rsc servo
     write_rsc(_control_output);
 }
 
 // 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)
+void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt)
 {
     // sanity check ramp time
     if (_ramp_time <= 0) {
@@ -92,7 +103,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 += (1.0f / (_ramp_time * _loop_rate));
+        _rotor_ramp_output += (dt / _ramp_time);
         if (_rotor_ramp_output > rotor_ramp_input) {
             _rotor_ramp_output = rotor_ramp_input;
         }
@@ -103,7 +114,7 @@ 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()
+void AP_MotorsHeli_RSC::update_rotor_runup(float dt)
 {
     // sanity check runup time
     if (_runup_time < _ramp_time) {
@@ -114,7 +125,7 @@ void AP_MotorsHeli_RSC::update_rotor_runup()
     }
 
     // ramp speed estimate towards control out
-    float runup_increment = 1.0f / (_runup_time * _loop_rate);
+    float runup_increment = dt / _runup_time;
     if (_rotor_runup_output < _rotor_ramp_output) {
         _rotor_runup_output += runup_increment;
         if (_rotor_runup_output > _rotor_ramp_output) {

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -26,9 +26,7 @@ public:
     friend class AP_MotorsHeli_Single;
     
     AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn,
-                      uint8_t default_channel,
+                      uint8_t default_channel) :
-                      uint16_t loop_rate) :
-        _loop_rate(loop_rate),
         _aux_fn(aux_fn),
         _default_channel(default_channel)
     {};
@@ -80,10 +78,8 @@ public:
     void        output(RotorControlState state);
 
 private:
-
+    uint64_t        _last_update_us;
-    // external variables
+    
-    float           _loop_rate;                 // main loop rate
-
     // channel setup for aux function
     RC_Channel_aux::Aux_servo_function_t _aux_fn;
     uint8_t         _default_channel;
@@ -109,10 +105,10 @@ private:
     AP_Int8         _pwm_rev;
     
     // 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);
+    void            update_rotor_ramp(float rotor_ramp_input, float dt);
 
     // update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
-    void            update_rotor_runup();
+    void            update_rotor_runup(float dt);
 
     // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1
     void            write_rsc(float servo_out);

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -49,8 +49,8 @@ public:
                          uint16_t       speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
         AP_MotorsHeli(loop_rate, speed_hz),
         _servo_aux(servo_aux),
-        _main_rotor(RC_Channel_aux::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC, loop_rate),
+        _main_rotor(RC_Channel_aux::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC),
-        _tail_rotor(RC_Channel_aux::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX, loop_rate),
+        _tail_rotor(RC_Channel_aux::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX),
         _swash_servo_1(CH_NONE), _swash_servo_2(CH_NONE), _swash_servo_3(CH_NONE), _yaw_servo(CH_NONE)
     {
         AP_Param::setup_object_defaults(this, var_info);

libraries/AP_Motors/AP_Motors_Class.h

@@ -128,6 +128,9 @@ public:
     // pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
     void                set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input);
 
+    // set loop rate. Used to support loop rate as a parameter
+    void                set_loop_rate(uint16_t loop_rate) { _loop_rate = loop_rate; }
+    
 protected:
     // output functions that should be overloaded by child classes
     virtual void        output_armed_stabilizing()=0;