/
IbusReader.cs
291 lines (256 loc) · 8.43 KB
/
IbusReader.cs
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
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
/*
* Created by SharpDevelop.
* User: Cleric
* Date: 9.6.2017 г.
* Time: 17:47 ч.
*/
using System;
using System.Collections;
using System.IO.Ports;
using System.Windows.Forms;
namespace vJoySerialFeeder
{
/// <summary>
/// For initial reference I looked at the code at https://github.com/aanon4/FlySkyIBus
/// That code depends on the gap between the frames to determine where a frame starts.
/// I wanted to make a reader that is independent of inter-frame gap. This is achieved by keeping
/// a buffer and trying to parse a valid frame starting at different offsets. Once a vlid frame
/// is found, from then on, frames can be read one after another without checking for a
/// gap. If for some reason the checksum is wrong for more than three frames we assume that we have
/// lost sync and we start looking for the beginning of a valid frame anew.
///
/// IBUS protocol is very simple, but yet it is enough for most uses. It consists of frame with
/// the following structure:
/// * 1 byte -> length of the frame including the length byte itself and checksum.
/// * (length-3) bytes -> data
/// * 2 bytes -> checksum. It is calculated by taking 0xffff and subtracting each frame byte
/// from that. The checksum is stored with LSB first.
///
/// The 'data' part of the frame has the following format:
/// * 1 byte -> command. This determines the payload type - currently only command 0x40 is known
/// for 'channel data'
/// * remaining bytes -> payload
/// for the 0x40 command, the payload has the form of an array of 16bit integers - one integer
/// for each channel. The integers are stored with LSB first
///
/// So the general form is:
/// | frame length | data | checksum |
/// | <len> | <cmd(0x40)> <ch1l><ch1h> <ch2l><ch2h> ... | <chkl><chkh> |
///
/// </summary>
public class IbusReader : SerialReader
{
const byte PROTOCOL_OVERHEAD = 3; // Overhead is <len> + <chkl><chkh>
const byte PROTOCOL_COMMAND40 = 0x40; // Command is always 0x40
const int PROTOCOL_MAX_LENGTH = 0xff;
const int MAX_CHECKSUM_RETRIES = 3;
const byte PROTOCOL_IA6_MAGIC = 0x55;
const int PROTOCOL_IA6_LENGTH = 31;
int numDiscards = 0;
bool ia6Ibus;
bool use16bitChannels;
public override void Start()
{
parseConfig(config);
serialPort.ReadTimeout = 500;
if(ia6Ibus) {
Buffer.FrameLength = PROTOCOL_IA6_LENGTH;
}
else {
Buffer.FrameLength = PROTOCOL_MAX_LENGTH;
}
}
public override void Stop()
{
}
public override int ReadChannels() {
if(ia6Ibus)
return ReadChannelsIa6();
else
return ReadChannelsStandard();
}
public int ReadChannelsStandard()
{
int idx; // index in the buffer
int data_start; // index of the start of command byte and payload in the buffer
int data_len; // length of the payload
int data_end; // end index of the payload
UInt16 chksum = 0;
// check length byte
var len = Buffer[0];
if(len > PROTOCOL_OVERHEAD) {
idx = 1;
data_start = idx;
data_len = (byte)(len - PROTOCOL_OVERHEAD);
data_end = data_start + data_len;
chksum = (UInt16)(0xFFFF - len);
}
else {
// not a valid frame length, try parsing from the next index
Buffer.Slide(1);
Buffer.FrameLength = PROTOCOL_MAX_LENGTH;
System.Diagnostics.Debug.WriteLine("bad start");
return 0;
}
// consume all the data
while(idx < data_start + data_len) {
chksum -= Buffer[idx++];
}
// check checksum
if(chksum == Buffer[idx++] + (Buffer[idx++] << 8)) {
// Valid packet
Buffer.FrameLength = data_len + PROTOCOL_OVERHEAD; // used for next serial read
numDiscards = 0; // reset discards
if(Buffer[data_start] == PROTOCOL_COMMAND40) {
// Execute command - we only know command 0x40
data_end = data_start + data_len;
data_start++; // skip command byte
int ch = 0;
int index = data_start;
ushort mask = (ushort)(use16bitChannels ? 0xFFFF : 0x0FFF);
while (index + 1 < data_end)
channelData[ch++] = ReadU16(ref index) & mask;
if(!use16bitChannels) {
//see https://github.com/betaflight/betaflight/pull/8749
index = data_start + 1;
while (index + 5 < data_end) {
channelData[ch++] = ((Buffer[index] & 0xF0) >> 4) | (Buffer[index + 2] & 0xF0) | ((Buffer[index + 4] & 0xF0) << 4);
index += 6;
}
}
Buffer.Slide(idx);
return ch;
}
}
else {
// incorrect checksum
// wrong checksum
if(numDiscards++ < MAX_CHECKSUM_RETRIES)
{
// Usually if it is a real bit error we should just try and read the next
// packet.
Buffer.Slide(idx);
System.Diagnostics.Debug.WriteLine("bad checksum");
}
else {
// we are getting too many consecutive discards we should
// try to re-sync
Buffer.Slide(1);
Buffer.FrameLength = PROTOCOL_MAX_LENGTH; // reset frame length
System.Diagnostics.Debug.WriteLine("bad checksum - resyncing");
}
}
return 0;
}
/// <summary>
/// The IA6 receiver has undocumented IBUS-like output.
/// See http://endoflifecycle.blogspot.com/2016/10/flysky-ia6-ibus-setup.html
/// for details.
///
/// The protocol is also different. See:
/// https://www.rcgroups.com/forums/showthread.php?2711184-Serial-output-from-FS-IA6-%28Semi-I-BUS%29
///
/// The frame size is fixed to 31 bytes. There are 14 channels.
///
/// | magic signature | data | checksum |
/// | <0x55> | <ch1l><ch1h> <ch2l><ch2h> ... | <chkl><chkh> |
/// The checksum is simply the sum of the 16bit int values of the channels.
///
/// </summary>
/// <returns></returns>
public int ReadChannelsIa6()
{
int idx = 0; // index in the buffer
UInt16 chksum = 0;
// check magic first byte
if(Buffer[idx++] != PROTOCOL_IA6_MAGIC) {
Buffer.Slide(1);
System.Diagnostics.Debug.WriteLine("bad start - resyncing");
return 0;
}
while(idx < PROTOCOL_IA6_LENGTH - 2) {
chksum += ReadU16(ref idx);
}
// last u16 is the checksum
if(chksum != ReadU16(ref idx)) {
// incorrect checksum
Buffer.Slide(idx);
System.Diagnostics.Debug.WriteLine("bad checksum");
return 0;
}
int ch = 0;
int index = 1;
ushort mask = (ushort)(use16bitChannels ? 0xFFFF : 0x0FFF);
while (index + 1 < PROTOCOL_IA6_LENGTH - 2)
channelData[ch++] = ReadU16(ref index) & mask;
if(!use16bitChannels) {
//see https://github.com/betaflight/betaflight/pull/8749
index = 2;
while (index + 5 < PROTOCOL_IA6_LENGTH - 2) {
channelData[ch++] = ((Buffer[index] & 0xF0) >> 4) | (Buffer[index + 2] & 0xF0) | ((Buffer[index + 4] & 0xF0) << 4);
index += 6;
}
}
Buffer.Slide(idx);
return ch;
}
public override Configuration.SerialParameters GetDefaultSerialParameters()
{
return new Configuration.SerialParameters() {
BaudRate = 115200,
DataBits = 8,
Parity = Parity.None,
StopBits = StopBits.One
};
}
public override bool Configurable { get { return true; } }
/// <summary>
/// Show Ibus configuration
/// </summary>
/// <param name="config"></param>
/// <returns></returns>
public override string Configure(string config)
{
parseConfig(config);
using(var d = new IbusSetupForm(ia6Ibus, use16bitChannels)) {
d.ShowDialog();
if(d.DialogResult == DialogResult.OK) {
ia6Ibus = d.Ia6Ibus;
use16bitChannels = d.Use16bitChannels;
return buildConfig();
}
return null;
}
}
public override string ProtocolName { get { return "IBUS"; } }
private ushort ReadU16(ref int index) {
return (ushort)(Buffer[index++] | (Buffer[index++] << 8));
}
/// <summary>
/// Ibus configuration - "ia6" string if IA6 ibus should be used
/// "16bit" if 16bit channels are to be used
/// </summary>
/// <param name="config"></param>
/// <returns></returns>
private void parseConfig(string config) {
var tokens = config == null ?
new string[0]
:
config.Split(',');
foreach(var s in tokens) {
if(s == "ia6")
ia6Ibus = true;
else if(s == "16bit")
use16bitChannels = true;
}
}
private string buildConfig() {
var cfg = new ArrayList();
if(ia6Ibus)
cfg.Add("ia6");
if(use16bitChannels)
cfg.Add("16bit");
return string.Join(",", cfg.ToArray());
}
}
}