From f3a2db195e1dadee023035fa18cbeac7ae597517 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 25 Jul 2013 15:34:04 +0900 Subject: [PATCH] Copter Motors: remove old style stability patch --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 187 ------------------------ libraries/AP_Motors/AP_MotorsMatrix.h | 3 - 2 files changed, 190 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index c3be8cffa6..804a9e6845 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -89,7 +89,6 @@ void AP_MotorsMatrix::output_min() } } -#ifdef AP_MOTORS_MATRIX_SCALING_STABILITY_PATCH // output_armed - sends commands to the motors // includes new scaling stability patch void AP_MotorsMatrix::output_armed() @@ -277,192 +276,6 @@ void AP_MotorsMatrix::output_armed() } } } -#else -// output_armed - sends commands to the motors -void AP_MotorsMatrix::output_armed() -{ - int8_t i; - int16_t out_min = _rc_throttle->radio_min; - int16_t out_max = _rc_throttle->radio_max; - int16_t rc_yaw_constrained_pwm; - int16_t rc_yaw_excess; - int16_t upper_margin, lower_margin; - int16_t motor_adjustment = 0; - int16_t yaw_to_execute = 0; - - // initialize limits flag - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle = false; - - // Throttle is 0 to 1000 only - _rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_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(); - - // if we are not sending a throttle output, we cut the motors - if(_rc_throttle->servo_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 ) { - limit.roll_pitch = true; - } - if( _rc_yaw->pwm_out != 0 ) { - limit.yaw = true; - } - } 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; iwrite(_motor_to_channel_map[i], motor_out[i]); - } - } -} -#endif // AP_MOTORS_MATRIX_SCALING_STABILITY_PATCH // output_disarmed - sends commands to the motors void AP_MotorsMatrix::output_disarmed() diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 9ca3007528..ef7e0953bb 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -11,9 +11,6 @@ #include // RC Channel Library #include "AP_Motors_Class.h" -// comment out the line below to return to stability patch used in ArduCopter ver 2.8.1 ~ 2.9.1b -#define AP_MOTORS_MATRIX_SCALING_STABILITY_PATCH - #define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1 #define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1