SBUS to PPM decoder

/* * This example decodes a Futaba SBUS or compatible stream and convert it to PPM. * * It is based on: * - https://github.com/zendes/SBUS * - http://forum.arduino.cc/index.php?topic=99708.0 * - https://mbed.org/users/Digixx/notebook/futaba-s-bus-controlled-by-mbed/ * * There are some inexpensibe SBUS to PPM decoders, such as * http://www.hobbyking.com/hobbyking/store/__37953__frsky_4_channel_s_bus_to_pwm_decoder.html * This code is more a proof of concept than a real alternative. * * This code uses the RX0 as input. It's not possible to use a software serial port, because * there are too many conflicting interrupts between serial input and PWM generation. * * SoftwareServo could be replaced with the Servo library, but which cannot define the refresh * delay. I used it just for fun. * * Since RX0 is also used to communicate with the FTDI, the SBUS must be disconnected while * flashing the Arduino. * * Furthermore, the SBUS uses an inverted serial communication. Why should we do simple when * it's so much fun to do it hard ? Thanks to Futaba for this oddity. The serial signal must * be inverted, priorly to the RX0. This can be done with this inverter: * http://www.hobbyking.com/hobbyking/store/__24523__ZYX_S_S_BUS_Connection_Cable.html * (the shortest end goes to the receiver, the longest to the Arduino) */#include<SoftwareServo.h>// refresh delay for PPM (18 ms is good for any servo)#define PPM_REFRESH_DELAY 18// position goes from -100 to +100 - this ratio converts it to an angle// standard ratio is around 60#define PPM_ANGLE 60 SoftwareServomyServo1;SoftwareServomyServo2;SoftwareServomyServo3;SoftwareServomyServo4;voidsetup(){Serial.begin(100000);myServo1.attach(2);myServo2.attach(3);myServo3.attach(4);myServo4.attach(5);}voidloop(){staticbytebuffer[25];staticintchannels[18];staticinterrors=0;staticboolfailsafe=0;staticintidx;staticunsignedlonglast_refresh=0;staticintlost=0;byteb;inti;if(Serial.available()){b=Serial.read();if(idx==0&&b!=0x0F){// start byte// error - wait for the start byte}else{buffer[idx++]=b;// fill the buffer}if(idx==25){// decodeidx=0;if(buffer[24]!=0x00){errors++;}else{channels[0]=((buffer[1]|buffer[2]<<8)&0x07FF);channels[1]=((buffer[2]>>3|buffer[3]<<5)&0x07FF);channels[2]=((buffer[3]>>6|buffer[4]<<2|buffer[5]<<10)&0x07FF);channels[3]=((buffer[5]>>1|buffer[6]<<7)&0x07FF);channels[4]=((buffer[6]>>4|buffer[7]<<4)&0x07FF);channels[5]=((buffer[7]>>7|buffer[8]<<1|buffer[9]<<9)&0x07FF);channels[6]=((buffer[9]>>2|buffer[10]<<6)&0x07FF);channels[7]=((buffer[10]>>5|buffer[11]<<3)&0x07FF);channels[8]=((buffer[12]|buffer[13]<<8)&0x07FF);channels[9]=((buffer[13]>>3|buffer[14]<<5)&0x07FF);channels[10]=((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10)&0x07FF);channels[11]=((buffer[16]>>1|buffer[17]<<7)&0x07FF);channels[12]=((buffer[17]>>4|buffer[18]<<4)&0x07FF);channels[13]=((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9)&0x07FF);channels[14]=((buffer[20]>>2|buffer[21]<<6)&0x07FF);channels[15]=((buffer[21]>>5|buffer[22]<<3)&0x07FF);channels[16]=((buffer[23])&0x0001)?2047:0;channels[17]=((buffer[23]>>1)&0x0001)?2047:0;failsafe=((buffer[23]>>3)&0x0001)?1:0;if((buffer[23]>>2)&0x0001)lost++;if((millis()-last_refresh)>=PPM_REFRESH_DELAY){myServo1.write(toPct(channels[0])*PPM_ANGLE/100+90);myServo2.write(toPct(channels[1])*PPM_ANGLE/100+90);myServo3.write(toPct(channels[2])*PPM_ANGLE/100+90);myServo4.write(toPct(channels[3])*PPM_ANGLE/100+90);SoftwareServo::refresh();last_refresh=millis();}}}}}inttoPct(intval){return(int)lround(val/9.92)-100;}