Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
Robert Lefebvre 2012-09-19 20:42:04 -04:00
parent 25bcfdd1e7
commit 739d31f06e
1 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -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()
}
// 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 ) {