SBUS.cpp

00001 #include "mbed.h"
00002 #include "SBUS.h"
00003 
00004 SBUS::SBUS(PinName tx, PinName rx) :
00005 sbus(tx, rx),
00006 
00007 //CALIBRATE INPUT SIGNALS (USE getChannelValue)
00008 
00009 sbusMaximumValue(1811),
00010 sbusNeutralValue(992),
00011 sbusMinimumValue(172),
00012 sbusDeadband(10)
00013 
00014 {
00015     sbus.baud(100000);
00016     sbus.attach(callback(this, &SBUS::receiveData), Serial::RxIrq);      //Z TEMLE BEREMO UART ZMER KO PRIDEJO PODATKI
00017     sbus.format(8, Serial::Even, 2);
00018 }
00019 
00020 int SBUS::checkFailsafeTimer() {
00021     failsafetime = 12;
00022     if(lastreadtime < failsafetime){
00023         reportreadtime = lastreadtime;
00024         lastreadtime = failsafetime + 1;
00025         return 1;
00026     }
00027     else{
00028         return 0;
00029     }
00030 }
00031 
00032 int SBUS::failSafeTimerMs() {
00033     return reportreadtime;
00034 }
00035 
00036 float SBUS::getStickValue(int tag) {
00037     return stickValue[tag];
00038 }
00039 
00040 int SBUS::getChannelValue(int tag) {
00041     return channel[tag];
00042 }
00043 
00044 void SBUS::receiveData() {
00045     static int count = 0;
00046     char buf;
00047         
00048     buf = sbus.getc();
00049     if(count >= 25) return;
00050     receivedData[count] = buf;
00051     count++;
00052     
00053     if(count == 25 && receivedData[0] == 0x0F) {
00054         decordReceivedData();
00055         count = 0;
00056     }
00057 }
00058 
00059 void SBUS::decordReceivedData() {
00060     failsafetimer.stop();
00061     lastreadtime = failsafetimer.read_ms();
00062     failsafetimer.reset();
00063     failsafetimer.start();
00064     channel[0]  = ((receivedData[1]    |receivedData[2]<<8)                 & 0x07FF);
00065     channel[1]  = ((receivedData[2]>>3 |receivedData[3]<<5)                 & 0x07FF);
00066     channel[2]  = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10)  & 0x07FF);
00067     channel[3]  = ((receivedData[5]>>1 |receivedData[6]<<7)                 & 0x07FF);
00068     channel[4]  = ((receivedData[6]>>4 |receivedData[7]<<4)                 & 0x07FF);
00069     channel[5]  = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9)   & 0x07FF);
00070     channel[6]  = ((receivedData[9]>>2 |receivedData[10]<<6)                & 0x07FF);
00071     channel[7]  = ((receivedData[10]>>5|receivedData[11]<<3)                & 0x07FF);
00072     channel[8]  = ((receivedData[12]   |receivedData[13]<<8)                & 0x07FF);
00073     channel[9]  = ((receivedData[13]>>3|receivedData[14]<<5)                & 0x07FF);
00074     channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
00075     channel[11] = ((receivedData[16]>>1|receivedData[17]<<7)                & 0x07FF);
00076     channel[12] = ((receivedData[17]>>4|receivedData[18]<<4)                & 0x07FF);
00077     channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9)  & 0x07FF);
00078     channel[14] = ((receivedData[20]>>2|receivedData[21]<<6)                & 0x07FF);
00079     channel[15] = ((receivedData[21]>>5|receivedData[22]<<3)                & 0x07FF);
00080     convertReceivedData();
00081 }
00082 
00083 void SBUS::convertReceivedData() {
00084     for(int i = 0; i < 16; i++) {
00085         float buf;
00086         if(channel[i] > (sbusNeutralValue + sbusDeadband)){                                                           //THIS IS MIDDLE DEADBAND (DEAD ZONE)
00087             buf = ((float)(channel[i] - sbusNeutralValue) / (float)(sbusMaximumValue - sbusNeutralValue)); 
00088         }   
00089         else if(channel[i] < (sbusNeutralValue - sbusDeadband)){                                                      //THIS IS MIDDLE DEADBAND (DEAD ZONE)
00090             buf = -((float)(channel[i] - sbusNeutralValue) / (float)(sbusMinimumValue - sbusNeutralValue));
00091         }
00092         else buf = 0.0f;
00093         buf = (int)(buf*100)/100.0f;             //Limit output resolution to 2 decimals (x.yz)           
00094         if(buf > 1.0f){                  //Limit to -1.0 to 1.0
00095             buf = 1.0f;
00096         }
00097         if(buf < -1.0f){
00098             buf = -1.0f;
00099         }
00100         stickValue[i] = buf;
00101     }
00102 }