From 62cb5c172b1b1c11861a1fabad87081fab2cb4f9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 12 Sep 2013 22:27:44 +0900 Subject: [PATCH] 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 | 2 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 11 ++++- libraries/AP_Motors/AP_MotorsOcta.cpp | 4 +- libraries/AP_Motors/AP_MotorsOctaQuad.cpp | 2 +- libraries/AP_Motors/AP_MotorsQuad.cpp | 6 +-- libraries/AP_Motors/AP_Motors_Class.cpp | 49 +++++++++++++++++-- libraries/AP_Motors/AP_Motors_Class.h | 58 +++++++++++++---------- 7 files changed, 95 insertions(+), 37 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHexa.cpp b/libraries/AP_Motors/AP_MotorsHexa.cpp index f6934d2cf8..a5f2d62b8f 100644 --- a/libraries/AP_Motors/AP_MotorsHexa.cpp +++ b/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); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 2b7f233bc2..038b823b0a 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/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(); diff --git a/libraries/AP_Motors/AP_MotorsOcta.cpp b/libraries/AP_Motors/AP_MotorsOcta.cpp index 3a90761901..b82c0b91c1 100644 --- a/libraries/AP_Motors/AP_MotorsOcta.cpp +++ b/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); diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp index 4698225b54..20be604603 100644 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp +++ b/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); diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index fa3c40d6b5..94f07d3665 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/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); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index bdb5c45bde..d0b2a7ddc2 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/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; - AP_Notify::flags.armed = _armed; + _flags.armed = arm; + 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; + } +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index c75bf40722..a5eb4313a5 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/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() { - if( _armed ) { output_armed(); }else{ output_disarmed(); } - }; + void output(); // 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_disarmed() { - }; + virtual void output_armed() {}; + 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 - bool _armed; // true if motors are armed - uint8_t _frame_orientation; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2 - 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 _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 + uint16_t _speed_hz; // speed in hz to send updates to motors + 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 }; #endif // __AP_MOTORS_CLASS_H__