Copter: slow start motors from 0 to min throttle

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -150,16 +150,16 @@ void AP_MotorsMatrix::output_armed()
     // if we are not sending a throttle output, we cut the motors
     if (_rc_throttle->servo_out == 0) {
         // range check spin_when_armed
-        if (_spin_when_armed < 0) {
+        if (_spin_when_armed_ramped < 0) {
-             _spin_when_armed = 0;
+             _spin_when_armed_ramped = 0;
         }
-        if (_spin_when_armed > _min_throttle) {
+        if (_spin_when_armed_ramped > _min_throttle) {
-            _spin_when_armed = _min_throttle;
+            _spin_when_armed_ramped = _min_throttle;
         }
         for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
             // spin motors at minimum
             if (motor_enabled[i]) {
-                motor_out[i] = _rc_throttle->radio_min + _spin_when_armed;
+                motor_out[i] = _rc_throttle->radio_min + _spin_when_armed_ramped;
             }
         }
 

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -102,15 +102,15 @@ void AP_MotorsTri::output_armed()
     // if we are not sending a throttle output, we cut the motors
     if(_rc_throttle->servo_out == 0) {
         // range check spin_when_armed
-        if (_spin_when_armed < 0) {
+        if (_spin_when_armed_ramped < 0) {
-            _spin_when_armed = 0;
+            _spin_when_armed_ramped = 0;
         }
-        if (_spin_when_armed > _min_throttle) {
+        if (_spin_when_armed_ramped > _min_throttle) {
-            _spin_when_armed = _min_throttle;
+            _spin_when_armed_ramped = _min_throttle;
         }
-        motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed;
+        motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed_ramped;
-        motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed;
+        motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed_ramped;
-        motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed;
+        motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed_ramped;
 
         // Every thing is limited
         limit.throttle_lower = true;

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -64,7 +64,8 @@ AP_Motors::AP_Motors( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_
     _speed_hz(speed_hz),
     _min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
     _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
-    _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE)
+    _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
+    _spin_when_armed_ramped(0)
 {
     uint8_t i;
 
@@ -77,6 +78,9 @@ AP_Motors::AP_Motors( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_
         set_motor_to_channel_map(APM2_MOTOR_TO_CHANNEL_MAP);
 #endif
 
+    // slow start motors from zero to min throttle
+    _flags.slow_start_low_end = true;
+
     // clear output arrays
     for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         motor_out[i] = 0;
@@ -93,6 +97,9 @@ void AP_Motors::Init()
 void AP_Motors::armed(bool arm)
 {
     _flags.armed = arm;
+    if (!_flags.armed) {
+        _flags.slow_start_low_end = true;
+    }
     AP_Notify::flags.armed = arm;
 };
 
@@ -187,6 +194,15 @@ void AP_Motors::slow_start(bool true_false)
 // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
 void AP_Motors::update_max_throttle()
 {
+    // ramp up minimum spin speed if necessary
+    if (_flags.slow_start_low_end) {
+        _spin_when_armed_ramped += AP_MOTOR_SLOW_START_LOW_END_INCREMENT;
+        if (_spin_when_armed_ramped >= _spin_when_armed) {
+            _spin_when_armed_ramped = _spin_when_armed;
+            _flags.slow_start_low_end = false;
+        }
+    }
+
     // return max throttle if we're not slow_starting
     if (!_flags.slow_start) {
         return;

libraries/AP_Motors/AP_Motors_Class.h

@@ -55,8 +55,9 @@
 #define AP_MOTOR_THROTTLE_LIMIT     0x04
 #define AP_MOTOR_ANY_LIMIT          0xFF
 
-// slow start increment - max throttle increase per (100hz) iteration.  i.e. 10 = full speed in 1 second
+// slow start increments - throttle increase per (100hz) iteration.  i.e. 5 = full speed in 2 seconds
-#define AP_MOTOR_SLOW_START_INCREMENT   10
+#define AP_MOTOR_SLOW_START_INCREMENT           10      // max throttle ramp speed (i.e. motors can reach full throttle in 2 seconds)
+#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT   2       // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 1 second)
 
 /// @class      AP_Motors
 class AP_Motors {
@@ -150,6 +151,7 @@ protected:
         uint8_t armed               : 1;    // 1 if the motors are armed, 0 if disarmed
         uint8_t frame_orientation   : 2;    // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3
         uint8_t slow_start          : 1;    // 1 if slow start is active
+        uint8_t slow_start_low_end  : 1;    // 1 just after arming so we can ramp up the spin_when_armed value
     } _flags;
 
     // parameters
@@ -166,5 +168,6 @@ protected:
     int16_t             _min_throttle;          // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
     int16_t             _max_throttle;          // the maximum throttle to be sent to the motors (sometimes limited by slow start)
     int16_t             _hover_out;             // the estimated hover throttle in pwm (i.e. 1000 ~ 2000).  calculated from the THR_MID parameter
+    int16_t             _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero
 };
 #endif  // __AP_MOTORS_CLASS_H__