From 1df891e2ce76d550436564a7cc1c50814d2b859c Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 10 Oct 2012 15:54:13 +0900 Subject: [PATCH] AP_Motors: added reached_limit method which returns bit mask indicating which control inputs could not be achieved --- libraries/AP_Motors/AP_Motors.h | 12 ++ libraries/AP_Motors/AP_MotorsMatrix.cpp | 271 +++++++++++++----------- 2 files changed, 155 insertions(+), 128 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index 6b2bf7b51f..d3e7cf4903 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -50,6 +50,12 @@ #define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100) #define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100) +// bit mask for recording which limits we have reached when outputting to motors +#define AP_MOTOR_NO_LIMITS_REACHED 0x00 +#define AP_MOTOR_ROLLPITCH_LIMIT 0x01 +#define AP_MOTOR_YAW_LIMIT 0x02 +#define AP_MOTOR_THROTTLE_LIMIT 0x04 + /// @class AP_Motors class AP_Motors { public: @@ -120,6 +126,11 @@ public: virtual void output_min() { }; + // reached_limits - return whether we hit the limits of the motors + virtual uint8_t reached_limit( uint8_t which_limit = 0x00 ) { + return _reached_limit & which_limit; + } + // get basic information about the platform virtual uint8_t get_num_motors() { return 0; @@ -169,6 +180,7 @@ protected: 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 + uint8_t _reached_limit; // bit mask to record which motor limits we hit (if any) during most recent output. Used to provide feedback to attitude controllers }; #endif // AP_MOTORS \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 2a02d82c48..0f5420be23 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -109,142 +109,18 @@ void AP_MotorsMatrix::output_armed() int16_t motor_adjustment = 0; int16_t yaw_to_execute = 0; + // initialize reached_limit flag + _reached_limit = AP_MOTOR_NO_LIMITS_REACHED; + // Throttle is 0 to 1000 only _rc_throttle->servo_out = constrain(_rc_throttle->servo_out, 0, _max_throttle); - if(_rc_throttle->servo_out > 0) - out_min = _rc_throttle->radio_min + _min_throttle; - // capture desired roll, pitch, yaw and throttle from receiver _rc_roll->calc_pwm(); _rc_pitch->calc_pwm(); _rc_throttle->calc_pwm(); _rc_yaw->calc_pwm(); - // initialise rc_yaw_contrained_pwm that we will certainly output and rc_yaw_excess that we will do on best-efforts basis. - // Note: these calculations and many others below depend upon _yaw_factors always being 0, -1 or 1. - if( _rc_yaw->pwm_out < -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) { - rc_yaw_constrained_pwm = -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; - rc_yaw_excess = _rc_yaw->pwm_out+AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; - }else if( _rc_yaw->pwm_out > AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) { - rc_yaw_constrained_pwm = AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; - rc_yaw_excess = _rc_yaw->pwm_out-AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; - }else{ - rc_yaw_constrained_pwm = _rc_yaw->pwm_out; - rc_yaw_excess = 0; - } - - // initialise upper and lower margins - upper_margin = lower_margin = out_max - out_min; - - // add roll, pitch, throttle and constrained yaw for each motor - for( i=0; iradio_out + - _rc_roll->pwm_out * _roll_factor[i] + - _rc_pitch->pwm_out * _pitch_factor[i] + - rc_yaw_constrained_pwm * _yaw_factor[i]; - - // calculate remaining room between fastest running motor and top of pwm range - if( out_max - motor_out[i] < upper_margin) { - upper_margin = out_max - motor_out[i]; - } - // calculate remaining room between slowest running motor and bottom of pwm range - if( motor_out[i] - out_min < lower_margin ) { - lower_margin = motor_out[i] - out_min; - } - } - } - - // if motors are running too fast and we have enough room below, lower overall throttle - if( upper_margin < 0 || lower_margin < 0 ) { - - // calculate throttle adjustment that equalizes upper and lower margins. We will never push the throttle beyond this point - motor_adjustment = (upper_margin - lower_margin) / 2; // i.e. if overflowed by 20 on top, 30 on bottom, upper_margin = -20, lower_margin = -30. will adjust motors -5. - - // if we have overflowed on the top, reduce but no more than to the mid point - if( upper_margin < 0 ) { - motor_adjustment = max(upper_margin, motor_adjustment); - } - - // if we have underflowed on the bottom, increase throttle but no more than to the mid point - if( lower_margin < 0 ) { - motor_adjustment = min(-lower_margin, motor_adjustment); - } - } - - // move throttle up or down to to pull within tolerance - if( motor_adjustment != 0 ) { - for( i=0; i 0 && _yaw_factor[i] > 0 ) { - yaw_to_execute = min(yaw_to_execute, upper_margin); - } - - // motor is decreasing, check lower limit - if( rc_yaw_excess > 0 && _yaw_factor[i] < 0 ) { - yaw_to_execute = min(yaw_to_execute, lower_margin); - } - - // motor is decreasing, check lower limit - if( rc_yaw_excess < 0 && _yaw_factor[i] > 0 ) { - yaw_to_execute = max(yaw_to_execute, -lower_margin); - } - - // motor is increasing, check upper limit - if( rc_yaw_excess < 0 && _yaw_factor[i] < 0 ) { - yaw_to_execute = max(yaw_to_execute, -upper_margin); - } - } - } - // check yaw_to_execute is reasonable - if( yaw_to_execute != 0 && ((yaw_to_execute>0 && rc_yaw_excess>0) || (yaw_to_execute<0 && rc_yaw_excess<0)) ) { - // add the additional yaw - for( i=0; iservo_out == 0) { for( i=0; iradio_min; } } + // if we have any roll, pitch or yaw input then it's breaching the limit + if( _rc_roll->pwm_out != 0 || _rc_pitch->pwm_out != 0 ) { + _reached_limit |= AP_MOTOR_ROLLPITCH_LIMIT; + } + if( _rc_yaw->pwm_out != 0 ) { + _reached_limit |= AP_MOTOR_YAW_LIMIT; + } + } else { // non-zero throttle + + out_min = _rc_throttle->radio_min + _min_throttle; + + // initialise rc_yaw_contrained_pwm that we will certainly output and rc_yaw_excess that we will do on best-efforts basis. + // Note: these calculations and many others below depend upon _yaw_factors always being 0, -1 or 1. + if( _rc_yaw->pwm_out < -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) { + rc_yaw_constrained_pwm = -AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; + rc_yaw_excess = _rc_yaw->pwm_out+AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; + }else if( _rc_yaw->pwm_out > AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM ) { + rc_yaw_constrained_pwm = AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; + rc_yaw_excess = _rc_yaw->pwm_out-AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM; + }else{ + rc_yaw_constrained_pwm = _rc_yaw->pwm_out; + rc_yaw_excess = 0; + } + + // initialise upper and lower margins + upper_margin = lower_margin = out_max - out_min; + + // add roll, pitch, throttle and constrained yaw for each motor + for( i=0; iradio_out + + _rc_roll->pwm_out * _roll_factor[i] + + _rc_pitch->pwm_out * _pitch_factor[i] + + rc_yaw_constrained_pwm * _yaw_factor[i]; + + // calculate remaining room between fastest running motor and top of pwm range + if( out_max - motor_out[i] < upper_margin) { + upper_margin = out_max - motor_out[i]; + } + // calculate remaining room between slowest running motor and bottom of pwm range + if( motor_out[i] - out_min < lower_margin ) { + lower_margin = motor_out[i] - out_min; + } + } + } + + // if motors are running too fast and we have enough room below, lower overall throttle + if( upper_margin < 0 || lower_margin < 0 ) { + + // calculate throttle adjustment that equalizes upper and lower margins. We will never push the throttle beyond this point + motor_adjustment = (upper_margin - lower_margin) / 2; // i.e. if overflowed by 20 on top, 30 on bottom, upper_margin = -20, lower_margin = -30. will adjust motors -5. + + // if we have overflowed on the top, reduce but no more than to the mid point + if( upper_margin < 0 ) { + motor_adjustment = max(upper_margin, motor_adjustment); + } + + // if we have underflowed on the bottom, increase throttle but no more than to the mid point + if( lower_margin < 0 ) { + motor_adjustment = min(-lower_margin, motor_adjustment); + } + } + + // move throttle up or down to to pull within tolerance + if( motor_adjustment != 0 ) { + for( i=0; i 0 && _yaw_factor[i] > 0 ) { + yaw_to_execute = min(yaw_to_execute, upper_margin); + } + + // motor is decreasing, check lower limit + if( rc_yaw_excess > 0 && _yaw_factor[i] < 0 ) { + yaw_to_execute = min(yaw_to_execute, lower_margin); + } + + // motor is decreasing, check lower limit + if( rc_yaw_excess < 0 && _yaw_factor[i] > 0 ) { + yaw_to_execute = max(yaw_to_execute, -lower_margin); + } + + // motor is increasing, check upper limit + if( rc_yaw_excess < 0 && _yaw_factor[i] < 0 ) { + yaw_to_execute = max(yaw_to_execute, -upper_margin); + } + } + } + // check yaw_to_execute is reasonable + if( yaw_to_execute != 0 && ((yaw_to_execute>0 && rc_yaw_excess>0) || (yaw_to_execute<0 && rc_yaw_excess<0)) ) { + // add the additional yaw + for( i=0; i