AP_Motors: add calc_spin_up_to_pwm to reduce repeated code

libraries/AP_Motors/AP_MotorsCoax.cpp

@@ -129,12 +129,12 @@ void AP_MotorsCoax::output_to_motors()
         case SPIN_WHEN_ARMED:
             // sends output to motors when armed but not flying
             hal.rcout->cork();
-            rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
-            rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
-            rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
-            rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
-            rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
-            rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
+            rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
+            rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
+            rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
+            rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
+            rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
+            rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
             hal.rcout->push();
             break;
         case SPOOL_UP:

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -102,7 +102,7 @@ void AP_MotorsMatrix::output_to_motors()
             // sends output to motors when armed but not flying
             for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
                 if (motor_enabled[i]) {
-                    motor_out[i] = constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle);
+                    motor_out[i] = calc_spin_up_to_pwm();
                 }
             }
             break;

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -310,7 +310,10 @@ int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
     thrust_in = constrain_float(thrust_in, 0, 1);
     return constrain_int16((get_pwm_output_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
             (get_pwm_output_max() - (get_pwm_output_min() + _min_throttle))), get_pwm_output_min() + _min_throttle, get_pwm_output_max());
-}
+
+int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const
+{
+    return get_pwm_output_min() + constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min * (get_pwm_output_max()-get_pwm_output_min());}
 
 // get minimum or maximum pwm value that can be output to motors
 int16_t AP_MotorsMulticopter::get_pwm_output_min() const
@@ -379,7 +382,7 @@ void AP_MotorsMulticopter::output_logic()
             }
 
             // set and increment ramp variables
-            _throttle_low_end_pct = 0.0f;
+            _spin_up_ratio = 0.0f;
             _throttle_thrust_max = 0.0f;
             break;
 
@@ -396,25 +399,25 @@ void AP_MotorsMulticopter::output_logic()
             // set and increment ramp variables
             float spool_step = 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
             if (_spool_desired == DESIRED_SHUT_DOWN){
-                _throttle_low_end_pct -= spool_step;
+                _spin_up_ratio -= spool_step;
                 // constrain ramp value and update mode
-                if (_throttle_low_end_pct <= 0.0f) {
-                    _throttle_low_end_pct = 0.0f;
+                if (_spin_up_ratio <= 0.0f) {
+                    _spin_up_ratio = 0.0f;
                     _multicopter_flags.spool_mode = SHUT_DOWN;
                 }
             } else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
-                _throttle_low_end_pct += spool_step;
+                _spin_up_ratio += spool_step;
                 // constrain ramp value and update mode
-                if (_throttle_low_end_pct >= 1.0f) {
-                    _throttle_low_end_pct = 1.0f;
+                if (_spin_up_ratio >= 1.0f) {
+                    _spin_up_ratio = 1.0f;
                     _multicopter_flags.spool_mode = SPOOL_UP;
                 }
             } else {    // _spool_desired == SPIN_WHEN_ARMED
-                float spin_when_armed_low_end_pct = 0.0f;
+                float spin_up_armed_ratio = 0.0f;
                 if (_min_throttle > 0) {
-                    spin_when_armed_low_end_pct = (float)_spin_when_armed / _min_throttle;
+                    spin_up_armed_ratio = (float)_spin_when_armed / _min_throttle;
                 }
-                _throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct, -spool_step, spool_step);
+                _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
             }
             _throttle_thrust_max = 0.0f;
             break;
@@ -436,7 +439,7 @@ void AP_MotorsMulticopter::output_logic()
             }
 
             // set and increment ramp variables
-            _throttle_low_end_pct = 1.0f;
+            _spin_up_ratio = 1.0f;
             _throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
 
             // constrain ramp value and update mode
@@ -465,7 +468,7 @@ void AP_MotorsMulticopter::output_logic()
             }
 
             // set and increment ramp variables
-            _throttle_low_end_pct = 1.0f;
+            _spin_up_ratio = 1.0f;
             _throttle_thrust_max = get_current_limit_max_throttle();
             break;
 
@@ -486,7 +489,7 @@ void AP_MotorsMulticopter::output_logic()
             }
 
             // set and increment ramp variables
-            _throttle_low_end_pct = 1.0f;
+            _spin_up_ratio = 1.0f;
             _throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
 
             // constrain ramp value and update mode

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -128,6 +128,9 @@ protected:
     // convert thrust (0~1) range back to pwm range
     int16_t             calc_thrust_to_pwm(float thrust_in) const;
 
+    // calculate spin up to pwm range
+    int16_t             calc_spin_up_to_pwm() const;
+
     // apply any thrust compensation for the frame
     virtual void        thrust_compensation(void) {}
 
@@ -161,7 +164,7 @@ protected:
     float               _throttle_thrust_max;       // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
 
     // spool variables
-    float               _throttle_low_end_pct;      // throttle percentage (0 ~ 1) between zero and throttle_min
+    float               _spin_up_ratio;      // throttle percentage (0 ~ 1) between zero and throttle_min
 
     // battery voltage, current and air pressure compensation variables
     float               _batt_voltage_resting;  // battery voltage reading at minimum throttle

libraries/AP_Motors/AP_MotorsSingle.cpp

@@ -132,12 +132,12 @@ void AP_MotorsSingle::output_to_motors()
         case SPIN_WHEN_ARMED:
             // sends output to motors when armed but not flying
             hal.rcout->cork();
-            rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
-            rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
-            rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
-            rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
-            rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
-            rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
+            rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
+            rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
+            rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
+            rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
+            rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
+            rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
             hal.rcout->push();
             break;
         case SPOOL_UP:

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -137,9 +137,9 @@ void AP_MotorsTri::output_to_motors()
         case SPIN_WHEN_ARMED:
             // sends output to motors when armed but not flying
             hal.rcout->cork();
-            rc_write(AP_MOTORS_MOT_1, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
-            rc_write(AP_MOTORS_MOT_2, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
-            rc_write(AP_MOTORS_MOT_4, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
+            rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
+            rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());
+            rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm());
             rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
             hal.rcout->push();
             break;