// Tested with Flysky radio+arduino nano+raspberry pi+navio board (and PPM Example program set to 4 channels). // Accuracy seems to be in the 10's of microseconds (looks like a 1.0% spread with 6 channels at 50hz) // Modified to use direct pin access and fixed point math where needed // The arduino will need to be dedicated to the task or you probably will introduce timing inaccuracy // feel free to swipe/modify the code, no licence on this one. but no promises either ;) const long numberofchannels = 6; // 4,5,6,7,8 only const int channel1pin = 2; // D2 These are not configurable in this version (unless you know how to replace the direct pin references within the code) const int channel2pin = 3; // D3 const int channel3pin = 4; // D4 const int channel4pin = 5; // D5 const int channel5pin = 6; // D6 const int channel6pin = 7; // D7 const int channel7pin = 8; const int channel8pin = 9; const int outputpin = 10; const int ppmfrequency = 50; // hz, you can change this but the lower frequency the better the accuaracy const int ppmchannelinterval = 64;// millionths of a second, probably doesnt need to be altered const int ppmpulseinterval = 5000;// minimum should be 4000 this is the sync period (it will go high for this period of time) // next 2 values are *10000 (using fixed point math) const long pwmbasei = 500; // minimum pwm pulse time (pwm on radio's normally have a 1-2ms pulse, with a ~1ms pulse high initally followed by the actual timing // 1ms is represented on a 50hz radio as 0.05*the full cycle. you can 0 this if you want ppm to replicate the way pwm works const long pwmscalei = 20; // 20*0.05=1 so use the whole ppm bandwidth for this channel (the rest of the 1-2ms pwm pulse 1-2=1, // if you zero the pwmbase this should be 10 as 2ms (complete pwm pulse)*10=1.0) const long radiolosstime = 50000; // time without a channel 1 high before radio loss is assumed. const long ppmtime = 1000000/ppmfrequency; const unsigned long ppmpulsespace = (ppmtime-ppmchannelinterval*(numberofchannels+1)-ppmpulseinterval);// constant that defines the "free" space available for all channels int pinstate1 = 0; int pinstate2 = 0; int pinstate3 = 0; int pinstate4 = 0; int pinstate5 = 0; int pinstate6 = 0; int pinstate7 = 0; int pinstate8 = 0; unsigned long pintimeup1 = 0; unsigned long pintimeup2 = 0; unsigned long pintimeup3 = 0; unsigned long pintimeup4 = 0; unsigned long pintimeup5 = 0; unsigned long pintimeup6 = 0; unsigned long pintimeup7 = 0; unsigned long pintimeup8 = 0; unsigned long pintimedown1 = 0; unsigned long pintimedown2 = 0; unsigned long pintimedown3 = 0; unsigned long pintimedown4 = 0; unsigned long pintimedown5 = 0; unsigned long pintimedown6 = 0; unsigned long pintimedown7 = 0; unsigned long pintimedown8 = 0; long pinpwm1 = 0; long pinpwm2 = 0; long pinpwm3 = 0; long pinpwm4 = 0; long pinpwm5 = 0; long pinpwm6 = 0; long pinpwm7 = 0; long pinpwm8 = 0; unsigned long pinpwmppm1 = 0; unsigned long pinpwmppm2 = 0; unsigned long pinpwmppm3 = 0; unsigned long pinpwmppm4 = 0; unsigned long pinpwmppm5 = 0; unsigned long pinpwmppm6 = 0; unsigned long pinpwmppm7 = 0; unsigned long pinpwmppm8 = 0; long pinppmdowntime = 0; unsigned long pinppmtime = 0; int ppmstate = 0; void setup() { pinMode(channel1pin, INPUT); pinMode(channel2pin, INPUT); pinMode(channel3pin, INPUT); pinMode(channel4pin, INPUT); pinMode(channel5pin, INPUT); pinMode(channel6pin, INPUT); pinMode(channel7pin, INPUT); pinMode(channel8pin, INPUT); pinMode(outputpin, OUTPUT); // Serial.begin(115200); } int val = 0; int pvald = 0; const unsigned int pinsd = 1<<PORTD2 | 1<<PORTD3 | 1<<PORTD4 | 1<<PORTD5 | 1<<PORTD6 | 1<<PORTD7; int counter = 0; unsigned long ptime = 0; void loop() { // loop is improve accuracy while still freeing up at the end so OS stuff can run for (int r=0;r<1000;r++) { unsigned long currenttime = micros(); // only accurate to 16us // need to cope with overflow... (its every 71 mins with unsigned longs) if (currenttime<ptime) {// probably reset, for now we'll just reset all the other times pintimedown1 = 0; pintimeup1 = 0; pinstate1 = 0; pintimedown2 = 0; pintimeup2 = 0; pinstate2 = 0; pintimedown3 = 0; pintimeup3 = 0; pinstate3 = 0; pintimedown4 = 0; pintimeup4 = 0; pinstate4 = 0; pintimedown5 = 0; pintimeup5 = 0; pinstate5 = 0; pintimedown6 = 0; pintimeup6 = 0; pinstate6 = 0; pintimedown7 = 0; pintimeup7 = 0; pinstate7 = 0; pintimedown8 = 0; pintimeup8 = 0; pinstate8 = 0; pinppmtime = 0; ppmstate = 0; } ptime = currenttime; // pin1 //val = digitalRead(channel1pin); val = PIND & pinsd; // detect a pin changed on PIND if (val!=pvald) { // if something changed on PIND pvald = val; val = PIND & 1<<PORTD2; if ((val!=0) && (pinstate1==0)) {// start of high so record the time and move to next state if ((pintimedown1>0) && (pintimeup1>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm1 = pintimeup1-pintimedown1; long t = (pintimeup1-pintimedown1)+(currenttime-pintimedown1); pinpwm1 = pinpwm1*10000/t; /* counter++; if (counter>50) { Serial.print(pinpwm1);Serial.print(" ");Serial.print(pinpwm2);Serial.print(" ");Serial.print(pinpwm3);Serial.print(" ");Serial.print(pinpwm4);Serial.print(" ");Serial.print("\n"); counter = 0; }*/ } pintimedown1 = currenttime; pinstate1 = 1; } else if ((val==0) && (pinstate1==1)) { pintimeup1 = currenttime; pinstate1 = 0; } // val = digitalRead(channel2pin); val = PIND & 1<<PORTD3; if ((val!=0) && (pinstate2==0)) {// start of high so record the time and move to next state if ((pintimedown2>0) && (pintimeup2>0)) { pinpwm2 = pintimeup2-pintimedown2; long t = (pintimeup2-pintimedown2)+(currenttime-pintimedown2); pinpwm2 = pinpwm2*10000/t; } pintimedown2 = currenttime; pinstate2 = 1; } else if ((val==0) && (pinstate2==1)) { pintimeup2 = currenttime; pinstate2 = 0; } // val = digitalRead(channel3pin); val = PIND & 1<<PORTD4; if ((val!=0) && (pinstate3==0)) {// start of high so record the time and move to next state if ((pintimedown3>0) && (pintimeup3>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm3 = pintimeup3-pintimedown3; long t = (pintimeup3-pintimedown3)+(currenttime-pintimedown3); pinpwm3 = pinpwm3*10000/t; } pintimedown3 = currenttime; pinstate3 = 1; } else if ((val==0) && (pinstate3==1)) { pintimeup3 = currenttime; pinstate3 = 0; } // val = digitalRead(channel4pin); val = PIND & 1<<PORTD5; if ((val!=0) && (pinstate4==0)) {// start of high so record the time and move to next state if ((pintimedown4>0) && (pintimeup4>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm4 = pintimeup4-pintimedown4; long t = (pintimeup4-pintimedown4)+(currenttime-pintimedown4); pinpwm4 = pinpwm4*10000/t; } pintimedown4 = currenttime; pinstate4 = 1; } else if ((val==0) && (pinstate4==1)) { pintimeup4 = currenttime; pinstate4 = 0; } if (numberofchannels>4) { // val = digitalRead(channel5pin); val = PIND & 1<<PORTD6; if ((val!=0) && (pinstate5==0)) {// start of high so record the time and move to next state if ((pintimedown5>0) && (pintimeup5>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm5 = pintimeup5-pintimedown5; long t = (pintimeup5-pintimedown5)+(currenttime-pintimedown5); pinpwm5 = pinpwm5*10000/t; } pintimedown5 = currenttime; pinstate5 = 1; } else if ((val==0) && (pinstate5==1)) { pintimeup5 = currenttime; pinstate5 = 0; } if (numberofchannels>5) {// read channel6 //val = digitalRead(channel6pin); val = PIND & 1<<PORTD7; if ((val!=0) && (pinstate6==0)) {// start of high so record the time and move to next state if ((pintimedown6>0) && (pintimeup6>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm6 = pintimeup6-pintimedown6; long t = (pintimeup6-pintimedown6)+(currenttime-pintimedown6); pinpwm6 = pinpwm6*10000/t; } pintimedown6 = currenttime; pinstate6 = 1; } else if ((val==0) && (pinstate6==1)) { pintimeup6 = currenttime; pinstate6 = 0; } }// end of numberofchannels>5 }// end of numberofchannels>4 }// end of val!=pvald if (numberofchannels>6) { // val = digitalRead(channel7pin); val = PINB & 1<<PORTB0; if ((val!=0) && (pinstate7==0)) {// start of high so record the time and move to next state if ((pintimedown7>0) && (pintimeup7>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm7 = pintimeup7-pintimedown7; long t = (pintimeup7-pintimedown7)+(currenttime-pintimedown7); pinpwm7 = pinpwm7*10000/t; } pintimedown7 = currenttime; pinstate7 = 1; } else if ((val==0) && (pinstate7==1)) { pintimeup7 = currenttime; pinstate7 = 0; } if (numberofchannels>7) { //val = digitalRead(channel8pin); val = PINB & 1<<PORTB1; if ((val!=0) && (pinstate8==0)) {// start of high so record the time and move to next state if ((pintimedown8>0) && (pintimeup8>0)) { // we have both values, pwm is pintimeup1/(pintimeup1+pintimedown1) pinpwm8 = pintimeup8-pintimedown8; long t = (pintimeup8-pintimedown8)+(currenttime-pintimedown8); pinpwm8 = pinpwm8*10000/t; } pintimedown8 = currenttime; pinstate8 = 1; } else if ((val==0) && (pinstate8==1)) { pintimeup8 = currenttime; pinstate8 = 0; } }// end of numberofchannels>7 }// end of numberofchannels>6 if (ppmstate==0) { if ((currenttime-pintimeup1)>radiolosstime) {// loss of radio signal ppmstate = 100; } } if (ppmstate==100) { if ((currenttime-pintimeup1)<radiolosstime) {// radio back! ppmstate = 0; } } if (currenttime>=pinppmtime) { if (ppmstate<9)// creating a binary tree for the if's should be an improvement over the if then else's flat out (and according to other people's tests with case statements might be better) { if (ppmstate<5) { if (ppmstate==0) { pinpwmppm1 = (pinpwm1-pwmbasei)*pwmscalei; // these values will be between 500 and 1000 for normal RC receivers pinpwmppm2 = (pinpwm2-pwmbasei)*pwmscalei; // been done this way so it also copes with other PWM sources/frequencies pinpwmppm3 = (pinpwm3-pwmbasei)*pwmscalei; pinpwmppm4 = (pinpwm4-pwmbasei)*pwmscalei; pinpwmppm5 = (pinpwm5-pwmbasei)*pwmscalei; pinpwmppm6 = (pinpwm6-pwmbasei)*pwmscalei; pinpwmppm7 = (pinpwm7-pwmbasei)*pwmscalei; pinpwmppm8 = (pinpwm8-pwmbasei)*pwmscalei; // end of pin1 should be currenttime+(tmp-ppmchannelinterval*4-ppmpulseinterval)*pinpwmppm1 pinppmtime = (unsigned long)(currenttime+(ppmpulsespace*pinpwmppm1)/(numberofchannels*10000)); pinppmdowntime = (long)((ppmpulsespace*(10000-pinpwmppm1))/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=1; } else if (ppmstate==1) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); ppmstate = 2; pinppmtime+=ppmchannelinterval; // } } else if (ppmstate==2) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm2/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm2)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=3; // } } else if (ppmstate==3) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); ppmstate = 4; pinppmtime+=ppmchannelinterval; // } } else if (ppmstate==4) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm3/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm3)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=5; // } } }else{ if (ppmstate==5) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); ppmstate = 6; pinppmtime+=ppmchannelinterval; // } } else if (ppmstate==6) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm4/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm4)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=7; // } } else if (ppmstate==7) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); pinppmtime+=ppmchannelinterval; if (numberofchannels>4) ppmstate = 8; else ppmstate = 16; // } } else if (ppmstate==8) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm5/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm5)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=9; // } } } }else{ if (ppmstate<14) { if (ppmstate==9) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); if (numberofchannels>5) ppmstate = 10; else ppmstate = 16; pinppmtime+=ppmchannelinterval; // } } else if (ppmstate==10) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm6/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm6)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=11; // } } else if (ppmstate==11) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); if (numberofchannels>6) ppmstate = 12; else ppmstate = 16; pinppmtime+=ppmchannelinterval; // } } else if (ppmstate==12) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm7/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm7)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=13; // } } else if (ppmstate==13) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); if (numberofchannels>7) ppmstate = 14; else ppmstate = 16; pinppmtime+=ppmchannelinterval; // } } }else{ if (ppmstate==14) { // if (currenttime>pinppmtime) // { pinppmtime = (unsigned long)(currenttime+ppmpulsespace*pinpwmppm8/(numberofchannels*10000)); pinppmdowntime += (long)(ppmpulsespace*(10000-pinpwmppm8)/(numberofchannels*10000)); //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); ppmstate=15; // } } else if (ppmstate==15) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); ppmstate = 16; pinppmtime+=ppmchannelinterval; // } } else if (ppmstate==16) { // if (currenttime>pinppmtime) // { // now for a long high so the other side can sync up (just change this to LOW if it uses a LOW sync) //digitalWrite(outputpin,HIGH); PORTB |= _BV(PORTB2); // PORTB &= ~_BV(PORTB2); // for LOW ppmstate = 17; pinppmtime = currenttime+ppmpulseinterval+pinppmdowntime; //pinppmdowntime is to flesh out the buffer so it maintains the correct frequency updates // } } else if (ppmstate==17) { // if (currenttime>pinppmtime) // { //digitalWrite(outputpin,LOW); PORTB &= ~_BV(PORTB2); ppmstate=18; // loop around pinppmtime = currenttime+ppmchannelinterval; // } } else if (ppmstate==18) { // if (currenttime>pinppmtime) // { ppmstate=0; // loop around // } } } } } } } </plaintext>