Diff: SBUS.cpp

Revision:

0:e2bc011508c7

Child:

1:a1307d47b1b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SBUS.cpp	Fri Jun 28 07:34:58 2019 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "SBUS.h"
+
+SBUS::SBUS(PinName tx, PinName rx) :
+com(tx, rx),
+stickMaximumValue(0x068F),
+stickNeutralValue(0x03FF),
+stickMinimumValue(0x016F),
+stickResolution(stickMaximumValue - stickMinimumValue)
+{
+    com.baud(100000);
+    com.attach(this, &SBUS::receiveData, Serial::RxIrq);
+    com.format(8, Serial::Even, 2);
+    switchPositionValue[0] = 0x076F;
+    switchPositionValue[1] = 0x03FF;
+    switchPositionValue[2] = 0x080F;
+    switchFPositionValue[0] = 0x076F;
+    switchFPositionValue[1] = 0x0074;
+}
+
+float SBUS::getStickValue(int tag) {
+    return stickValue[tag];
+}
+
+int SBUS::getSwitchValue(int tag) {
+    return switchValue[tag];
+}
+
+int SBUS::getChannelValue(int tag) {
+    return channel[tag];
+}
+
+void SBUS::receiveData() {
+    static int count = 0;
+    char buf;
+    
+    buf = ~com.getc();
+    
+    if(buf == 0xF0) {
+        decordReceivedData();
+        count = 0;
+    }
+    if(count >= 25) return;
+    receivedData[count] = buf;
+    count++;
+}
+
+void SBUS::decordReceivedData() {
+    channel[0]  = ((receivedData[1]    |receivedData[2]<<8)                 & 0x07FF);
+    channel[1]  = ((receivedData[2]>>3 |receivedData[3]<<5)                 & 0x07FF);
+    channel[2]  = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10)  & 0x07FF);
+    channel[3]  = ((receivedData[5]>>1 |receivedData[6]<<7)                 & 0x07FF);
+    channel[4]  = ((receivedData[6]>>4 |receivedData[7]<<4)                 & 0x07FF);
+    channel[5]  = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9)   & 0x07FF);
+    channel[6]  = ((receivedData[9]>>2 |receivedData[10]<<6)                & 0x07FF);
+    channel[7]  = ((receivedData[10]>>5|receivedData[11]<<3)                & 0x07FF);
+    channel[8]  = ((receivedData[12]   |receivedData[13]<<8)                & 0x07FF);
+    channel[9]  = ((receivedData[13]>>3|receivedData[14]<<5)                & 0x07FF);
+    channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
+    channel[11] = ((receivedData[16]>>1|receivedData[17]<<7)                & 0x07FF);
+    channel[12] = ((receivedData[17]>>4|receivedData[18]<<4)                & 0x07FF);
+    channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9)  & 0x07FF);
+    channel[14] = ((receivedData[20]>>2|receivedData[21]<<6)                & 0x07FF);
+    channel[15] = ((receivedData[21]>>5|receivedData[22]<<3)                & 0x07FF);
+    convertReceivedData();
+}
+
+void SBUS::convertReceivedData() {
+    stickValue[analog_rx] = 1.0f - ((channel[right_RL] - stickMinimumValue) / (float)stickResolution);
+    stickValue[analog_ry] = 1.0f - ((channel[right_UD] - stickMinimumValue) / (float)stickResolution);
+    stickValue[analog_lx] = 1.0f - ((channel[left_RL] - stickMinimumValue) / (float)stickResolution);
+    stickValue[analog_ly] = (channel[left_UD] - stickMinimumValue) / (float)stickResolution;
+    for(int i = 0; i < 12; i++) switchValue[i] = ((channel[i + 3] == switchPositionValue[High]) ? 1 : 0);
+}