forked from robotmaker/Arduino_SBUS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
SBUS_20.pde
253 lines (216 loc) · 12 KB
/
SBUS_20.pde
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
// Graphical represetentation of Futuba SBUS - also used in FrySky Receivers
// Example by Colin Bacon
// ROBOTmaker
// www.robotmaker.eu
//Thanks goes to Futaba for making this SBUS protocol so challenging for everyone. Talk about "why make things simple if we can make things extermely difficult..."
//To get this to work you'll need to have SBUS conneted receiver that is connected via an Arduiono or FTDI USB thingy to your PC
//As the SBUS is an inverted signal (yes - to make life even more fun), you'll need to invert the signal using a simple transitor+2 restistors or if you have an FTDI there is a program to invert the signals automatically
//Detail of how to do this are on our website
// References
//https://developer.mbed.org/users/Digixx/notebook/futaba-s-bus-controlled-by-mbed/
//http://www.clubamcl.fr/techniques/la-technologie-futaba-s-bus/
//Thanks goes to https://www.ordinoscope.net/index.php/Electronique/Protocoles/SBUS for the arduino code which I converted to processing
import processing.serial.*;
Serial myPort; // The serial port
float x, y;
String myString = null;
int [] buffer = new int [25];
int [] channels = new int [18];
int errors = 0;
boolean failsafe = false;
int idx, inByte, lost, packetErrors;
long last_refresh = 0;
float ymag = 0;
float newYmag = 0;
float xmag = 0;
float newXmag = 0;
void setup()
{
size(1500, 600, FX2D); //give good dashboard resolutions
// size(1500, 600, P3D); //used for drawing cubes
//fullScreen(); //This sets it to full screen, but comment out the size commmand above
pixelDensity(1);
x = width * 0.3;
y = height * 0.5;
// Variable for drawing the cube
// List all the available serial ports
printArray(Serial.list());
// Open the port you are using at the rate you want:
myPort = new Serial(this, Serial.list()[0], 100000,'E',8,2);
myString = myPort.readStringUntil(15); //clean the first buffer
println(myPort); //Debug to show the port opened
}
void draw()
{
//Check for incoming Serial Port Data
if (myPort.available() == 0){
//If no incoming data then count lost packages
packetErrors++;
text ("No Signal", 140,20);
}
//When data is comming in then check for a start byte No.1=B11110000 and stop byte No.25=B00000000
while (myPort.available() > 0) {
inByte = myPort.read(); //Read the Byte that just came in
//if it's a new packet and the start byte is not B00001111 (DEC15) then it's an error.
//Note that the SBUS byte data is MSB, which causes some fun later
if (idx == 0 && inByte != 0x0F) // error - wait for the start byte
{
text("Package Error", 100,100);
}
// if it's a new packet and the start byte is B00001111 (DEC15) then start reading the next 25 bytes.
else {
buffer[idx++] = inByte; // fill the buffer with 25 Bytes
}
// if the buffer of 25 Bytes is reached then start to decode
if (idx == 25)
{
idx = 0; //reset the buffer count for the next cycle
if (buffer[24] != 0x00)
{ //Check that the packet size is 25 bytes long with stop byte b00000000 as the last byte
errors++; //Count the number of errors
println(errors); //Print to the error totals to the dashboard
}
else
{ //Start decoding the bits and bytes
//Buffer[0] contains the start byte value of 15, so this is ignored
//The Channels are 11 bits in length, so the bytes need to be split-up
//To make this conversion more 'interesting' as the bytes arrive as MSB i.e. the highest byte arrives first and the channels are
//assembled as little endian, which means the 1st 3 bits of the second byte need to comes before the 1st byte.
//--------------Channel 1 -------------------
// The 1st channel = 1st byte + 3 bits from the 2nd byte
// This channel packet of 11 bits is then read backwards.
// As the inByte variable is already converted to integers we can move bits along as follows:
// byte 1 = 00000000 00000011 (say the value is 3)
// byte 2 = 01100100 (say the value of the 2nd byte is 100)
// but we are only interested in the last 3 bits which need to be moved infront of the 1st byte,...
// byte 2 = 01100100 00000000 - ...So shift this byte along left by 8 and it looks like this
// byte 2 = | (or) the two bytes together and you get this
// byte 1 = 00000000 00000011
// byte 2 = 01100100 00000000
// Channel 1 = 01100100 00000011 <- the intermediate result
// But we were only interested in the 1st 3 bits of byte 2 so we need strip the 1st 5 bits by and-ing with 2047 (11 bits)
// & (and) the two together and you get this....
// Channel1 = 01100100 00000011
// 2047 = 00000111 11111111
// Channel1 = 00000100 00000011 <- the final result
//The bytes are litte Endian which means that to read the full
channels[0] = ((buffer[1]|buffer[2]<<8) & 0x07FF); //The first Channel
//--------------Channel 2 -------------------
// 2nd Channel = last 5 bit of byte2 and 8 bits of byte 3. Here we need to play around some more.
// Remove the 1st 3 bits of byte2 by pushing them off to the right
// byte 2 = 00000000 01100100
// byte 2 = 00000000 00001100 moved 3 to the right
// byte 3 = 00000000 01010100 lets say that byte 3 is this
// byte 3 = 00001010 10000000 moved 5 to the left to get the 5 bytes we need in place (remember this is MSB so bits in byte 3 comes before bits in byte 2)
// | (or) byte 2 and 3 them together to get the 11 bits in place for channels2
// byte 2 = 00000000 00001100
// byte 3 = 00001010 10000000
// Channel2 = 00001010 10001100 < - result of| (or-ing) them together
// again we were only interested in 11 bits so strip the package by and-ing with 2047 (11 bits)
// & (and) the two together and you get this....
// Channel2 = 00001010 10001100 < - interim result of| (or-ing) them together
// 2047 = 00000111 11111111
// Channel2 = 00000010 10001100 < - result of & (and-ing) them together
channels[1] = ((buffer[2]>>3|buffer[3]<<5) & 0x07FF);
//--------------Channel 3 -------------------
// 3rd Channel = last 2 bit of byte3 and 8 bits of byte 4 and some bits from byte 5.
// Here we have even more to play around with. Someone with a sense of humour surely designed this.
// Talk about "why make things simple if we can make it very difficult..."
// Remove the 1st 6 bits of byte3 by pushing them off to the right
// byte 2 = 00000000 00001100
// byte 3 = 00000000 00000001 moved 6 to the right
// byte 4 = 00000000 10101010 lets say that byte 4 is this
// byte 4 = 00000010 10101000 moved 2 to the left to get the bytes we need in place
// | (or) byte 3 and 4 together to get the 11 bits in place for channels3
// byte 3 = 00000000 00000001
// byte 4 = 00000010 10101000
// Channel3 = 00000010 10101001 < - result of| (or-ing) them together
// byte 5 = 00000000 10110101 lets say that byte 5 is this
// byte 5 = 11010100 00000000 moved 10 to the left to get the bytes we need in place
// Channel3 = 00000010 10101001 < - result of prrevious| (or-ing) them together
// Channel3 = 11010110 10101001 <- result of or-ing shift byte 5 and previous or-ing
// | (or) byte 5 together with the current channel 3 to get the 11 bits in place for channels3
// again we were only interested in 11 bits so strip the package by and-ing with 2047 (11 bits)
// & (and) the two together and you get this....
// Channel3 = 11010110 10101001 <- result of or-ing shift byte 5 and previous or-ing
// 2047 = 00000111 11111111
// Channel3 = 00000110 10101001 < - result of & (and-ing) them together
// ...and so on
channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
channels[16] = ((buffer[23]));
//channels[16] = ((buffer[23]) & 0x0001) ? 2047 : 0;
//channels[17] = ((buffer[23] >> 1) & 0x0001) ? 2047 : 0;
//failsafe = ((buffer[23] >> 3) & 0x0001) ? 1 : 0;
//if ((buffer[23] >> 2) & 0x0001) lost++;
}
} //For loop
//pushMatrix();
int pos1=0,channelText = 0;
//Set the background colour of the bargraphs
background(199);
//Cycle through all channels and draw a bar chart for each channel value
for(int c=0;c<16;c++){
//Set the colour of the bar chart graphics
noStroke(); //remove the graphic boarder lines
fill(17,37,204); //set the colour of bar chart
//Map Channel bar graph
float z= float(channels[c]); //convert to a float
float x1 = map(z,160,2000,0,width*0.95);
int yoffSet = 50;
pos1 = pos1 + 20; //y postion of bar on the canvas
channelText = channelText + 1;
rect(100, pos1+yoffSet , x1, 15);
fill(0);
text("Channel" + channelText + " = ", 5,pos1+10+yoffSet);
text(channels[c],70,pos1+10 + yoffSet);
//Draw the dashboard title and status messages
textSize(26);
text("Open TX SBUS - Status Dashboard", width/2-150, 30); //Place the text in the centre of the dashboard
textSize(10); //Reset text size
//If the bits in byte23 are set, then there is an error. Display a warning
if (buffer[23] >= 12) {
fill(226,72,47);
ellipse(50,40,20,20); //Draw a red warning led
fill(0); //colour the text
textSize(12); //Reset text size
text("Signal Lost",30,20);
textSize(11); //Reset text size
}
else
{
fill(22,205,65);
ellipse(50,40,20,20); //Draw a green warning led
fill(0); //colour the text
textSize(12); //Reset text size
text("Signal OK",30,20);
textSize(11); //Reset text size
}
//Place the error counts message
textSize(12); //Reset text size
text("Errors", 120,20);
text("bytes23= ", 100,40);
text(buffer[23],150,40);
textSize(11); //Reset text size
text("Count= ", 100,55);
text(errors, 150,55);
//Place the lost package message
textSize(12); //Reset text size
text("Lost Packages", 170,20);
textSize(11); //Reset text size
text(packetErrors, 190,40);
} //if
} //While
}