Luka Slapnik / sbus_decode

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@0:e2bc011508c7, 2019-06-28 (annotated)

Committer:
sshogo
Date:
Fri Jun 28 07:34:58 2019 +0000
Revision:
0:e2bc011508c7
Child:
1:a1307d47b1b8
first commit

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 0:e2bc011508c7 69 stickValue[analog_rx] = 1.0f - ((channel[right_RL] - stickMinimumValue) / (float)stickResolution);
sshogo 0:e2bc011508c7 70 stickValue[analog_ry] = 1.0f - ((channel[right_UD] - stickMinimumValue) / (float)stickResolution);
sshogo 0:e2bc011508c7 71 stickValue[analog_lx] = 1.0f - ((channel[left_RL] - stickMinimumValue) / (float)stickResolution);
sshogo 0:e2bc011508c7 72 stickValue[analog_ly] = (channel[left_UD] - stickMinimumValue) / (float)stickResolution;
sshogo 0:e2bc011508c7 73 for(int i = 0; i < 12; i++) switchValue[i] = ((channel[i + 3] == switchPositionValue[High]) ? 1 : 0);
sshogo 0:e2bc011508c7 74 }