SBUS.cpp

00001 #include "mbed.h"
00002 #include "SBUS.h"
00003 
00004 SBUS::SBUS(PinName tx, PinName rx) :
00005 com(tx, rx),
00006 /*stickMaximumValue(0x068F),
00007 stickNeutralValue(0x03FF),
00008 stickMinimumValue(0x016F),*/
00009 stickMaximumValue(0x0690),
00010 stickNeutralValue(0x0400),
00011 stickMinimumValue(0x0170),
00012 stickResolution(stickMaximumValue - stickMinimumValue)
00013 {
00014     com.baud(100000);
00015     com.attach(this, &SBUS::receiveData, Serial::RxIrq);
00016     com.format(8, Serial::Even, 2);
00017     /*switchPositionValue[0] = 0x076F;
00018     switchPositionValue[1] = 0x03FF;
00019     switchPositionValue[2] = 0x008F;
00020     switchFPositionValue[0] = 0x076F;
00021     switchFPositionValue[1] = 0x0074;*/
00022     switchPositionValue[0] = 0x076F;
00023     switchPositionValue[1] = 0x03FF;
00024     switchPositionValue[2] = 0x008F;
00025     switchFPositionValue[0] = 0x076F;
00026     switchFPositionValue[1] = 0x0074;
00027     connstatus = false;
00028 }
00029 
00030 float SBUS::getStickValue(int tag) {
00031     return stickValue[tag];
00032 }
00033 
00034 int SBUS::getSwitchValue(int tag) {
00035     return switchValue[tag];
00036 }
00037 
00038 int SBUS::getChannelValue(int tag) {
00039     return channel[tag];
00040 }
00041 
00042 bool SBUS::getConnectStatus() {
00043     return connstatus;
00044 }
00045 
00046 void SBUS::receiveData() {
00047     static int count = 0;
00048     char buf;
00049     
00050     buf = com.getc();
00051     if(count >= 25) return;
00052     receivedData[count] = buf;
00053     count++;
00054     
00055     if(count == 25 && receivedData[0] == 0x0F) {
00056         decordReceivedData();
00057         count = 0;
00058         connstatus = true;
00059     } else {
00060         resetall();
00061         connstatus = false;
00062     }
00063 }
00064 
00065 void SBUS::decordReceivedData() {
00066     channel[0]  = ((receivedData[1]    |receivedData[2]<<8)                 & 0x07FF);
00067     channel[1]  = ((receivedData[2]>>3 |receivedData[3]<<5)                 & 0x07FF);
00068     channel[2]  = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10)  & 0x07FF);
00069     channel[3]  = ((receivedData[5]>>1 |receivedData[6]<<7)                 & 0x07FF);
00070     channel[4]  = ((receivedData[6]>>4 |receivedData[7]<<4)                 & 0x07FF);
00071     channel[5]  = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9)   & 0x07FF);
00072     channel[6]  = ((receivedData[9]>>2 |receivedData[10]<<6)                & 0x07FF);
00073     channel[7]  = ((receivedData[10]>>5|receivedData[11]<<3)                & 0x07FF);
00074     channel[8]  = ((receivedData[12]   |receivedData[13]<<8)                & 0x07FF);
00075     channel[9]  = ((receivedData[13]>>3|receivedData[14]<<5)                & 0x07FF);
00076     channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
00077     channel[11] = ((receivedData[16]>>1|receivedData[17]<<7)                & 0x07FF);
00078     channel[12] = ((receivedData[17]>>4|receivedData[18]<<4)                & 0x07FF);
00079     channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9)  & 0x07FF);
00080     channel[14] = ((receivedData[20]>>2|receivedData[21]<<6)                & 0x07FF);
00081     channel[15] = ((receivedData[21]>>5|receivedData[22]<<3)                & 0x07FF);
00082     convertReceivedData();
00083 }
00084 
00085 void SBUS::convertReceivedData() {
00086     for(int i = 0; i < 4; i++) {
00087         float buf;
00088         if(channel[i] > (stickNeutralValue + 20)) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
00089         else if(channel[i] < (stickNeutralValue - 20)) buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
00090         else buf = 0.0f;
00091         buf = (int)(buf*100)/100.0f;
00092         buf = (buf > 1.0f) ? 1.0f : buf;
00093         switch(i) {
00094             case right_RL:
00095                 stickValue[analog_rx] = buf;
00096                 break;
00097             case right_UD:
00098                 stickValue[analog_ry] = buf;
00099                 break;
00100             case left_RL:
00101                 stickValue[analog_lx] = buf;
00102                 break;
00103             case left_UD:
00104                 stickValue[analog_ly] = -buf;
00105                 break;
00106             default:
00107                 break;
00108         }
00109     }
00110     for(int i = 0; i < 12; i++) {
00111         switch(channel[i + 4]) {
00112             case 0x078b:
00113             case 0x770:
00114                 switchValue[i] = High;
00115                 break;
00116             case 0x0400:
00117                 switchValue[i] = Neutral;
00118                 break;
00119             case 0x0090:
00120                 switchValue[i] = Low;
00121                 break;
00122             default:
00123                 //switchValue[i] = 3;
00124                 break;
00125         }
00126     }
00127 }
00128 
00129 void SBUS::resetall() {
00130     for(int i = 0; i < 16; i++) channel[i] = 0;
00131     for(int i = 0; i < 50; i++) receivedData[i] = 0;
00132     for(int i = 0; i < 4; i++) stickValue[i] = 0.0f;
00133     for(int i = 0; i < 12; i++) switchValue[i] = Neutral;
00134 }