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@15:c96df23cad7d, 2020-06-29 (annotated)

Committer:
sllez
Date:
Mon Jun 29 15:54:10 2020 +0000
Revision:
15:c96df23cad7d
Parent:
14:481c23f2e4eb 
koncno

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) :
sllez 15:c96df23cad7d 5 sbus(tx, rx),
sllez 15:c96df23cad7d 6
sllez 15:c96df23cad7d 7 //CALIBRATE INPUT SIGNALS (USE getChannelValue)
sllez 15:c96df23cad7d 8
sllez 15:c96df23cad7d 9 sbusMaximumValue(1811),
sllez 15:c96df23cad7d 10 sbusNeutralValue(992),
sllez 15:c96df23cad7d 11 sbusMinimumValue(172),
sllez 15:c96df23cad7d 12 sbusDeadband(10)
sllez 15:c96df23cad7d 13
sshogo 0:e2bc011508c7 14 {
sllez 15:c96df23cad7d 15 sbus.baud(100000);
sllez 15:c96df23cad7d 16 sbus.attach(callback(this, &SBUS::receiveData), Serial::RxIrq); //Z TEMLE BEREMO UART ZMER KO PRIDEJO PODATKI
sllez 15:c96df23cad7d 17 sbus.format(8, Serial::Even, 2);
sllez 15:c96df23cad7d 18 }
sllez 15:c96df23cad7d 19
sllez 15:c96df23cad7d 20 int SBUS::checkFailsafeTimer() {
sllez 15:c96df23cad7d 21 failsafetime = 12;
sllez 15:c96df23cad7d 22 if(lastreadtime < failsafetime){
sllez 15:c96df23cad7d 23 reportreadtime = lastreadtime;
sllez 15:c96df23cad7d 24 lastreadtime = failsafetime + 1;
sllez 15:c96df23cad7d 25 return 1;
sllez 15:c96df23cad7d 26 }
sllez 15:c96df23cad7d 27 else{
sllez 15:c96df23cad7d 28 return 0;
sllez 15:c96df23cad7d 29 }
sllez 15:c96df23cad7d 30 }
sllez 15:c96df23cad7d 31
sllez 15:c96df23cad7d 32 int SBUS::failSafeTimerMs() {
sllez 15:c96df23cad7d 33 return reportreadtime;
sshogo 0:e2bc011508c7 34 }
sshogo 0:e2bc011508c7 35
sshogo 0:e2bc011508c7 36 float SBUS::getStickValue(int tag) {
sshogo 0:e2bc011508c7 37 return stickValue[tag];
sshogo 0:e2bc011508c7 38 }
sshogo 0:e2bc011508c7 39
sshogo 0:e2bc011508c7 40 int SBUS::getChannelValue(int tag) {
sshogo 0:e2bc011508c7 41 return channel[tag];
sshogo 0:e2bc011508c7 42 }
sshogo 0:e2bc011508c7 43
sshogo 0:e2bc011508c7 44 void SBUS::receiveData() {
sshogo 0:e2bc011508c7 45 static int count = 0;
sshogo 0:e2bc011508c7 46 char buf;
sllez 15:c96df23cad7d 47
sllez 15:c96df23cad7d 48 buf = sbus.getc();
sshogo 14:481c23f2e4eb 49 if(count >= 25) return;
sshogo 14:481c23f2e4eb 50 receivedData[count] = buf;
sshogo 14:481c23f2e4eb 51 count++;
sshogo 0:e2bc011508c7 52
sshogo 14:481c23f2e4eb 53 if(count == 25 && receivedData[0] == 0x0F) {
sshogo 0:e2bc011508c7 54 decordReceivedData();
sshogo 0:e2bc011508c7 55 count = 0;
sshogo 0:e2bc011508c7 56 }
sshogo 0:e2bc011508c7 57 }
sshogo 0:e2bc011508c7 58
sshogo 0:e2bc011508c7 59 void SBUS::decordReceivedData() {
sllez 15:c96df23cad7d 60 failsafetimer.stop();
sllez 15:c96df23cad7d 61 lastreadtime = failsafetimer.read_ms();
sllez 15:c96df23cad7d 62 failsafetimer.reset();
sllez 15:c96df23cad7d 63 failsafetimer.start();
sshogo 0:e2bc011508c7 64 channel[0] = ((receivedData[1] |receivedData[2]<<8) & 0x07FF);
sshogo 0:e2bc011508c7 65 channel[1] = ((receivedData[2]>>3 |receivedData[3]<<5) & 0x07FF);
sshogo 0:e2bc011508c7 66 channel[2] = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10) & 0x07FF);
sshogo 0:e2bc011508c7 67 channel[3] = ((receivedData[5]>>1 |receivedData[6]<<7) & 0x07FF);
sshogo 0:e2bc011508c7 68 channel[4] = ((receivedData[6]>>4 |receivedData[7]<<4) & 0x07FF);
sshogo 0:e2bc011508c7 69 channel[5] = ((receivedData[7]>>7 |receivedData[8]<<1 |receivedData[9]<<9) & 0x07FF);
sshogo 0:e2bc011508c7 70 channel[6] = ((receivedData[9]>>2 |receivedData[10]<<6) & 0x07FF);
sshogo 0:e2bc011508c7 71 channel[7] = ((receivedData[10]>>5|receivedData[11]<<3) & 0x07FF);
sshogo 0:e2bc011508c7 72 channel[8] = ((receivedData[12] |receivedData[13]<<8) & 0x07FF);
sshogo 0:e2bc011508c7 73 channel[9] = ((receivedData[13]>>3|receivedData[14]<<5) & 0x07FF);
sshogo 0:e2bc011508c7 74 channel[10] = ((receivedData[14]>>6|receivedData[15]<<2|receivedData[16]<<10) & 0x07FF);
sshogo 0:e2bc011508c7 75 channel[11] = ((receivedData[16]>>1|receivedData[17]<<7) & 0x07FF);
sshogo 0:e2bc011508c7 76 channel[12] = ((receivedData[17]>>4|receivedData[18]<<4) & 0x07FF);
sshogo 0:e2bc011508c7 77 channel[13] = ((receivedData[18]>>7|receivedData[19]<<1|receivedData[20]<<9) & 0x07FF);
sshogo 0:e2bc011508c7 78 channel[14] = ((receivedData[20]>>2|receivedData[21]<<6) & 0x07FF);
sshogo 0:e2bc011508c7 79 channel[15] = ((receivedData[21]>>5|receivedData[22]<<3) & 0x07FF);
sshogo 0:e2bc011508c7 80 convertReceivedData();
sshogo 0:e2bc011508c7 81 }
sshogo 0:e2bc011508c7 82
sshogo 0:e2bc011508c7 83 void SBUS::convertReceivedData() {
sllez 15:c96df23cad7d 84 for(int i = 0; i < 16; i++) {
sshogo 1:a1307d47b1b8 85 float buf;
sllez 15:c96df23cad7d 86 if(channel[i] > (sbusNeutralValue + sbusDeadband)){ //THIS IS MIDDLE DEADBAND (DEAD ZONE)
sllez 15:c96df23cad7d 87 buf = ((float)(channel[i] - sbusNeutralValue) / (float)(sbusMaximumValue - sbusNeutralValue));
sllez 15:c96df23cad7d 88 }
sllez 15:c96df23cad7d 89 else if(channel[i] < (sbusNeutralValue - sbusDeadband)){ //THIS IS MIDDLE DEADBAND (DEAD ZONE)
sllez 15:c96df23cad7d 90 buf = -((float)(channel[i] - sbusNeutralValue) / (float)(sbusMinimumValue - sbusNeutralValue));
sllez 15:c96df23cad7d 91 }
sshogo 10:ac27dfde496e 92 else buf = 0.0f;
sllez 15:c96df23cad7d 93 buf = (int)(buf*100)/100.0f; //Limit output resolution to 2 decimals (x.yz)
sllez 15:c96df23cad7d 94 if(buf > 1.0f){ //Limit to -1.0 to 1.0
sllez 15:c96df23cad7d 95 buf = 1.0f;
sshogo 1:a1307d47b1b8 96 }
sllez 15:c96df23cad7d 97 if(buf < -1.0f){
sllez 15:c96df23cad7d 98 buf = -1.0f;
sshogo 2:493d10424466 99 }
sllez 15:c96df23cad7d 100 stickValue[i] = buf;
sshogo 2:493d10424466 101 }
sshogo 0:e2bc011508c7 102 }