jarno
/
ThrottleQuadrant
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

v1.2 

Improved signal filtering. DELTA_MIN defines treshold for ignoring small changes, DELTA_MAX defines treshold for ignoring sudden large changes from e.g. faulty potentiometers. Not perfect yet.
This commit is contained in:
Jarno Rankinen 2020-01-14 10:07:59 +02:00
parent 95fa7c4b34
commit 7b8ec5e03f
1 changed files with 67 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
ThrottleQuadrant.ino
View File

@ -35,6 +35,9 @@
* *
* v1.11 Added rule to ignore spike values outside min-max range * v1.11 Added rule to ignore spike values outside min-max range
* *
* v1.2 Improved signal filtering. DELTA_MIN defines treshold for ignoring small changes, DELTA_MAX defines treshold for ignoring sudden large changes
* from e.g. faulty potentiometers. Not perfect yet.
*
**********************************************************************************/ **********************************************************************************/
@ -45,7 +48,9 @@
//constants to filter input value glitching //constants to filter input value glitching
const uint8_t DELTA_MIN = 2; const uint8_t DELTA_MIN = 2;
const uint8_t DELTA_MAX = 30; const uint8_t DELTA_MAX = 10;
bool update = false;
uint16_t throttle_l = 0; // raw data from the slider uint16_t throttle_l = 0; // raw data from the slider
uint16_t throttle_lmin = 14, throttle_lmax = 236; //min & max values for calibration uint16_t throttle_lmin = 14, throttle_lmax = 236; //min & max values for calibration
@ -90,15 +95,15 @@ unsigned int delta(uint16_t a, uint16_t b) {
void setup() { void setup() {
//Check that these are correct for your build //Check that these are correct for your build
pinMode(A0, INPUT_PULLUP); //xa pinMode(A0, INPUT_PULLUP); //left throttle, X-axis
pinMode(A1, INPUT_PULLUP); //ya pinMode(A1, INPUT_PULLUP); //right throttle, Y-axis
pinMode(A2, INPUT_PULLUP); //xb pinMode(A2, INPUT_PULLUP); //left prop, Z-axis
pinMode(A3, INPUT_PULLUP); //yb pinMode(A3, INPUT_PULLUP); //right prop, X-rotation axis
pinMode(A4, INPUT_PULLUP); //xc pinMode(A4, INPUT_PULLUP); //left mixture, Y-rotation axis
pinMode(A5, INPUT_PULLUP); //yc pinMode(A5, INPUT_PULLUP); //right mixture, rudder axis
Serial.begin(9600); Serial.begin(9600);
Joystick.begin(); Joystick.begin(false);
tl_old = throttle_l = analogRead(A0); tl_old = throttle_l = analogRead(A0);
tr_old = throttle_r = analogRead(A1); tr_old = throttle_r = analogRead(A1);
@ -114,32 +119,32 @@ void loop() {
throttle_l = analogRead(A0); throttle_l = analogRead(A0);
if (calib == false && (throttle_l > throttle_lmax)) throttle_l = throttle_lmax; if (calib == false && (throttle_l > throttle_lmax)) throttle_l = throttle_lmax;
if (calib == false && (throttle_l < throttle_lmin)) throttle_l = throttle_lmin; if (calib == false && (throttle_l < throttle_lmin)) throttle_l = throttle_lmin;
throttle_l_axis = 256*throttle_l/(throttle_lmax-throttle_lmin+5) - 128 - throttle_lmin; //convert raw values -> -128 - 128 (roughly, joystick calibration is still recommended via e.g. Windows joystick interface)
throttle_r = analogRead(A1); throttle_r = analogRead(A1);
if (calib == false && (throttle_r > throttle_rmax)) throttle_r = throttle_rmax; if (calib == false && (throttle_r > throttle_rmax)) throttle_r = throttle_rmax;
if (calib == false && (throttle_r < throttle_rmin)) throttle_r = throttle_rmin; if (calib == false && (throttle_r < throttle_rmin)) throttle_r = throttle_rmin;
throttle_r_axis = 256*throttle_r/(throttle_rmax-throttle_rmin+5) - 128 - throttle_rmin;
prop_l = analogRead(A2); prop_l = analogRead(A2);
if (calib == false && (prop_l > prop_lmax)) prop_l = prop_lmax; if (calib == false && (prop_l > prop_lmax)) prop_l = prop_lmax;
if (calib == false && (prop_l < prop_lmin)) prop_l = prop_lmin; if (calib == false && (prop_l < prop_lmin)) prop_l = prop_lmin;
prop_l_axis = 256*prop_l/(prop_lmax-prop_lmin+5) - 128 - prop_lmin;
prop_r = analogRead(A3); prop_r = analogRead(A3);
if (calib == false && (prop_r > prop_rmax)) prop_r = prop_rmax; if (calib == false && (prop_r > prop_rmax)) prop_r = prop_rmax;
if (calib == false && (prop_r < prop_rmin)) prop_r = prop_rmin; if (calib == false && (prop_r < prop_rmin)) prop_r = prop_rmin;
prop_r_axis = prop_r * rotation_multip - (prop_rmin + 5);
mix_l = analogRead(A4); mix_l = analogRead(A4);
if (calib == false && (mix_l > mix_lmax)) mix_l = mix_lmax; if (calib == false && (mix_l > mix_lmax)) mix_l = mix_lmax;
if (calib == false && (mix_l < mix_lmin)) mix_l = mix_lmin; if (calib == false && (mix_l < mix_lmin)) mix_l = mix_lmin;
mix_l_axis = mix_l * rotation_multip - (mix_lmin + 5);
mix_r = analogRead(A5); mix_r = analogRead(A5);
if (calib == false && (mix_r > mix_rmax)) mix_r = mix_rmax; if (calib == false && (mix_r > mix_rmax)) mix_r = mix_rmax;
if (calib == false && (mix_r < mix_rmin)) mix_r = mix_rmin; if (calib == false && (mix_r < mix_rmin)) mix_r = mix_rmin;
mix_r_axis = 254*(mix_r - mix_rmin)/(mix_rmax - mix_rmin);
if(calib==true) { //Serial monitor for calibration if(calib==true) { //Serial monitor for calibration
Serial.println(); Serial.println();
@ -162,58 +167,55 @@ void loop() {
Serial.print(" -> "); Serial.print(" -> ");
Serial.print("mix_r_axis=");Serial.println(mix_r_axis); Serial.print("mix_r_axis=");Serial.println(mix_r_axis);
} }
//Serial.println(mix_l);
//Serial.println(mix_r); if (DELTA_MIN<delta(throttle_l, tl_old) && delta(throttle_l, tl_old)<DELTA_MAX) {
//Serial.println(mix_r_axis); throttle_l_axis = 256*throttle_l/(throttle_lmax-throttle_lmin+5) - 128 - throttle_lmin; //convert raw values -> -128 - 128 (roughly, joystick calibration is still recommended via e.g. Windows joystick interface)
/*
Joystick.setXAxis(throttle_l_axis); Joystick.setXAxis(throttle_l_axis);
tl_old = throttle_l;
update = true;
}
else if (DELTA_MIN<delta(throttle_r, tr_old) && delta(throttle_r, tr_old)<DELTA_MAX) {
throttle_r_axis = 256*throttle_r/(throttle_rmax-throttle_rmin+5) - 128 - throttle_rmin;
Joystick.setYAxis(throttle_r_axis); Joystick.setYAxis(throttle_r_axis);
tr_old = throttle_r;
update = true;
}
else if (DELTA_MIN<delta(prop_l, pl_old) && delta(prop_l, pl_old)<DELTA_MAX) {
prop_l_axis = 256*prop_l/(prop_lmax-prop_lmin+5) - 128 - prop_lmin;
Joystick.setZAxis(prop_l_axis); Joystick.setZAxis(prop_l_axis);
pl_old = prop_l;
update = true;
}
else if (DELTA_MIN<delta(prop_r, pr_old) && delta(prop_r, pr_old)<DELTA_MAX) {
prop_r_axis = prop_r * rotation_multip - (prop_rmin + 5);
Joystick.setXAxisRotation(prop_r_axis); Joystick.setXAxisRotation(prop_r_axis);
pr_old = prop_r;
update = true;
}
else if (DELTA_MIN<delta(mix_l, ml_old) && delta(mix_l, ml_old)<DELTA_MAX) {
mix_l_axis = mix_l * rotation_multip - (mix_lmin + 5);
Joystick.setYAxisRotation(mix_l_axis); Joystick.setYAxisRotation(mix_l_axis);
ml_old = mix_l;
update = true;
}
else if (DELTA_MIN<delta(mix_r, mr_old) && delta(mix_r, mr_old)<DELTA_MAX) {
mix_r_axis = 254*(mix_r - mix_rmin)/(mix_rmax - mix_rmin);
Joystick.setRudder(mix_r_axis); Joystick.setRudder(mix_r_axis);
mr_old = mix_r;
update = true;
}
/* else update=false;
if (delta(throttle_l_axis, tl_old)>3 || delta(throttle_r_axis, tr_old)>3 || delta(prop_l_axis, pl_old)>3 || delta(prop_r_axis, pr_old)>3 || delta(mix_l_axis, ml_old)>3 || delta(mix_r_axis, mr_old)>3){
if (delta(throttle_l_axis, tl_old)<50 || delta(throttle_r_axis, tr_old)<50 || delta(prop_l_axis, pl_old)<50 || delta(prop_r_axis, pr_old)<50 || delta(mix_l_axis, ml_old)<50 || delta(mix_r_axis, mr_old)<50){
tl_old = throttle_l_axis;
tr_old = throttle_r_axis;
pl_old = prop_l_axis;
pr_old = prop_r_axis;
ml_old = mix_l_axis;
mr_old = mix_r_axis;
if (update==true) {
Joystick.sendState(); Joystick.sendState();
} update=false;
}
*/
if ((delta(throttle_l_axis, tl_old)>DELTA_MIN || delta(throttle_r_axis, tr_old)>DELTA_MIN) && (delta(throttle_l_axis, tl_old)<DELTA_MAX || delta(throttle_r_axis, tr_old))<DELTA_MAX) {
Joystick.setXAxis(throttle_l_axis);
Joystick.setYAxis(throttle_r_axis);
tl_old = throttle_l_axis;
tr_old = throttle_r_axis;
} }
if ((delta(prop_l_axis, pl_old)>DELTA_MIN || delta(prop_r_axis, pr_old)>DELTA_MIN) && (delta(prop_l_axis, pl_old)<DELTA_MAX || delta(prop_r_axis, pr_old)<DELTA_MAX)) { Serial.println(delta(mr_old, mix_r));
Joystick.setZAxis(prop_l_axis);
Joystick.setXAxisRotation(prop_r_axis);
pl_old = prop_l_axis;
pr_old = prop_r_axis;
}
if ((delta(mix_l_axis, ml_old)>DELTA_MIN || delta(mix_r_axis, mr_old)>DELTA_MIN) && (delta(mix_l_axis, ml_old)<DELTA_MAX || delta(mix_r_axis, mr_old)<DELTA_MAX)) {
Joystick.setYAxisRotation(mix_l_axis);
Joystick.setRudder(mix_r_axis);
ml_old = mix_l_axis;
mr_old = mix_r_axis;
}
delay(20);
} }