Luka Slapnik / sbus_decode1

final1

Dependents:   CyberbotV1

SBUS.cpp@1:a1307d47b1b8, 2019-06-28 (annotated)

Committer:
sshogo
Date:
Fri Jun 28 07:59:32 2019 +0000
Revision:
1:a1307d47b1b8
Parent:
0:e2bc011508c7
Child:
2:493d10424466
fix convertReceivedData(); stickValue : 1.0-0.0 -> 1.0-(-1.0)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sshogo 0:e2bc011508c7 1 #include "mbed.h"
sshogo 0:e2bc011508c7 2 #include "SBUS.h"
sshogo 0:e2bc011508c7 3
sshogo 0:e2bc011508c7 4 SBUS::SBUS(PinName tx, PinName rx) :
sshogo 0:e2bc011508c7 5 com(tx, rx),
sshogo 0:e2bc011508c7 6 stickMaximumValue(0x068F),
sshogo 0:e2bc011508c7 7 stickNeutralValue(0x03FF),
sshogo 0:e2bc011508c7 8 stickMinimumValue(0x016F),
sshogo 0:e2bc011508c7 9 stickResolution(stickMaximumValue - stickMinimumValue)
sshogo 0:e2bc011508c7 10 {
sshogo 0:e2bc011508c7 11 com.baud(100000);
sshogo 0:e2bc011508c7 12 com.attach(this, &SBUS::receiveData, Serial::RxIrq);
sshogo 0:e2bc011508c7 13 com.format(8, Serial::Even, 2);
sshogo 0:e2bc011508c7 14 switchPositionValue[0] = 0x076F;
sshogo 0:e2bc011508c7 15 switchPositionValue[1] = 0x03FF;
sshogo 0:e2bc011508c7 16 switchPositionValue[2] = 0x080F;
sshogo 0:e2bc011508c7 17 switchFPositionValue[0] = 0x076F;
sshogo 0:e2bc011508c7 18 switchFPositionValue[1] = 0x0074;
sshogo 0:e2bc011508c7 19 }
sshogo 0:e2bc011508c7 20
sshogo 0:e2bc011508c7 21 float SBUS::getStickValue(int tag) {
sshogo 0:e2bc011508c7 22 return stickValue[tag];
sshogo 0:e2bc011508c7 23 }
sshogo 0:e2bc011508c7 24
sshogo 0:e2bc011508c7 25 int SBUS::getSwitchValue(int tag) {
sshogo 0:e2bc011508c7 26 return switchValue[tag];
sshogo 0:e2bc011508c7 27 }
sshogo 0:e2bc011508c7 28
sshogo 0:e2bc011508c7 29 int SBUS::getChannelValue(int tag) {
sshogo 0:e2bc011508c7 30 return channel[tag];
sshogo 0:e2bc011508c7 31 }
sshogo 0:e2bc011508c7 32
sshogo 0:e2bc011508c7 33 void SBUS::receiveData() {
sshogo 0:e2bc011508c7 34 static int count = 0;
sshogo 0:e2bc011508c7 35 char buf;
sshogo 0:e2bc011508c7 36
sshogo 0:e2bc011508c7 37 buf = ~com.getc();
sshogo 0:e2bc011508c7 38
sshogo 0:e2bc011508c7 39 if(buf == 0xF0) {
sshogo 0:e2bc011508c7 40 decordReceivedData();
sshogo 0:e2bc011508c7 41 count = 0;
sshogo 0:e2bc011508c7 42 }
sshogo 0:e2bc011508c7 43 if(count >= 25) return;
sshogo 0:e2bc011508c7 44 receivedData[count] = buf;
sshogo 0:e2bc011508c7 45 count++;
sshogo 0:e2bc011508c7 46 }
sshogo 0:e2bc011508c7 47
sshogo 0:e2bc011508c7 48 void SBUS::decordReceivedData() {
sshogo 0:e2bc011508c7 49 channel[0] = ((receivedData[1] |receivedData[2]<<8) & 0x07FF);
sshogo 0:e2bc011508c7 50 channel[1] = ((receivedData[2]>>3 |receivedData[3]<<5) & 0x07FF);
sshogo 0:e2bc011508c7 51 channel[2] = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10) & 0x07FF);
sshogo 0:e2bc011508c7 52 channel[3] = ((receivedData[5]>>1 |receivedData[6]<<7) & 0x07FF);
sshogo 0:e2bc011508c7 53 channel[4] = ((receivedData[6]>>4 |receivedData[7]<<4) & 0x07FF);
sshogo 0:e2bc011508c7 54 channel[5] = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9) & 0x07FF);
sshogo 0:e2bc011508c7 55 channel[6] = ((receivedData[9]>>2 |receivedData[10]<<6) & 0x07FF);
sshogo 0:e2bc011508c7 56 channel[7] = ((receivedData[10]>>5|receivedData[11]<<3) & 0x07FF);
sshogo 0:e2bc011508c7 57 channel[8] = ((receivedData[12] |receivedData[13]<<8) & 0x07FF);
sshogo 0:e2bc011508c7 58 channel[9] = ((receivedData[13]>>3|receivedData[14]<<5) & 0x07FF);
sshogo 0:e2bc011508c7 59 channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
sshogo 0:e2bc011508c7 60 channel[11] = ((receivedData[16]>>1|receivedData[17]<<7) & 0x07FF);
sshogo 0:e2bc011508c7 61 channel[12] = ((receivedData[17]>>4|receivedData[18]<<4) & 0x07FF);
sshogo 0:e2bc011508c7 62 channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9) & 0x07FF);
sshogo 0:e2bc011508c7 63 channel[14] = ((receivedData[20]>>2|receivedData[21]<<6) & 0x07FF);
sshogo 0:e2bc011508c7 64 channel[15] = ((receivedData[21]>>5|receivedData[22]<<3) & 0x07FF);
sshogo 0:e2bc011508c7 65 convertReceivedData();
sshogo 0:e2bc011508c7 66 }
sshogo 0:e2bc011508c7 67
sshogo 0:e2bc011508c7 68 void SBUS::convertReceivedData() {
sshogo 1:a1307d47b1b8 69 for(int i = 0; i < 4; i++) {
sshogo 1:a1307d47b1b8 70 float buf;
sshogo 1:a1307d47b1b8 71 if(channel[i] > stickNeutralValue) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
sshogo 1:a1307d47b1b8 72 else buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
sshogo 1:a1307d47b1b8 73 switch(i) {
sshogo 1:a1307d47b1b8 74 case right_RL:
sshogo 1:a1307d47b1b8 75 stickValue[analog_rx] = -buf;
sshogo 1:a1307d47b1b8 76 break;
sshogo 1:a1307d47b1b8 77 case right_UD:
sshogo 1:a1307d47b1b8 78 stickValue[analog_ry] = -buf;
sshogo 1:a1307d47b1b8 79 break;
sshogo 1:a1307d47b1b8 80 case left_RL:
sshogo 1:a1307d47b1b8 81 stickValue[analog_lx] = -buf;
sshogo 1:a1307d47b1b8 82 break;
sshogo 1:a1307d47b1b8 83 case left_UD:
sshogo 1:a1307d47b1b8 84 stickValue[analog_ly] = buf;
sshogo 1:a1307d47b1b8 85 break;
sshogo 1:a1307d47b1b8 86 default:
sshogo 1:a1307d47b1b8 87 break;
sshogo 1:a1307d47b1b8 88 }
sshogo 1:a1307d47b1b8 89 }
sshogo 0:e2bc011508c7 90 for(int i = 0; i < 12; i++) switchValue[i] = ((channel[i + 3] == switchPositionValue[High]) ? 1 : 0);
sshogo 0:e2bc011508c7 91 }