mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 18:38:28 -04:00
ArduCopter motors_octa: 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
f055cbc720
commit
ad5c31cd38
@ -150,14 +150,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_11; i++ ) {
|
for(int8_t m = 0; m <= 8; m++ ) {
|
||||||
if(i == CH_5 || i == CH_6 || i == CH_9)
|
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
Block a user