AP_MotorsMulticopter: output_logic implements spooling

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -143,21 +143,14 @@ void AP_MotorsMulticopter::output()
     // calc filtered battery voltage and lift_max
     update_lift_max_from_batt_voltage();
 
-    // move throttle_low_comp towards desired throttle low comp
-    update_throttle_rpy_mix();
+    // run spool logic
+    output_logic();
 
-    if (_flags.armed) {
-        if (!_flags.interlock) {
-            output_armed_zero_throttle();
-        } else if (_flags.stabilizing) {
+    // calculate thrust
     output_armed_stabilizing();
-        } else {
-            output_armed_not_stabilizing();
-        }
-    } else {
-        _multicopter_flags.slow_start_low_end = true;
-        output_disarmed();
-    }
+
+    // convert rpy_thrust values to pwm
+    output_to_motors();
 };
 
 // update the throttle input filter
@@ -381,6 +374,163 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad
     _min_throttle = (float)min_throttle * _throttle_pwm_scalar;   
 }
 
+void AP_MotorsMulticopter::output_logic()
+{
+    // force desired and current spool mode if disarmed or not interlocked
+    if (!_flags.armed || !_flags.interlock) {
+        _multicopter_flags.spool_desired = DESIRED_SHUT_DOWN;
+        _multicopter_flags.spool_mode = SHUT_DOWN;
+        _multicopter_flags.slow_start_low_end = true;
+    }
+
+    switch (_multicopter_flags.spool_mode) {
+        case SHUT_DOWN:
+            // Motors should be stationary.
+            // Servos set to their trim values or in a test condition.
+
+            // set limits flags
+            limit.roll_pitch = true;
+            limit.yaw = true;
+            limit.throttle_lower = true;
+            limit.throttle_upper = true;
+
+            // make sure the motors are spooling in the correct direction
+            if(_multicopter_flags.spool_desired != DESIRED_SHUT_DOWN){
+                _multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
+                break;
+            }
+
+            // set and increment ramp variables
+            _throttle_low_end_pct = 0.0f;
+            _throttle_thrust_max = 0.0f;
+            _throttle_rpy_mix = 0.0f;
+            _throttle_rpy_mix_desired = 0.0f;
+            break;
+
+        case SPIN_WHEN_ARMED:{
+            // Motors should be stationary or at spin when armed.
+            // Servoes should be moving to correct the current attitude.
+
+            // set limits flags
+            limit.roll_pitch = true;
+            limit.yaw = true;
+            limit.throttle_lower = true;
+            limit.throttle_upper = true;
+
+            // set and increment ramp variables
+            float spool_step = 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
+            if(_multicopter_flags.spool_desired == DESIRED_SHUT_DOWN){
+                _throttle_low_end_pct -= spool_step;
+            } else if(_multicopter_flags.spool_desired == DESIRED_THROTTLE_UNLIMITED ){
+                _throttle_low_end_pct += spool_step;
+            }else{
+                _throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct()-_throttle_low_end_pct, -spool_step, spool_step);
+            }
+            _throttle_thrust_max = 0.0f;
+            _throttle_rpy_mix = 0.0f;
+            _throttle_rpy_mix_desired = 0.0f;
+
+            // constrain ramp value and update mode
+            if (_throttle_low_end_pct <= 0.0f) {
+                _throttle_low_end_pct = 0.0f;
+                _multicopter_flags.spool_mode = SHUT_DOWN;
+            } else if (_throttle_low_end_pct >= 1.0f) {
+                _throttle_low_end_pct = 1.0f;
+                _multicopter_flags.spool_mode = SPOOL_UP;
+            }
+            break;
+        }
+        case SPOOL_UP:
+            // Maximum throttle should move from minimum to maximum.
+            // Servoes should exhibit normal flight behavior.
+
+            // initialize limits flags
+            limit.roll_pitch = false;
+            limit.yaw = false;
+            limit.throttle_lower = false;
+            limit.throttle_upper = false;
+
+            // make sure the motors are spooling in the correct direction
+            if(_multicopter_flags.spool_desired != DESIRED_THROTTLE_UNLIMITED ){
+                _multicopter_flags.spool_mode = SPOOL_DOWN;
+                break;
+            }
+
+            // set and increment ramp variables
+            _throttle_low_end_pct = 1.0f;
+            _throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
+            _throttle_rpy_mix = 0.0f;
+            _throttle_rpy_mix_desired = 0.0f;
+
+            // constrain ramp value and update mode
+            if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
+                _throttle_thrust_max = get_current_limit_max_throttle();
+                _multicopter_flags.spool_mode = THROTTLE_UNLIMITED;
+            } else if (_throttle_thrust_max < 0.0f) {
+                _throttle_thrust_max = 0.0f;
+            }
+            break;
+
+        case THROTTLE_UNLIMITED:
+            // Throttle should exhibit normal flight behavior.
+            // Servoes should exhibit normal flight behavior.
+
+            // initialize limits flags
+            limit.roll_pitch = false;
+            limit.yaw = false;
+            limit.throttle_lower = false;
+            limit.throttle_upper = false;
+
+            // make sure the motors are spooling in the correct direction
+            if(_multicopter_flags.spool_desired != DESIRED_THROTTLE_UNLIMITED ){
+                _multicopter_flags.spool_mode = SPOOL_DOWN;
+                break;
+            }
+
+            // set and increment ramp variables
+            _throttle_low_end_pct = 1.0f;
+            _throttle_thrust_max = get_current_limit_max_throttle();
+            update_throttle_rpy_mix();
+            break;
+
+        case SPOOL_DOWN:
+            // Maximum throttle should move from maximum to minimum.
+            // Servoes should exhibit normal flight behavior.
+
+            // initialize limits flags
+            limit.roll_pitch = false;
+            limit.yaw = false;
+            limit.throttle_lower = false;
+            limit.throttle_upper = false;
+
+            // make sure the motors are spooling in the correct direction
+            if(_multicopter_flags.spool_desired == DESIRED_THROTTLE_UNLIMITED ){
+                _multicopter_flags.spool_mode = SPOOL_UP;
+                break;
+            }
+
+            // set and increment ramp variables
+            _throttle_low_end_pct = 1.0f;
+            _throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
+            _throttle_rpy_mix -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
+            _throttle_rpy_mix_desired = _throttle_rpy_mix;
+
+            // constrain ramp value and update mode
+            if (_throttle_thrust_max <= 0.0f){
+                _throttle_thrust_max = 0.0f;
+            }
+            if (_throttle_rpy_mix <= 0.0f){
+                _throttle_rpy_mix = 0.0f;
+            }
+            if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
+                _throttle_thrust_max = get_current_limit_max_throttle();
+            } else if (is_zero(_throttle_thrust_max) && is_zero(_throttle_rpy_mix)) {
+                _multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
+            }
+            break;
+    }
+}
+
 // slow_start - set to true to slew motors from current speed to maximum
 // Note: this must be set immediately before a step up in throttle
 void AP_MotorsMulticopter::slow_start(bool true_false)

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -39,6 +39,9 @@
  #define AP_MOTOR_SLOW_START_LOW_END_INCREMENT   1       // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second)
 #endif
 
+// spool definition
+#define AP_MOTORS_SPOOL_UP_TIME         0.5f    // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
+
 /// @class      AP_MotorsMulticopter
 class AP_MotorsMulticopter : public AP_Motors {
 public:
@@ -49,6 +52,9 @@ public:
     // output - sends commands to the motors
     virtual void        output();
 
+    // output_to_motors - sends commands to the motors
+    virtual void        output_to_motors(){};
+
     // set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
     void                set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
 
@@ -73,6 +79,26 @@ public:
     // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
     void                set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; }
 
+    // spool up states
+    enum spool_up_down_mode {
+        SHUT_DOWN = 0,                      // all motors stop
+        SPIN_WHEN_ARMED = 1,                // all motors at spin when armed
+        SPOOL_UP = 2,                       // increasing maximum throttle while stabilizing
+        THROTTLE_UNLIMITED = 3,                  // throttle is no longer constrained by start up procedure
+        SPOOL_DOWN = 4,                     // decreasing maximum throttle while stabilizing
+    };
+
+    // spool up states
+    enum spool_up_down_desired {
+        DESIRED_SHUT_DOWN = 0,              // all motors stop
+        DESIRED_SPIN_WHEN_ARMED = 1,        // all motors at spin when armed
+        DESIRED_SPIN_MIN_THROTTLE = 2,      // all motors at min throttle
+        DESIRED_THROTTLE_UNLIMITED = 3,          // throttle is no longer constrained by start up procedure
+    };
+
+    void                set_desired_spool_state(enum spool_up_down_desired spool) { _multicopter_flags.spool_desired = spool;}
+    void                output_logic();
+
     // slow_start - set to true to slew motors from current speed to maximum
     // Note: this must be set immediately before a step up in throttle
     void                slow_start(bool true_false);
@@ -146,6 +172,8 @@ protected:
 
     // flag bitmask
     struct {
+        spool_up_down_mode     spool_mode       : 4;    // motor's current spool mode
+        spool_up_down_desired  spool_desired    : 2;    // caller's desired spool mode
         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
     } _multicopter_flags;
@@ -170,6 +198,10 @@ 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 as pct * 10 (i.e. 0 ~ 1000)
+    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
 
     // battery voltage, current and air pressure compensation variables
     float               _batt_voltage_resting;  // battery voltage reading at minimum throttle