Copter Motors: add slow start feature

Combined armed, frame orientation and slow_start into flags bitmask
Removed ability to set max throttle because it was never used
Re-ordered class variables

libraries/AP_Motors/AP_MotorsHexa.cpp

@@ -29,7 +29,7 @@ void AP_MotorsHexa::setup_motors()
     AP_MotorsMatrix::setup_motors();
 
     // hard coded config for supported frames
-    if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+    if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
         // plus frame set-up
         add_motor(AP_MOTORS_MOT_1,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
         add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -59,7 +59,7 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
 void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
 {
     // return if nothing has changed
-    if( new_orientation == _frame_orientation ) {
+    if( new_orientation == _flags.frame_orientation ) {
         return;
     }
 
@@ -132,7 +132,14 @@ void AP_MotorsMatrix::output_armed()
 
     // Throttle is 0 to 1000 only
     // To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
-    _rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
+    if (_rc_throttle->servo_out < 0) {
+        _rc_throttle->servo_out = 0;
+        limit.throttle_lower = true;
+    }
+    if (_rc_throttle->servo_out > _max_throttle) {
+        _rc_throttle->servo_out = _max_throttle;
+        limit.throttle_upper = true;
+    }
 
     // capture desired roll, pitch, yaw and throttle from receiver
     _rc_roll->calc_pwm();

libraries/AP_Motors/AP_MotorsOcta.cpp

@@ -29,7 +29,7 @@ void AP_MotorsOcta::setup_motors()
     AP_MotorsMatrix::setup_motors();
 
     // hard coded config for supported frames
-    if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+    if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
         // plus frame set-up
         add_motor(AP_MOTORS_MOT_1,    0,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
         add_motor(AP_MOTORS_MOT_2,  180,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
@@ -40,7 +40,7 @@ void AP_MotorsOcta::setup_motors()
         add_motor(AP_MOTORS_MOT_7,  -90,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
         add_motor(AP_MOTORS_MOT_8,   90,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
 
-    }else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
+    }else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
         // V frame set-up
         add_motor_raw(AP_MOTORS_MOT_1,  1.0,  0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
         add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);

libraries/AP_Motors/AP_MotorsOctaQuad.cpp

@@ -29,7 +29,7 @@ void AP_MotorsOctaQuad::setup_motors()
     AP_MotorsMatrix::setup_motors();
 
     // hard coded config for supported frames
-    if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+    if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
         // plus frame set-up
         add_motor(AP_MOTORS_MOT_1,    0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
         add_motor(AP_MOTORS_MOT_2,  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);

libraries/AP_Motors/AP_MotorsQuad.cpp

@@ -29,21 +29,21 @@ void AP_MotorsQuad::setup_motors()
     AP_MotorsMatrix::setup_motors();
 
     // hard coded config for supported frames
-    if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
+    if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
         // plus frame set-up
         add_motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
         add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
         add_motor(AP_MOTORS_MOT_3,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
         add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
 
-    }else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
+    }else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
         // V frame set-up
         add_motor(AP_MOTORS_MOT_1,   45,  0.7981,  1);
         add_motor(AP_MOTORS_MOT_2, -135,  1.0000,  3);
         add_motor(AP_MOTORS_MOT_3,  -45, -0.7981,  4);
         add_motor(AP_MOTORS_MOT_4,  135, -1.0000,  2);
 
-    }else if( _frame_orientation == AP_MOTORS_H_FRAME ) {
+    }else if( _flags.frame_orientation == AP_MOTORS_H_FRAME ) {
         // H frame set-up - same as X but motors spin in opposite directiSons
         add_motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
         add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -62,8 +62,6 @@ AP_Motors::AP_Motors( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_
     _rc_throttle(rc_throttle),
     _rc_yaw(rc_yaw),
     _speed_hz(speed_hz),
-    _armed(false),
-    _frame_orientation(0),
     _min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
     _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
     _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE)
@@ -94,8 +92,8 @@ void AP_Motors::Init()
 
 void AP_Motors::armed(bool arm)
 {
-    _armed = arm;
+    _flags.armed = arm;
-    AP_Notify::flags.armed = _armed;
+    AP_Notify::flags.armed = arm;
 };
 
 // set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
@@ -124,6 +122,20 @@ void AP_Motors::throttle_pass_through()
     }
 }
 
+// output - sends commands to the motors
+void AP_Motors::output()
+{
+    // update max throttle
+    update_max_throttle();
+
+    // output to motors
+    if (_flags.armed ) {
+        output_armed();
+    }else{
+        output_disarmed();
+    }
+};
+
 // setup_throttle_curve - used to linearlise thrust output by motors
 // returns true if set up successfully
 bool AP_Motors::setup_throttle_curve()
@@ -160,3 +172,32 @@ bool AP_Motors::setup_throttle_curve()
 
     return retval;
 }
+
+// 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_Motors::slow_start(bool true_false)
+{
+    // set slow_start flag
+    _flags.slow_start = true;
+
+    // initialise maximum throttle to current throttle
+    _max_throttle = constrain_int16(_rc_throttle->servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
+}
+
+// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
+void AP_Motors::update_max_throttle()
+{
+    // return max throttle if we're not slow_starting
+    if (!_flags.slow_start) {
+        return;
+    }
+
+    // increase slow start throttle
+    _max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
+
+    // turn off slow start if we've reached max throttle
+    if (_max_throttle >= _rc_throttle->servo_out) {
+        _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
+        _flags.slow_start = false;
+    }
+}

libraries/AP_Motors/AP_Motors_Class.h

@@ -55,6 +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
+#define AP_MOTOR_SLOW_START_INCREMENT   10
+
 /// @class      AP_Motors
 class AP_Motors {
 public:
@@ -81,26 +84,23 @@ public:
     virtual void        set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; };
 
     // set frame orientation (normally + or X)
-    virtual void        set_frame_orientation( uint8_t new_orientation ) { _frame_orientation = new_orientation; };
+    virtual void        set_frame_orientation( uint8_t new_orientation ) { _flags.frame_orientation = new_orientation; };
 
     // enable - starts allowing signals to be sent to motors
     virtual void        enable() = 0;
 
     // arm, disarm or check status status of motors
-    bool                armed() { return _armed; };
+    bool                armed() { return _flags.armed; };
     void                armed(bool arm);
 
     // set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
     void                set_min_throttle(uint16_t min_throttle);
-    void                set_max_throttle(uint16_t max_throttle) { _max_throttle = max_throttle; };
     // set_mid_throttle - sets the mid throttle which is close to the hover throttle of the copter
     // 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_mid_throttle(uint16_t mid_throttle);
 
     // output - sends commands to the motors
-    void        output() {
+    void                output();
-        if( _armed ) { output_armed(); }else{ output_disarmed(); }
-    };
 
     // output_min - sends minimum values out to the motors
     virtual void        output_min() = 0;
@@ -118,6 +118,10 @@ public:
     // 1 if motor is enabled, 0 otherwise
     bool                motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
 
+    // 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);
+
     // final output values sent to the motors.  public (for now) so that they can be access for logging
     int16_t             motor_out[AP_MOTORS_MAX_NUM_MOTORS];
 
@@ -135,26 +139,32 @@ public:
 protected:
 
     // output functions that should be overloaded by child classes
-    virtual void        output_armed() {
+    virtual void        output_armed() {};
-    };
+    virtual void        output_disarmed() {};
-    virtual void        output_disarmed() {
-    };
 
+    // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
+    void                update_max_throttle();
+
+    // flag bitmask
+    struct AP_Motors_flags {
+        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
+    } _flags;
+
+    // parameters
+    AP_CurveInt16_Size4 _throttle_curve;        // curve used to linearize the pwm->thrust
+    AP_Int8             _throttle_curve_enabled;        // enable throttle curve
+    AP_Int8             _throttle_curve_mid;    // throttle which produces 1/2 the maximum thrust.  expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
+    AP_Int8             _throttle_curve_max;    // throttle which produces the maximum thrust.  expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
+    AP_Int8             _spin_when_armed;       // used to control whether the motors always spin when armed.  pwm value above radio_min 
+
+    // internal variables
     RC_Channel*         _rc_roll, *_rc_pitch, *_rc_throttle, *_rc_yaw;  // input in from users
     uint8_t             _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS];        // mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2
-    uint16_t            _speed_hz;                      // speed in hz to send updates to motors
+    uint16_t            _speed_hz;              // speed in hz to send updates to motors
-    bool                _armed;                         // true if motors are armed
+    int16_t             _min_throttle;          // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
-    uint8_t             _frame_orientation;     // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2
+    int16_t             _max_throttle;          // the maximum throttle to be sent to the motors (sometimes limited by slow start)
-    int16_t             _min_throttle;          // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
+    int16_t             _hover_out;             // the estimated hover throttle in pwm (i.e. 1000 ~ 2000).  calculated from the THR_MID parameter
-    int16_t             _max_throttle;          // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
-    AP_CurveInt16_Size4 _throttle_curve;                // curve used to linearize the pwm->thrust
-    AP_Int8             _throttle_curve_enabled;        // enable throttle curve
-    AP_Int8             _throttle_curve_mid;  // throttle which produces 1/2 the maximum thrust.  expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
-    AP_Int8             _throttle_curve_max;  // throttle which produces the maximum thrust.  expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
-
-    // for new stability patch
-    int16_t             _hover_out;                     // the estimated hover throttle in pwm (i.e. 1000 ~ 2000).  calculated from the THR_MID parameter
-
-    AP_Int8             _spin_when_armed;       // used to control whether the motors always spin when armed.  pwm value above radio_min 
 };
 #endif  // __AP_MOTORS_CLASS_H__