Browse Source

Change to the "Stability Patch" in Motors_Matrix in order to make it cover both the upper throttle range and lower, and also to cause it to raise/lower all motors when a single motor goes out of ranges.

mission-4.1.18
Robert Lefebvre 13 years ago
parent
commit
9e32d45d7b
  1. 26
      libraries/AP_Motors/AP_MotorsMatrix.cpp

26
libraries/AP_Motors/AP_MotorsMatrix.cpp

@ -103,6 +103,9 @@ void AP_MotorsMatrix::output_armed()
int8_t i; int8_t i;
int16_t out_min = _rc_throttle->radio_min; int16_t out_min = _rc_throttle->radio_min;
int16_t out_max = _rc_throttle->radio_max; int16_t out_max = _rc_throttle->radio_max;
int16_t max_motor = 1000;
int16_t min_motor = 2000;
//int16_t yaw_contribution = 0; //int16_t yaw_contribution = 0;
// Throttle is 0 to 1000 only // Throttle is 0 to 1000 only
@ -134,14 +137,27 @@ void AP_MotorsMatrix::output_armed()
} }
// stability patch // stability patch
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && motor_out[i] > out_max ) { if( motor_enabled[i] && motor_out[i] > max_motor ) {
if( opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED ) { max_motor = motor_out[i];
motor_out[opposite_motor[i]] -= motor_out[i] - out_max; }
} }
motor_out[i] = out_max; if ( max_motor > out_max ){
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
motor_out[i] -= (max_motor - out_max);
}
}
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && motor_out[i] < min_motor ) {
min_motor = motor_out[i];
} }
} }
if ( min_motor < out_min ){
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
motor_out[i] -= (min_motor - out_min);
}
}
// adjust for throttle curve // adjust for throttle curve
if( _throttle_curve_enabled ) { if( _throttle_curve_enabled ) {

Loading…
Cancel
Save