-
Notifications
You must be signed in to change notification settings - Fork 0
/
FrSkyPort.ino
454 lines (408 loc) · 13.9 KB
/
FrSkyPort.ino
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
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
#include "HardwareSerial.h"
#include "FrSkySPort.h"
uint16_t crc; // used for crc calc of frsky-packet
uint8_t lastRx; // Last byte received from FrSKY. Look for 7E <dev ID>
uint8_t cell_count = 0;
uint32_t latlong = 0;
uint32_t new_data_counter = 0;
uint32_t skip_data_counter = 0;
uint32_t dev_poll_counter = 0;
class TelemetryItem
{
public:
// These are the data types we can send. They are related to the FrSKY defined ID's FR_ID_...
enum dataType {
TI_FR_ID_SPEED,
TI_FR_ID_RPM,
TI_FR_ID_CURRENT,
TI_FR_ID_ALTITUDE,
TI_FR_ID_AIR_SPEED,
TI_FR_ID_LONG, // FrSKY uses a combined FR_ID_LATLONG and use a data bit to encode LAT/LONG
TI_FR_ID_LAT, // FrSKY uses a combined FR_ID_LATLONG and use a data bit to encode LAT/LONG
TI_FR_ID_HEADING,
TI_FR_ID_ADC2,
TI_FR_ID_CELLS01, // FrSKY uses FR_ID_CELLS and encode which cells in data
TI_FR_ID_CELLS23, // FrSKY uses FR_ID_CELLS and encode which cells in data
TI_FR_ID_CELLS45, // FrSKY uses FR_ID_CELLS and encode which cells in data
TI_FR_ID_ACCX,
TI_FR_ID_ACCY,
TI_FR_ID_ACCZ,
TI_FR_ID_VFAS,
TI_FR_ID_T1,
TI_FR_ID_T2,
TI_FR_ID_VARIO,
TI_FR_ID_GPS_ALT,
TI_FR_ID_FUEL,
TI_NBR_ITEMS
};
TelemetryItem(dataType id, uint16_t frid);
uint32_t fetchData(dataType, bool &valid);
uint32_t fetchVoltage(void);
bool transmitData(bool force);
private:
dataType m_id;
uint16_t m_frid;
uint16_t m_skip;
uint16_t m_skipmax;
bool m_valid;
uint32_t m_data;
};
TelemetryItem::TelemetryItem(dataType id, uint16_t frid)
{
m_id = id;
m_frid = frid;
m_skip = 0;
m_skipmax = 30; // unchanging data gets skipped this many times
m_data = 0;
m_valid = false; // no data yet
}
/**
* \brief convert MK data to FrSKY format
* \param type which type of data to send
* \param valid return flag - was data valid
* \param device a few types of data only work for for specific devices
* \return 32 bit data item
*/
uint32_t TelemetryItem::fetchData(dataType type, bool &valid)
{
uint32_t value=0;
valid = true;
switch (type) {
case TI_FR_ID_SPEED:
value = mk_groundspeed; // knt to km/h conversion missing?
break;
case TI_FR_ID_RPM:
value = mk_sat_visible * 2; // * 2 if number of blades on Taranis is set to 2
break;
case TI_FR_ID_CURRENT:
value = mk_current_battery;
break;
case TI_FR_ID_ALTITUDE:
value = mk_bar_altitude; // from barometer, 100 = 1m
break;
case TI_FR_ID_LONG:
if (mk_sat_visible>2) {
if (mk_longitude < 0) {
latlong = ((abs(mk_longitude)/100)*6) | 0xC0000000;
} else {
latlong = ((abs(mk_longitude)/100)*6) | 0x80000000;
}
value = latlong;
} else {
valid = false;
}
break;
case TI_FR_ID_LAT:
if (mk_sat_visible>2) {
if (mk_latitude < 0 ) {
latlong = ((abs(mk_latitude)/100)*6) | 0x40000000;
} else {
latlong = ((abs(mk_latitude)/100)*6);
}
value = latlong;
} else {
valid = false;
}
break;
case TI_FR_ID_HEADING:
value = mk_heading * 100; // 10000 = 100 deg
break;
case TI_FR_ID_ADC2:
value = mk_voltage_battery; //
break;
case TI_FR_ID_CELLS01:
value = (cell_voltage(1) << 20) | (cell_voltage(0) << 8) | (ap_cell_count << 4) | 0; // Battery cells 0 and 1, 2 cells
break;
case TI_FR_ID_CELLS23:
if (ap_cell_count > 2) {
uint32_t offset;
offset = ap_cell_count > 3 ? 0x02: 0x01;
value = (cell_voltage(3) << 20) | (cell_voltage(2) << 8) | (ap_cell_count << 4) | offset; // Battery cell 2,3
} else {
valid = false;
}
break;
case TI_FR_ID_CELLS45:
if (ap_cell_count > 4) {
//uint32_t offset;
//offset = ap_cell_count > 5 ? 0x04: 0x03;
//value = (temp << 20) | (temp << 8) | offset; // Battery cell 2,3
valid = false;
} else {
valid = false;
}
break;
case TI_FR_ID_ACCX:
value = mk_compass_heading;
break;
case TI_FR_ID_ACCY:
value = mk_flying_time;
break;
case TI_FR_ID_ACCZ:
value = mk_vertical_speed;
break;
case TI_FR_ID_VFAS:
value = mk_voltage_battery * 10;
DEBUG_PRINTLN(value);
break;
case TI_FR_ID_T1:
value = mk_error_code_1;
break;
case TI_FR_ID_T2:
value = mk_error_code_2;
break;
case TI_FR_ID_VARIO:
value = mk_climb_rate; // cm/s
break;
case TI_FR_ID_GPS_ALT:
if (mk_gps_status % 10 == 3) {
value = mk_gps_altitude / 10; // from GPS, units: MK mm, FrSKY cm
} else {
valid = false;
}
break;
case TI_FR_ID_FUEL:
value = mk_used_capacity;
break;
case TI_FR_ID_AIR_SPEED:
value = mk_gps_status;
break;
default:
valid = false;
break;
}
return value;
}
/**
* \brief Fetch & send data if available and (new or stale)
* \return 1 if data was sent,
*/
bool TelemetryItem::transmitData(bool force)
{
bool valid = false;
uint32_t data = fetchData(m_id, valid);
if (valid) {
if (force || (data != m_data) || (m_skip > m_skipmax)) {
FrSkySPort_SendPackage(m_frid, data);
m_data = data; // Save copy of what was sent
m_skip = 0;
return true;
} else {
m_skip++;
}
}
return false;
}
/**
* \brief A collection of TelemetryItem s.
* Handles mapping to FrSKY ID's and encoding logic.
*/
class TelemetrySet
{
public:
TelemetrySet(const TelemetryItem::dataType activeIds[], uint16_t len);
void sendItem();
void sendFlvss(void);
private:
TelemetryItem* m_items[TelemetryItem::TI_NBR_ITEMS] = {}; //! Map data type to object
const uint16_t m_frId[TelemetryItem::TI_NBR_ITEMS] = { // Mapping from internal ID's to FrSKY ID's
FR_ID_SPEED, //TI_FR_ID_SPEED
FR_ID_RPM, //TI_FR_ID_RPM
FR_ID_CURRENT, //TI_FR_ID_CURRENT
FR_ID_ALTITUDE, //TI_FR_ID_ALTITUDE
FR_ID_AIR_SPEED, //TI_FR_ID_AIR_SPEED
FR_ID_LATLONG, //TI_FR_ID_LONG
FR_ID_LATLONG, //TI_FR_ID_LAT
FR_ID_HEADING, //TI_FR_ID_HEADING
FR_ID_ADC2, //TI_FR_ID_ADC2
FR_ID_CELLS, //TI_FR_ID_CELLS01
FR_ID_CELLS, //TI_FR_ID_CELLS23
FR_ID_CELLS, //TI_FR_ID_CELLS45
FR_ID_ACCX, //TI_FR_ID_ACCX
FR_ID_ACCY, //TI_FR_ID_ACCY
FR_ID_ACCZ, //TI_FR_ID_ACCZ
FR_ID_VFAS, //TI_FR_ID_VFAS
FR_ID_T1, //TI_FR_ID_T1
FR_ID_T2, //TI_FR_ID_T2
FR_ID_VARIO, //TI_FR_ID_VARIO
FR_ID_GPS_ALT, //TI_FR_ID_GPS_ALT
FR_ID_FUEL, //TI_FR_ID_FUEL
}; //! Map data type to FrSKY ID
uint16_t m_count = 0; //! Number of active telemetry items (minus cells)
uint16_t m_next = 0; //! Next telemetry item to send
uint16_t m_idx[TelemetryItem::TI_NBR_ITEMS] = {}; // list of active items
uint16_t m_idxCell[3] = {}; // types of cells measurements
uint16_t m_cell = 0; // Last cell voltage record type sent
uint16_t m_cell_count = 0; // How many record types (0-3)
};
/**
* \brief Build two list of telemetry items. One for cells and one for the rest.
*/
TelemetrySet::TelemetrySet(const TelemetryItem::dataType activeIds[], uint16_t len)
{
uint16_t i;
TelemetryItem::dataType type;
uint16_t frtype;
for (i=0; i<len; i++) {
type = activeIds[i];
frtype = m_frId[type];
m_items[type] = new TelemetryItem(type, frtype); // Map each object according to data type
// Cells need special handling because of overloaded device id
if (frtype == FR_ID_CELLS) {
m_idxCell[m_cell_count++] = type;
} else {
m_idx[m_count++] = type; // map consequtive index to used data type
}
}
}
/**
* \brief Send the next ready TelemetryItem
*/
void TelemetrySet::sendItem(void)
{
bool done = false;
uint16_t tries=0; //
//DEBUG_PRINT("I");
do {
done = m_items[m_idx[m_next]]->transmitData(false);
if (done) {
new_data_counter++;
}
m_next++;
tries++;
if (m_next >= m_count) {
m_next = 0;
}
if (!done && (tries >= m_count)) {
// Only try each item once per round.
//DEBUG_PRINT("-");
skip_data_counter++;
done = true;
}
} while (!done);
}
void TelemetrySet::sendFlvss(void)
{
bool done=false;
if (m_cell_count > 0) {
done = m_items[m_idxCell[m_cell]]->transmitData(true);
m_cell++;
if (m_cell >= m_cell_count) {
m_cell = 0;
}
}
}
// These are the telemetry items we will actually transmit.
// Comment out items not relevant to save bandwidth.
const TelemetryItem::dataType activeItems[] = {
TelemetryItem::TI_FR_ID_SPEED,
TelemetryItem::TI_FR_ID_RPM,
TelemetryItem::TI_FR_ID_CURRENT,
TelemetryItem::TI_FR_ID_ALTITUDE,
TelemetryItem::TI_FR_ID_AIR_SPEED,
TelemetryItem::TI_FR_ID_LONG,
TelemetryItem::TI_FR_ID_LAT,
TelemetryItem::TI_FR_ID_HEADING,
//TelemetryItem::TI_FR_ID_ADC2,
TelemetryItem::TI_FR_ID_CELLS01,
TelemetryItem::TI_FR_ID_CELLS23,
//TelemetryItem::TI_FR_ID_CELLS45,
TelemetryItem::TI_FR_ID_ACCX,
TelemetryItem::TI_FR_ID_ACCY,
TelemetryItem::TI_FR_ID_ACCZ,
TelemetryItem::TI_FR_ID_VFAS,
TelemetryItem::TI_FR_ID_T1,
TelemetryItem::TI_FR_ID_T2,
TelemetryItem::TI_FR_ID_VARIO,
TelemetryItem::TI_FR_ID_GPS_ALT,
TelemetryItem::TI_FR_ID_FUEL,
};
void FrSkySPort_Initialize(void)
{
FrSkySPort_Serial.begin(FrSkySPort_BAUD);
FrSkySPort_C3 = 0x10; // Tx invert
FrSkySPort_C1 = 0xA0; // Single wire mode
FrSkySPort_S2 = 0x10; // Rx Invert
}
TelemetrySet *channel=NULL; //! This points to the object with the TelemetrySet
void FrSkySPort_Setup(void)
{
FrSkySPort_Initialize();
channel = new TelemetrySet(activeItems, sizeof(activeItems));
}
void FrSkySPort_Process(void)
{
uint8_t data = 0;
while ( FrSkySPort_Serial.available()) {
data = FrSkySPort_Serial.read();
if (lastRx == START_STOP) {
// FrSKY scans all known nodes PLUS ONE of the unknown (in round robin) for each cycle.
// Recognizing more ID's means that releative bandwith increases. In this case to 6/7th
if ((data == SENSOR_ID1) /* || (data == SENSOR_ID2) ||
(data == SENSOR_ID3) || (data == SENSOR_ID4) ||
(data == SENSOR_ID5) || (data == SENSOR_ID6)*/) {
channel->sendItem();
} else if (data == SENSOR_ID_FLVSS) {
channel->sendFlvss();
} else {
// Some other sensor was probed
// digitalWrite(mkPin, HIGH);
// delay(2);
// digitalWrite(mkPin, LOW);
//DEBUG_PRINT2(data,HEX);
dev_poll_counter++;
if (dev_poll_counter % 300 == 0) {
uint32_t tot;
tot = new_data_counter + skip_data_counter + dev_poll_counter;
DEBUG_PRINT("New:"); DEBUG_PRINT(100*new_data_counter/tot);
DEBUG_PRINT(" Skip:"); DEBUG_PRINT(100*skip_data_counter/tot);
DEBUG_PRINT(" Poll:"); DEBUG_PRINTLN(100*dev_poll_counter/tot);
}
}
}
lastRx = data;
}
}
void FrSkySPort_SendByte(uint8_t b)
{
FrSkySPort_TransmitByte( b );
// CRC update
crc += b; //0-1FF
crc += ( crc >> 8 ); //0-100
crc &= 0x00ff;
}
void FrSkySPort_TransmitByte(uint8_t b)
{
if ( b == 0x7e ) {
FrSkySPort_Serial.write( 0x7d );
FrSkySPort_Serial.write( 0x5e );
} else if ( b == 0x7d ) {
FrSkySPort_Serial.write( 0x7d );
FrSkySPort_Serial.write( 0x5d );
} else {
FrSkySPort_Serial.write( b );
}
}
void FrSkySPort_SendCrc()
{
FrSkySPort_TransmitByte( 0xFF-crc );
crc = 0; // CRC reset
}
void FrSkySPort_SendPackage(uint16_t id, uint32_t value)
{
digitalWrite(LED, HIGH); // Flash LED when sending telemetry.
FrSkySPort_C3 |= 32; // Transmit direction, to S.Port
FrSkySPort_SendByte(DATA_FRAME);
uint8_t *bytes = (uint8_t*)&id;
FrSkySPort_SendByte(bytes[0]);
FrSkySPort_SendByte(bytes[1]);
bytes = (uint8_t*)&value;
FrSkySPort_SendByte(bytes[0]);
FrSkySPort_SendByte(bytes[1]);
FrSkySPort_SendByte(bytes[2]);
FrSkySPort_SendByte(bytes[3]);
FrSkySPort_SendCrc();
FrSkySPort_Serial.flush();
FrSkySPort_C3 ^= 32; // Transmit direction, from S.Port
digitalWrite(LED, LOW);
}