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() @@ -103,6 +103,9 @@ 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 max_motor = 1000;
int16_t min_motor = 2000;
//int16_t yaw_contribution = 0;
// Throttle is 0 to 1000 only
@ -134,14 +137,27 @@ void AP_MotorsMatrix::output_armed() @@ -134,14 +137,27 @@ void AP_MotorsMatrix::output_armed()
}
// stability patch
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] && motor_out[i] > out_max ) {
if( opposite_motor[i] != AP_MOTORS_MATRIX_MOTOR_UNDEFINED ) {
motor_out[opposite_motor[i]] -= motor_out[i] - out_max;
}
motor_out[i] = out_max;
if( motor_enabled[i] && motor_out[i] > max_motor ) {
max_motor = motor_out[i];
}
}
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
if( _throttle_curve_enabled ) {

Loading…
Cancel
Save