SBUS.cpp

00001 #include "mbed.h"
00002 #include "SBUS.h"
00003 
00004 SBUS::SBUS(PinName TX, PinName RX) : myserial(TX, RX)
00005 {
00006     myserial.baud(100000);
00007     myserial.format(8, Serial::Even, 2);
00008 
00009     myserial.attach(this, &SBUS::_serialEvent, Serial::RxIrq);
00010     dataIndex = 0;
00011     failSafe = true;
00012 }
00013 
00014 void SBUS::_serialEvent()
00015 {
00016     int val = myserial.getc();
00017 
00018     if(dataIndex == 0 && val != SBUS_SYNCBYTE)
00019         return;
00020 
00021     rcvData[dataIndex] = val;
00022     dataIndex++;
00023 
00024     if (dataIndex == 25)
00025     {
00026         dataIndex = 0;
00027         if (rcvData[24] != 0x0)
00028         {
00029             chData[0] = ((rcvData[1] | rcvData[2] << 8) & 0x07FF);
00030             chData[1] = ((rcvData[2] >> 3 | rcvData[3] << 5) & 0x07FF);
00031             chData[2] = ((rcvData[3] >> 6 | rcvData[4] << 2 | rcvData[5] << 10) & 0x07FF);
00032             chData[3] = ((rcvData[5] >> 1 | rcvData[6] << 7) & 0x07FF);
00033             chData[4] = ((rcvData[6] >> 4 | rcvData[7] << 4) & 0x07FF);
00034             chData[5] = ((rcvData[7] >> 7 | rcvData[8] << 1 | rcvData[9] << 9) & 0x07FF);
00035             chData[6] = ((rcvData[9] >> 2 | rcvData[10] << 6) & 0x07FF);
00036             chData[7] = ((rcvData[10] >> 5 | rcvData[11] << 3) & 0x07FF);
00037             chData[8] = ((rcvData[12] | rcvData[13] << 8) & 0x07FF);
00038             chData[9] = ((rcvData[13] >> 3 | rcvData[14] << 5) & 0x07FF);
00039             chData[10] = ((rcvData[14] >> 6 | rcvData[15] << 2 | rcvData[16] << 10) & 0x07FF);
00040             chData[11] = ((rcvData[16] >> 1 | rcvData[17] << 7) & 0x07FF);
00041             chData[12] = ((rcvData[17] >> 4 | rcvData[18] << 4) & 0x07FF);
00042             chData[13] = ((rcvData[18] >> 7 | rcvData[19] << 1 | rcvData[20] << 9) & 0x07FF);
00043             chData[14] = ((rcvData[20] >> 2 | rcvData[21] << 6) & 0x07FF);
00044             chData[15] = ((rcvData[21] >> 5 | rcvData[22] << 3) & 0x07FF);
00045             chData[16] = rcvData[23];
00046 
00047             if ((rcvData[23] >> 3) & 0x0001)
00048                 failSafe = true;
00049             else
00050                 failSafe = false;
00051         }
00052     }
00053 }
00054 
00055 bool SBUS::isFailSafe()
00056 {
00057     return failSafe;
00058 }
00059 
00060 int16_t SBUS::_getData(uint8_t ch)
00061 {
00062     return chData[ch];
00063 }
00064 
00065 int SBUS::getStickVal(int axis)
00066 {
00067     switch (axis)
00068     {
00069     case 0:
00070         return _map(_constrain(_getData(3), 368, 1680), 368, 1680, -255, 255);
00071     case 1:
00072         return _map(_constrain(_getData(1), 368, 1680), 368, 1680, 255, -255);
00073     case 2:
00074         return _map(_constrain(_getData(0), 368, 1680), 368, 1680, -255, 255);
00075     case 3:
00076         return _map(_constrain(_getData(2), 368, 1680), 368, 1680, -255, 255);
00077     default:
00078         return 0;
00079     }
00080 }
00081 
00082 int SBUS::getSwitchVal(int parm)
00083 {
00084     if (parm == 0)
00085         return _map(_constrain(_getData(4), 144, 1904), 144, 1904, 0, 2);
00086     else if (parm <= 5)
00087         return _map(_constrain(_getData(parm + 4), 144, 1904), 144, 1904, 0, 2);
00088     else
00089         return 0;
00090 }
00091 
00092 
00093 float SBUS::_constrain(float in, float min, float max)
00094 {
00095     if(in > max)
00096         return max;
00097     else if(in < min)
00098         return min;
00099     else return in;
00100 }
00101 float SBUS::_map(float in, float inMin, float inMax, float outMin, float outMax) {
00102   // check it's within the range
00103   if (inMin<inMax) {
00104     if (in <= inMin)
00105       return outMin;
00106     if (in >= inMax)
00107       return outMax;
00108   } else {  // cope with input range being backwards.
00109     if (in >= inMin)
00110       return outMin;
00111     if (in <= inMax)
00112       return outMax;
00113   }
00114   // calculate how far into the range we are
00115   float scale = (in-inMin)/(inMax-inMin);
00116   // calculate the output.
00117   return outMin + scale*(outMax-outMin);
00118 }