Luka Slapnik / sbus_decode1

Dependents:   CyberbotV1

SBUS.cpp@13:17a61a516f0c, 2019-07-15 (annotated)

Committer:
sshogo
Date:
Mon Jul 15 00:50:24 2019 +0000
Revision:
13:17a61a516f0c
Parent:
12:ac222684f449
Child:
14:481c23f2e4eb
fix hosei

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 4:da8728f3d289 6 /*stickMaximumValue(0x068F),
sshogo 0:e2bc011508c7 7 stickNeutralValue(0x03FF),
sshogo 4:da8728f3d289 8 stickMinimumValue(0x016F),*/
sshogo 4:da8728f3d289 9 stickMaximumValue(0x0690),
sshogo 4:da8728f3d289 10 stickNeutralValue(0x0400),
sshogo 4:da8728f3d289 11 stickMinimumValue(0x0170),
sshogo 0:e2bc011508c7 12 stickResolution(stickMaximumValue - stickMinimumValue)
sshogo 0:e2bc011508c7 13 {
sshogo 0:e2bc011508c7 14 com.baud(100000);
sshogo 0:e2bc011508c7 15 com.attach(this, &SBUS::receiveData, Serial::RxIrq);
sshogo 0:e2bc011508c7 16 com.format(8, Serial::Even, 2);
sshogo 4:da8728f3d289 17 /*switchPositionValue[0] = 0x076F;
sshogo 4:da8728f3d289 18 switchPositionValue[1] = 0x03FF;
sshogo 4:da8728f3d289 19 switchPositionValue[2] = 0x008F;
sshogo 4:da8728f3d289 20 switchFPositionValue[0] = 0x076F;
sshogo 4:da8728f3d289 21 switchFPositionValue[1] = 0x0074;*/
sshogo 0:e2bc011508c7 22 switchPositionValue[0] = 0x076F;
sshogo 0:e2bc011508c7 23 switchPositionValue[1] = 0x03FF;
sshogo 2:493d10424466 24 switchPositionValue[2] = 0x008F;
sshogo 0:e2bc011508c7 25 switchFPositionValue[0] = 0x076F;
sshogo 0:e2bc011508c7 26 switchFPositionValue[1] = 0x0074;
sshogo 0:e2bc011508c7 27 }
sshogo 0:e2bc011508c7 28
sshogo 0:e2bc011508c7 29 float SBUS::getStickValue(int tag) {
sshogo 0:e2bc011508c7 30 return stickValue[tag];
sshogo 0:e2bc011508c7 31 }
sshogo 0:e2bc011508c7 32
sshogo 0:e2bc011508c7 33 int SBUS::getSwitchValue(int tag) {
sshogo 0:e2bc011508c7 34 return switchValue[tag];
sshogo 0:e2bc011508c7 35 }
sshogo 0:e2bc011508c7 36
sshogo 0:e2bc011508c7 37 int SBUS::getChannelValue(int tag) {
sshogo 0:e2bc011508c7 38 return channel[tag];
sshogo 0:e2bc011508c7 39 }
sshogo 0:e2bc011508c7 40
sshogo 0:e2bc011508c7 41 void SBUS::receiveData() {
sshogo 0:e2bc011508c7 42 static int count = 0;
sshogo 0:e2bc011508c7 43 char buf;
sshogo 0:e2bc011508c7 44
sshogo 4:da8728f3d289 45 buf = com.getc();
sshogo 0:e2bc011508c7 46
sshogo 4:da8728f3d289 47 if(buf == 0x0F) {
sshogo 0:e2bc011508c7 48 decordReceivedData();
sshogo 0:e2bc011508c7 49 count = 0;
sshogo 0:e2bc011508c7 50 }
sshogo 0:e2bc011508c7 51 if(count >= 25) return;
sshogo 0:e2bc011508c7 52 receivedData[count] = buf;
sshogo 0:e2bc011508c7 53 count++;
sshogo 0:e2bc011508c7 54 }
sshogo 0:e2bc011508c7 55
sshogo 0:e2bc011508c7 56 void SBUS::decordReceivedData() {
sshogo 0:e2bc011508c7 57 channel[0] = ((receivedData[1] |receivedData[2]<<8) & 0x07FF);
sshogo 0:e2bc011508c7 58 channel[1] = ((receivedData[2]>>3 |receivedData[3]<<5) & 0x07FF);
sshogo 0:e2bc011508c7 59 channel[2] = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10) & 0x07FF);
sshogo 0:e2bc011508c7 60 channel[3] = ((receivedData[5]>>1 |receivedData[6]<<7) & 0x07FF);
sshogo 0:e2bc011508c7 61 channel[4] = ((receivedData[6]>>4 |receivedData[7]<<4) & 0x07FF);
sshogo 0:e2bc011508c7 62 channel[5] = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9) & 0x07FF);
sshogo 0:e2bc011508c7 63 channel[6] = ((receivedData[9]>>2 |receivedData[10]<<6) & 0x07FF);
sshogo 0:e2bc011508c7 64 channel[7] = ((receivedData[10]>>5|receivedData[11]<<3) & 0x07FF);
sshogo 0:e2bc011508c7 65 channel[8] = ((receivedData[12] |receivedData[13]<<8) & 0x07FF);
sshogo 0:e2bc011508c7 66 channel[9] = ((receivedData[13]>>3|receivedData[14]<<5) & 0x07FF);
sshogo 0:e2bc011508c7 67 channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
sshogo 0:e2bc011508c7 68 channel[11] = ((receivedData[16]>>1|receivedData[17]<<7) & 0x07FF);
sshogo 0:e2bc011508c7 69 channel[12] = ((receivedData[17]>>4|receivedData[18]<<4) & 0x07FF);
sshogo 0:e2bc011508c7 70 channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9) & 0x07FF);
sshogo 0:e2bc011508c7 71 channel[14] = ((receivedData[20]>>2|receivedData[21]<<6) & 0x07FF);
sshogo 0:e2bc011508c7 72 channel[15] = ((receivedData[21]>>5|receivedData[22]<<3) & 0x07FF);
sshogo 0:e2bc011508c7 73 convertReceivedData();
sshogo 0:e2bc011508c7 74 }
sshogo 0:e2bc011508c7 75
sshogo 0:e2bc011508c7 76 void SBUS::convertReceivedData() {
sshogo 1:a1307d47b1b8 77 for(int i = 0; i < 4; i++) {
sshogo 1:a1307d47b1b8 78 float buf;
sshogo 13:17a61a516f0c 79 if(channel[i] > (stickNeutralValue + 20)) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
sshogo 13:17a61a516f0c 80 else if(channel[i] < (stickNeutralValue - 20)) buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
sshogo 10:ac27dfde496e 81 else buf = 0.0f;
sshogo 7:0a43fd370575 82 buf = (int)(buf*100)/100.0f;
sshogo 9:0c412933b84f 83 buf = (buf > 1.0f) ? 1.0f : buf;
sshogo 1:a1307d47b1b8 84 switch(i) {
sshogo 1:a1307d47b1b8 85 case right_RL:
sshogo 7:0a43fd370575 86 stickValue[analog_rx] = buf;
sshogo 1:a1307d47b1b8 87 break;
sshogo 1:a1307d47b1b8 88 case right_UD:
sshogo 4:da8728f3d289 89 stickValue[analog_ry] = buf;
sshogo 1:a1307d47b1b8 90 break;
sshogo 1:a1307d47b1b8 91 case left_RL:
sshogo 4:da8728f3d289 92 stickValue[analog_lx] = buf;
sshogo 1:a1307d47b1b8 93 break;
sshogo 1:a1307d47b1b8 94 case left_UD:
sshogo 4:da8728f3d289 95 stickValue[analog_ly] = -buf;
sshogo 1:a1307d47b1b8 96 break;
sshogo 1:a1307d47b1b8 97 default:
sshogo 1:a1307d47b1b8 98 break;
sshogo 1:a1307d47b1b8 99 }
sshogo 1:a1307d47b1b8 100 }
sshogo 2:493d10424466 101 for(int i = 0; i < 12; i++) {
sshogo 2:493d10424466 102 switch(channel[i + 4]) {
sshogo 4:da8728f3d289 103 case 0x078b:
sshogo 4:da8728f3d289 104 case 0x770:
sshogo 2:493d10424466 105 switchValue[i] = High;
sshogo 2:493d10424466 106 break;
sshogo 4:da8728f3d289 107 case 0x0400:
sshogo 2:493d10424466 108 switchValue[i] = Neutral;
sshogo 2:493d10424466 109 break;
sshogo 4:da8728f3d289 110 case 0x0090:
sshogo 2:493d10424466 111 switchValue[i] = Low;
sshogo 2:493d10424466 112 break;
sshogo 2:493d10424466 113 default:
sshogo 12:ac222684f449 114 //switchValue[i] = 3;
sshogo 2:493d10424466 115 break;
sshogo 2:493d10424466 116 }
sshogo 2:493d10424466 117 }
sshogo 0:e2bc011508c7 118 }