sbus.cpp

00001 #include "sbus.h"
00002 
00003 Sbus::Sbus(PinName tx,PinName rx) : com(tx,rx) {
00004     com.baud(100000);
00005     com.attach(this, &Sbus::serialReceiveHandler, Serial::RxIrq);
00006     com.format(8, Serial::Even, 2);
00007     
00008     stickMaximumValue = 0x0690;
00009     stickNeutralValue = 0x0400;
00010     stickMinimumValue = 0x0170;
00011     stickResolution   = stickMaximumValue - stickMinimumValue;
00012     
00013     enableReceive = false;
00014     failsafe = SBUS_SIGNAL_FAILSAFE;
00015     
00016     resetAllData();
00017     receiveTimer.start();
00018 }
00019 
00020 void Sbus::setup(int ch5,int ch6,int ch7,int ch8,int ch9,int ch10) {
00021     swSelectData[0] = ch5;
00022     swSelectData[1] = ch6;
00023     swSelectData[2] = ch7;
00024     swSelectData[3] = ch8;
00025     swSelectData[4] = ch9;
00026     swSelectData[5] = ch10;
00027 }
00028 
00029 void Sbus::serialReceiveHandler() {
00030     static int data_count = 0;
00031     
00032     char buf;
00033     buf = com.getc();
00034     
00035     if(receiveTimer.read() > 0.006f)  {
00036         if(buf == 0x0F) {
00037             data_count = 0;
00038             enableReceive = true;
00039             receiveTimer.reset();
00040         } else{
00041             enableReceive = false; 
00042         }      
00043     }
00044     
00045     if(enableReceive)    receiveTimer.reset();
00046     
00047     if(enableReceive) {
00048         if(data_count < 25) {
00049             receiveTimer.reset();
00050             receiveData[data_count] = static_cast<int>(buf);
00051             data_count++;
00052         }
00053     }
00054     
00055     if(data_count == 25) {
00056         if(receiveData[0] == 0x0F) {
00057             decordReceiveData();
00058         } else {
00059             resetAllData();
00060         }
00061         data_count = 0;
00062         enableReceive = false;
00063     }
00064 }
00065 
00066 void Sbus::decordReceiveData() {
00067     channel[0]  = ((receiveData[1]    |receiveData[2]<<8)                 & 0x07FF);
00068     channel[1]  = ((receiveData[2]>>3 |receiveData[3]<<5)                 & 0x07FF);
00069     channel[2]  = ((receiveData[3]>>6 |receiveData[4]<<2 |receiveData[5]<<10)  & 0x07FF);
00070     channel[3]  = ((receiveData[5]>>1 |receiveData[6]<<7)                 & 0x07FF);
00071     channel[4]  = ((receiveData[6]>>4 |receiveData[7]<<4)                 & 0x07FF);
00072     channel[5]  = ((receiveData[7]>>7 |receiveData[8]<<1 |receiveData[9]<<9)   & 0x07FF);
00073     channel[6]  = ((receiveData[9]>>2 |receiveData[10]<<6)                & 0x07FF);
00074     channel[7]  = ((receiveData[10]>>5|receiveData[11]<<3)                & 0x07FF);
00075     channel[8]  = ((receiveData[12]   |receiveData[13]<<8)                & 0x07FF);
00076     channel[9]  = ((receiveData[13]>>3|receiveData[14]<<5)                & 0x07FF);
00077     channel[10] = ((receiveData[14]>>6|receiveData[15]<<2|receiveData[16]<<10) & 0x07FF);
00078     channel[11] = ((receiveData[16]>>1|receiveData[17]<<7)                & 0x07FF);
00079     channel[12] = ((receiveData[17]>>4|receiveData[18]<<4)                & 0x07FF);
00080     channel[13] = ((receiveData[18]>>7|receiveData[19]<<1|receiveData[20]<<9)  & 0x07FF);
00081     channel[14] = ((receiveData[20]>>2|receiveData[21]<<6)                & 0x07FF);
00082     channel[15] = ((receiveData[21]>>5|receiveData[22]<<3)                & 0x07FF);
00083     convertReceiveData();
00084 }
00085 
00086 void Sbus::convertReceiveData() {
00087     for(int i=ANALOG_RX;i<=ANALOG_LY;i++) {
00088         float buf = 0.0f;
00089         if(channel[i] > (stickNeutralValue + 20)) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
00090         else if(channel[i] < (stickNeutralValue - 20)) buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
00091         else buf = 0.0f;
00092         
00093         buf = static_cast<float>((buf*100)/100.0f);
00094         if(buf > 1.0f)          buf = 1.0f;
00095         else if(buf < -1.0f)    buf = -1.0f;
00096         
00097         switch (i) {
00098             case 0:
00099                 stickValue[ANALOG_RX] = buf;
00100                 break;
00101             case 1:
00102                 stickValue[ANALOG_LY] = -buf;
00103                 break;
00104             case 2:
00105                 stickValue[ANALOG_RY] = buf;
00106                 break;
00107             case 3:
00108                 stickValue[ANALOG_LX] = buf;
00109                 break;
00110         }
00111     }
00112     
00113     
00114     for(int i = 0; i < 6; i++) {
00115         if(swSelectData[i] == VR) {
00116             float vr_buf = 0.0f;
00117             if(channel[4] > (stickNeutralValue + 20)) vr_buf = ((channel[4] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
00118             else if(channel[4] < (stickNeutralValue - 20)) vr_buf = -((channel[4] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
00119             else vr_buf = 0.0f;
00120             
00121             vr_buf = (int)(vr_buf*100)/100.0f;
00122             if(vr_buf > 1.0f) vr_buf = 1.0f;
00123             else if(vr_buf < -1.0f) vr_buf = -1.0f;
00124             volumeValue = vr_buf;
00125         }
00126     }        
00127 
00128     for(int i = 0; i < 6; i++) {
00129         switch(channel[i + 4]) {
00130             case 0x078b:
00131             case 0x770:
00132                 switchValue[swSelectData[i]] = HIGH;
00133                 break;
00134             case 0x0400:
00135                 switchValue[swSelectData[i]] = NEUTRAL;
00136                 break;
00137             case 0x0090:
00138                 switchValue[swSelectData[i]] = LOW;
00139                 break;
00140             default:
00141                 switchValue[i] = NEUTRAL;
00142                 break;
00143         }
00144     }
00145     
00146     failsafe = SBUS_SIGNAL_OK;
00147     if (receiveData[23] & (1<<2)) {
00148         failsafe = SBUS_SIGNAL_LOST;
00149     }
00150     if (receiveData[23] & (1<<3)) {
00151         failsafe = SBUS_SIGNAL_FAILSAFE;
00152     }
00153 }
00154 
00155 void Sbus::resetAllData() {
00156     for(int i = 0; i < 16; i++) channel[i] = 0;
00157     for(int i = 0; i < 50; i++) receiveData[i] = 0;
00158     for(int i = 0; i < 4; i++) stickValue[i] = 0.0f;
00159     for(int i = 0; i < 12; i++) switchValue[i] = NEUTRAL;
00160     volumeValue = 0.0f;
00161 }
00162 
00163 float Sbus::getStickValue(int ch) {
00164     return stickValue[ch];
00165 }
00166 
00167 float Sbus::getVolumeValue() {
00168     return volumeValue;   
00169 }
00170 
00171 int Sbus::getSwitchValue(int ch) {
00172     return switchValue[ch];
00173 }
00174 
00175 int Sbus::getChannelValue(int ch) {
00176     return channel[ch];
00177 }
00178 
00179 int Sbus::getFailSafe() {
00180     return failsafe;   
00181 }