mirror of https://github.com/ArduPilot/ardupilot
ArduCopter motors_hexa: rewrite janky loops to use ch_of_mot
* Previously the loop was written over channels, now it is over motors * the correct channel for that motor is determined by ch_of_mot. * ch_of_mot is defined correctly based on the config_channels macros.
This commit is contained in:
parent
b907c1fd5c
commit
f7e14fff37
|
@ -67,13 +67,12 @@ static void output_motors_armed()
|
||||||
|
|
||||||
|
|
||||||
// Tridge's stability patch
|
// Tridge's stability patch
|
||||||
for (int i = CH_1; i<=CH_8; i++) {
|
for (int m = 0; m <= 6; m++) {
|
||||||
if(i == CH_5 || i == CH_6)
|
int c = ch_of_mot(m);
|
||||||
continue;
|
int c_opp = ch_of_mot(m^1); // m^1 is the opposite motor. c_opp is channel of opposite motor.
|
||||||
if (motor_out[i] > out_max) {
|
if (motor_out[c] > out_max) {
|
||||||
// note that i^1 is the opposite motor
|
motor_out[c_opp] -= motor_out[c] - out_max;
|
||||||
motor_out[i^1] -= motor_out[i] - out_max;
|
motor_out[c] = out_max;
|
||||||
motor_out[i] = out_max;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -99,14 +98,13 @@ static void output_motors_armed()
|
||||||
|
|
||||||
// this filter slows the acceleration of motors vs the deceleration
|
// this filter slows the acceleration of motors vs the deceleration
|
||||||
// Idea by Denny Rowland to help with his Yaw issue
|
// Idea by Denny Rowland to help with his Yaw issue
|
||||||
for(int8_t i = CH_1; i <= CH_8; i++ ) {
|
for(int8_t m = 0; m <= 6; m++ ) {
|
||||||
if(i == CH_5 || i == CH_6)
|
int c = ch_of_mot(m);
|
||||||
continue;
|
if(motor_filtered[c] < motor_out[c]){
|
||||||
if(motor_filtered[i] < motor_out[i]){
|
motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
|
||||||
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
|
|
||||||
}else{
|
}else{
|
||||||
// don't filter
|
// don't filter
|
||||||
motor_filtered[i] = motor_out[i];
|
motor_filtered[c] = motor_out[c];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue