diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 469588cd44..68946423c5 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -97,12 +97,25 @@ static void output_motors_armed() } #endif - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); + // this filter slows the acceleration of motors vs the deceleration + // Idea by Denny Rowland to help with his Yaw issue + for(int8_t i = CH_1; i <= CH_8; i++ ) { + if(i == CH_5 || i == CH_6) + continue; + if(motor_filtered[i] < motor_out[i]){ + motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + }else{ + // don't filter + motor_filtered[i] = motor_out[i]; + } + } + + APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); + APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); + APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); + APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); + APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); + APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); #if INSTANT_PWM == 1 // InstantPWM diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index 207ad26e58..34a3baec2c 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -148,14 +148,27 @@ static void output_motors_armed() } #endif - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); - APM_RC.OutputCh(CH_10, motor_out[CH_10]); - APM_RC.OutputCh(CH_11, motor_out[CH_11]); + // this filter slows the acceleration of motors vs the deceleration + // Idea by Denny Rowland to help with his Yaw issue + for(int8_t i = CH_1; i <= CH_11; i++ ) { + if(i == CH_5 || i == CH_6 || i == CH_9) + continue; + if(motor_filtered[i] < motor_out[i]){ + motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + }else{ + // don't filter + motor_filtered[i] = motor_out[i]; + } + } + + APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); + APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); + APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); + APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); + APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); + APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); + APM_RC.OutputCh(CH_10, motor_filtered[CH_10]); + APM_RC.OutputCh(CH_11, motor_filtered[CH_11]); #if INSTANT_PWM == 1 // InstantPWM diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 5a77c1a406..aab2d894aa 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -116,14 +116,27 @@ static void output_motors_armed() } #endif - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); - APM_RC.OutputCh(CH_10, motor_out[CH_10]); - APM_RC.OutputCh(CH_11, motor_out[CH_11]); + // this filter slows the acceleration of motors vs the deceleration + // Idea by Denny Rowland to help with his Yaw issue + for(int8_t i = CH_1; i <= CH_11; i++ ) { + if(i == CH_5 || i == CH_6 || i == CH_9) + continue; + if(motor_filtered[i] < motor_out[i]){ + motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + }else{ + // don't filter + motor_filtered[i] = motor_out[i]; + } + } + + APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); + APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); + APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); + APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); + APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); + APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); + APM_RC.OutputCh(CH_10, motor_filtered[CH_10]); + APM_RC.OutputCh(CH_11, motor_filtered[CH_11]); #if INSTANT_PWM == 1 // InstantPWM diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index 718b0f96a8..6b2dec9b7f 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -89,10 +89,21 @@ static void output_motors_armed() } #endif - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); + // this filter slows the acceleration of motors vs the deceleration + // Idea by Denny Rowland to help with his Yaw issue + for(int8_t i=CH_1; i <= CH_4; i++ ) { + if(motor_filtered[i] < motor_out[i]){ + motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + }else{ + // don't filter + motor_filtered[i] = motor_out[i]; + } + } + + APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); + APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); + APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); + APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); #if INSTANT_PWM == 1 // InstantPWM diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index cbe0ba81d1..6e045b366c 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -106,12 +106,25 @@ static void output_motors_armed() } #endif - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); + // this filter slows the acceleration of motors vs the deceleration + // Idea by Denny Rowland to help with his Yaw issue + for(int8_t i = CH_1; i <= CH_8; i++ ) { + if(i == CH_5 || i == CH_6) + continue; + if(motor_filtered[i] < motor_out[i]){ + motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + }else{ + // don't filter + motor_filtered[i] = motor_out[i]; + } + } + + APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); + APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); + APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); + APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); + APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); + APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); #if INSTANT_PWM == 1 // InstantPWM