shogo sato / SBUS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

SBUS.cpp@15:55548ca76fc4, 2020-12-07 (annotated)

Committer:
sshogo
Date:
Mon Dec 07 07:41:34 2020 +0000
Revision:
15:55548ca76fc4
Parent:
14:481c23f2e4eb 
add functions:; - resetall() ; Reset all parameters; - getConnectStatus() ; Get connecting status from receiver to mbed; - connstatus ; Connecting status from receiver to mbed

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