forked from multiwii/multiwii-firmware
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Telemetry.cpp
654 lines (608 loc) · 18.8 KB
/
Telemetry.cpp
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
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
// ****************************************************************
// FrSky telemetry
// Version: 0.4.0 - by QuadBow
// Changes: V0.4.0: - supports 2.4 with new features added
// device specific selection of data to be sent
// different ways for displaying
// different ways to present data
// Version: 0.3.0
// Changes: V0.3.0: - new structure with cpp/h-files
// Date 20/09/2012
// Changes: V0.2.1: - make it work with 2.1 (shared dev)
// Date: 14/08/2012
// Changes: V0.2: - Byte stuffing added
// - vBat will be send, if "#define FAS_100" is comment out
// V0.1: - First release
// ****************************************************************
#include "Arduino.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "MultiWii.h"
#include "Sensors.h"
#include "Serial.h"
#include "Telemetry.h"
#if defined(FRSKY_TELEMETRY)
void init_telemetry(void)
{
SerialOpen(TELEMETRY_SERIAL,TELEMETRY_BAUD);
}
void write_FrSky8(uint8_t Data)
{
SerialWrite(TELEMETRY_SERIAL, Data);
}
void check_FrSky_stuffing(uint8_t Data) //byte stuffing
{
if (Data == 0x5E) {
write_FrSky8(0x5D);
write_FrSky8(0x3E);
}
else if (Data == 0x5D) {
write_FrSky8(0x5D);
write_FrSky8(0x3D);
}
else {
write_FrSky8(Data);
}
}
void write_FrSky16(uint16_t Data)
{
uint8_t Data_send;
Data_send = Data;
check_FrSky_stuffing(Data_send);
Data_send = Data >> 8 & 0xff;
check_FrSky_stuffing(Data_send);
}
static void sendDataHead(uint8_t Data_id)
{
write_FrSky8(Protocol_Header);
write_FrSky8(Data_id);
}
static void sendDataTail(void)
{
write_FrSky8(Protocol_Tail);
}
//*********************************************************************************
//----------------- Telemetrie Data ------------------------------------------
//*********************************************************************************
// Temperature and NumSats
void inline send_Temperature(void)
{
#if BARO
int16_t Data_Temperature1;
Data_Temperature1 = (baroTemperature + 50) / 100;
sendDataHead(ID_Temperature1);
write_FrSky16(Data_Temperature1);
#endif
#if GPS
int16_t Data_NumSats;
if (f.GPS_FIX && GPS_numSat >= 4) {
Data_NumSats = GPS_numSat;
sendDataHead(ID_Temperature2);
write_FrSky16(Data_NumSats);
}
#endif
}
// RPM is interpreted as Return distance to home Position in Meter
void inline send_RPM(void)
{
#if GPS
uint16_t Data_RPM;
if (f.GPS_FIX && GPS_numSat >= 4) {
Data_RPM = GPS_distanceToHome; // Distance to home alias RPM
sendDataHead(ID_RPM);
write_FrSky16(Data_RPM);
}
#endif
}
// Fuel level
void inline send_Fuel(void)
{
#if defined(POWERMETER)
uint16_t Data_Fuel;
if ((pMeter[PMOTOR_SUM] < (pAlarm / 4)) || (pAlarm == 0))
Data_Fuel = 100;
else if (pMeter[PMOTOR_SUM] < (pAlarm / 2))
Data_Fuel = 75;
else if (pMeter[PMOTOR_SUM] < (3 * pAlarm / 4))
Data_Fuel = 50;
else if (pMeter[PMOTOR_SUM] < pAlarm)
Data_Fuel = 25;
else
Data_Fuel = 0;
sendDataHead(ID_Fuel_level);
write_FrSky16(Data_Fuel);
#endif
}
// Cell voltage
void inline send_cell_volt(void) // Data compatibel to FrSky FLVS-01 voltage sensor
{
#if defined(VBAT_CELLS)
uint16_t Data_Volt;
uint16_t temp;
static uint8_t cell_counter = 0;
// the resolution of analog.vbatcells results in only one decimal (eg 3.70V or 3.80V and nothing in between)
// TODO: improve resolution of analog.vbatcells
temp = 50 * analog.vbatcells[cell_counter];
Data_Volt = (temp << 8) + (temp >> 8) + (cell_counter << 4);
if (++cell_counter >= VBAT_CELLS_NUM)
cell_counter = 0;
sendDataHead(ID_Volt);
write_FrSky16(Data_Volt);
#endif
}
// Altitude
void inline send_Altitude(void)
{
#if defined(TELEMETRY_ALT_BARO) and BARO
#if defined(FRSKY_FLD02)
int16_t Data_altitude;
Data_altitude = (alt.EstAlt + 50) / 100;
sendDataHead(ID_Altitude_bp);
write_FrSky16(Data_altitude);
#endif
#if defined OPENTX
int16_t Data_altitude_bp, Data_altitude_ap;
Data_altitude_bp = alt.EstAlt / 100;
sendDataHead(ID_Altitude_bp);
write_FrSky16(Data_altitude_bp);
Data_altitude_ap = alt.EstAlt - Data_altitude_bp * 100;
sendDataHead(ID_Altitude_ap);
write_FrSky16(Data_altitude_ap);
#endif
#endif
#if defined(TELEMETRY_ALT_GPS) and GPS
#if defined(FRSKY_FLD02)
#if not defined(TELEMETRY_ALT_BARO)
int16_t Data_altitude;
if (f.GPS_FIX && GPS_numSat >= 4) {
Data_altitude = GPS_altitude;
sendDataHead(ID_Altitude_bp);
write_FrSky16(Data_altitude);
}
#endif
#else
int16_t Data_GPS_altitude_bp;
uint16_t Data_GPS_altitude_ap;
if (f.GPS_FIX && GPS_numSat >= 4) {
Data_GPS_altitude_bp = GPS_altitude;
Data_GPS_altitude_ap = 0;
sendDataHead(ID_GPS_Altitude_bp);
write_FrSky16(Data_GPS_altitude_bp);
sendDataHead(ID_GPS_Altitude_ap);
write_FrSky16(Data_GPS_altitude_ap);
}
#endif
#endif
}
// Course
void inline send_Course(void)
{
#if not defined(FRSKY_FLD02)
#if defined TELEMETRY_COURSE_GPS and GPS
uint16_t Data_Course_bp;
uint16_t Data_Course_ap;
if (f.GPS_FIX && GPS_numSat >= 4) {
Data_Course_bp = GPS_ground_course / 10;
Data_Course_ap = GPS_ground_course - Data_Course_bp * 10;
sendDataHead(ID_Course_bp);
write_FrSky16(Data_Course_bp);
sendDataHead(ID_Course_ap);
write_FrSky16(Data_Course_ap);
}
#elif defined TELEMETRY_COURSE_MAG and MAG
uint16_t Data_Course_bp;
uint16_t Data_Course_ap;
Data_Course_bp = att.heading;
Data_Course_ap = 0;
sendDataHead(ID_Course_bp);
write_FrSky16(Data_Course_bp);
sendDataHead(ID_Course_ap);
write_FrSky16(Data_Course_ap);
#endif
#endif
}
// GPS speed
void inline send_GPS_speed(void)
{
#if GPS
uint16_t Data_GPS_speed_bp;
uint16_t Data_GPS_speed_ap;
uint16_t temp;
if (f.GPS_FIX && GPS_numSat >= 4) {
#if defined KILOMETER_HOUR // OPENTX specific format in kilometers per hour => factor 36/100 (will be devided by 10 later)
temp = (GPS_speed * 36) / 10;
#else // FRSKY specific format in knots => factor ~50 (will be devided by 10 later)
temp = (GPS_speed * 40) / 203;
#endif
Data_GPS_speed_bp = temp / 10; // here comes the devision by 10
Data_GPS_speed_ap = temp - Data_GPS_speed_bp * 10;
sendDataHead(ID_GPS_speed_bp);
write_FrSky16(Data_GPS_speed_bp);
sendDataHead(ID_GPS_speed_ap);
write_FrSky16(Data_GPS_speed_ap);
}
#endif
}
// GPS position
void inline send_GPS_longitude(void)
{
#if GPS
uint16_t Data_Longitude_bp;
uint16_t Data_Longitude_ap;
uint16_t Data_E_W;
uint32_t temp, rest, decimal;
if (f.GPS_FIX && GPS_numSat >= 4) {
temp = abs(GPS_coord[LON]);
#if defined(COORDFORMAT_DECIMALMINUTES)
decimal = temp / 10000000;
temp -= decimal * 10000000;
temp *= 6;
rest = temp;
temp /= 1000000;
rest -= temp * 1000000;
Data_Longitude_bp = decimal * 100 + temp;
Data_Longitude_ap = rest / 100;
#else
decimal = temp / 100000;
rest = temp - decimal * 100000;
Data_Longitude_bp = decimal;
Data_Longitude_ap = rest / 100;
#endif
Data_E_W = GPS_coord[LON] < 0 ? 'W' : 'E';
sendDataHead(ID_Longitude_bp);
write_FrSky16(Data_Longitude_bp);
sendDataHead(ID_Longitude_ap);
write_FrSky16(Data_Longitude_ap);
sendDataHead(ID_E_W);
write_FrSky16(Data_E_W);
}
#endif
}
void inline send_GPS_latitude(void)
{
#if GPS
uint16_t Data_Latitude_bp;
uint16_t Data_Latitude_ap;
uint16_t Data_N_S;
uint32_t temp, rest, decimal;
if (f.GPS_FIX && GPS_numSat >= 4) {
temp = abs(GPS_coord[LAT]);
#if defined(COORDFORMAT_DECIMALMINUTES)
decimal = temp / 10000000;
temp -= decimal * 10000000;
temp *= 6;
rest = temp;
temp /= 1000000;
rest -= temp * 1000000;
Data_Latitude_bp = decimal * 100 + temp;
Data_Latitude_ap = rest / 100;
#else
decimal = temp / 100000;
rest = temp - decimal * 100000;
Data_Latitude_bp = decimal * 100 + temp;
Data_Latitude_ap = rest / 100;
#endif
Data_N_S = GPS_coord[LAT] < 0 ? 'S' : 'N';
sendDataHead(ID_Latitude_bp);
write_FrSky16(Data_Latitude_bp);
sendDataHead(ID_Latitude_ap);
write_FrSky16(Data_Latitude_ap);
sendDataHead(ID_N_S);
write_FrSky16(Data_N_S);
}
#endif
}
// Time of arming
void inline send_Time(void)
{
uint16_t seconds_since_start;
uint16_t Data_rest;
uint16_t Data_hours;
uint16_t Data_minutes;
uint16_t Data_seconds;
if (f.ARMED) {
seconds_since_start = armedTime / 1000000;
Data_hours = seconds_since_start / 3600;
Data_rest = seconds_since_start - Data_hours * 3600;
Data_minutes = Data_rest / 60;
Data_seconds = Data_rest - Data_minutes * 60;
sendDataHead(ID_Hour_Minute);
write_FrSky16(Data_hours + Data_minutes * 256);
sendDataHead(ID_Second);
write_FrSky16(Data_seconds);
}
}
// ACC
void inline send_Accel(void)
{
#if ACC
int16_t Data_Acc_X;
int16_t Data_Acc_Y;
int16_t Data_Acc_Z;
Data_Acc_X = ((float)imu.accSmooth[0] / ACC_1G) * 1000;
Data_Acc_Y = ((float)imu.accSmooth[1] / ACC_1G) * 1000;
Data_Acc_Z = ((float)imu.accSmooth[2] / ACC_1G) * 1000;
sendDataHead(ID_Acc_X);
write_FrSky16(Data_Acc_X);
sendDataHead(ID_Acc_Y);
write_FrSky16(Data_Acc_Y);
sendDataHead(ID_Acc_Z);
write_FrSky16(Data_Acc_Z);
#endif
}
// Voltage (Ampere Sensor)
void inline send_Voltage_ampere(void) // Data compatibel to FrSky FAS-100 voltage sensor, FLD-02 uses only analog data A1 and A2
{
#if defined (VBAT) and not defined(FRSKY_FLD02)
#if defined OPENTX
uint16_t voltage;
voltage = analog.vbat * 10; // for OpenTX send the number of 1/10th of volt
sendDataHead(ID_VFAS);
write_FrSky16(voltage);
#else
uint16_t voltage;
uint16_t Data_Voltage_vBat_bp;
uint16_t Data_Voltage_vBat_ap;
voltage = ((analog.vbat * 110) / 21);
Data_Voltage_vBat_bp = voltage / 100;
sendDataHead(ID_Voltage_Amp_bp);
write_FrSky16(Data_Voltage_vBat_bp);
Data_Voltage_vBat_ap = ((voltage % 100) + 5) / 10;
sendDataHead(ID_Voltage_Amp_ap);
write_FrSky16(Data_Voltage_vBat_ap);
#endif
#endif
#if defined(POWERMETER)
uint16_t Data_Voltage_I_Motor;
Data_Voltage_I_Motor = analog.amperage;
sendDataHead(ID_Current);
write_FrSky16(Data_Voltage_I_Motor);
#endif
}
// Main function FrSky telemetry
void run_telemetry(void)
{
static uint32_t lastTime;
static uint8_t tele_loop;
if ((millis() - lastTime) > 125) {
// Data sent every 125ms due to scheduler
lastTime = millis();
tele_loop++;
switch (tele_loop) {
case 1:
send_Voltage_ampere();
send_Accel();
break;
case 2:
send_Fuel();
send_GPS_longitude();
break;
case 3:
send_Temperature();
send_Accel();
break;
case 4:
send_Altitude();
send_GPS_speed();
send_Course();
break;
case 5:
send_Voltage_ampere();
send_Accel();
break;
case 6:
send_RPM();
send_GPS_latitude();
break;
case 7:
send_GPS_speed();
send_Accel();
send_cell_volt();
break;
default:
send_Altitude();
send_Time();
tele_loop = 0;
break;
}
sendDataTail();
}
}
#endif // FRSKY telemetry
#if defined(SPORT_TELEMETRY)
static short _FrSkySport_crc;
static short _currentGPSValue;
void FrSkySport_sendByte(uint8_t byte)
{
// CRC update
_FrSkySport_crc += byte; //0-1FF
_FrSkySport_crc += _FrSkySport_crc >> 8; //0-100
_FrSkySport_crc &= 0x00ff;
_FrSkySport_crc += _FrSkySport_crc >> 8; //0-0FF
_FrSkySport_crc &= 0x00ff;
if ( (byte == FRSKY_START_STOP) || (byte == FRSKY_BYTESTUFF) ) {
SerialWrite(TELEMETRY_SERIAL, FRSKY_BYTESTUFF);
byte &= ~FRSKY_STUFF_MASK;
}
SerialWrite(TELEMETRY_SERIAL, byte);
}
void FrSkySport_sendCrc()
{
FrSkySport_sendByte(0xFF - _FrSkySport_crc);
}
void FrSkySport_sendValue(uint16_t id, uint32_t value)
{
_FrSkySport_crc = 0; // Reset CRC
FrSkySport_sendByte(0x10); // 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();
}
void FrSkySport_sendA2voltage()
{
#ifdef VBAT
uint32_t opentx_val = (255.0 * (float)(analog.vbat / (float)FRSKY_SPORT_A2_MAX));
FrSkySport_sendValue(FRSKY_SPORT_ADC2_ID, (opentx_val));
#endif
}
uint32_t FrSkySport_EncodeCoordinate(float latLon, bool isLat)
{
#if GPS
uint32_t otx_coord = 0;
if (!isLat)
{
otx_coord = abs(latLon); // now we have unsigned value and one bit to spare
otx_coord = (otx_coord + otx_coord / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (latLon < 0) otx_coord |= 0x40000000;
}
else {
otx_coord = abs(latLon); // now we have unsigned value and one bit to spare
otx_coord = (otx_coord + otx_coord / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (latLon < 0) otx_coord |= 0x40000000;
}
return otx_coord;
#endif
}
void FrSkySport_sendGPSCoordinate()
{
#if GPS
uint32_t GPSValueToSend = 0;
if (f.GPS_FIX && GPS_numSat >= 4)
{
switch (_currentGPSValue)
{
case 0:
GPSValueToSend = FrSkySport_EncodeCoordinate(GPS_coord[LON], false);
_currentGPSValue = 1;
break;
case 1:
GPSValueToSend = FrSkySport_EncodeCoordinate(GPS_coord[LAT], true);
_currentGPSValue = 0;
break;
}
FrSkySport_sendValue(FRSKY_SPORT_GPS_LONG_LATI_ID, GPSValueToSend);
}
#endif
}
void FrSkySport_sendGPSAltitude()
{
#if defined(TELEMETRY_ALT_GPS) and GPS
if (f.GPS_FIX && GPS_numSat >= 4)
{
FrSkySport_sendValue(FRSKY_SPORT_GPS_ALT_ID, (int32_t)(GPS_altitude)); // m???
}
#endif
}
void FrSkySport_sendGPSSpeed() {
#if GPS
if (f.GPS_FIX && GPS_numSat >= 4)
{
uint32_t speed = ((float)GPS_speed * 100);
FrSkySport_sendValue(FRSKY_SPORT_GPS_SPEED_ID, speed); // unknown unit, just a guess
}
#endif
}
void FrSkySport_sendAltitude()
{
#if defined(TELEMETRY_ALT_BARO) and BARO
FrSkySport_sendValue(FRSKY_SPORT_ALT_ID, (int32_t)(alt.EstAlt) * 100); // cm
#endif
}
void FrSkySport_sendHeading()
{
#if defined(TELEMETRY_COURSE_MAG) or (defined(TELEMETRY_COURSE_GPS) and GPS)
#if defined(TELEMETRY_COURSE_MAG)
uint32_t otx_heading = (uint32_t)(att.heading + 360) % 360 * 100;
FrSkySport_sendValue(FRSKY_SPORT_GPS_COURS_ID, otx_heading); // 1 deg = 100, 0 - 359000
#elif defined(TELEMETRY_COURSE_GPS) && defined(GPS)
if (f.GPS_FIX && GPS_numSat >= 4)
{
uint32_t otx_heading = (uint32_t)(GPS_ground_course + 360) % 360 * 100;
FrSkySport_sendValue(FRSKY_SPORT_GPS_COURS_ID, otx_heading); // 1 deg = 100, 0 - 359000
}
#endif
#endif
}
void FrSkySport_sendACCX()
{
#ifdef ACC
FrSkySport_sendValue(FRSKY_SPORT_ACCX_ID, imu.accSmooth[0] / 5); // unknown unit
#endif
}
void FrSkySport_sendACCY()
{
#ifdef ACC
FrSkySport_sendValue(FRSKY_SPORT_ACCY_ID, imu.accSmooth[1] / 5); // unknown unit
#endif
}
void FrSkySport_sendACCZ()
{
#ifdef ACC
FrSkySport_sendValue(FRSKY_SPORT_ACCZ_ID, imu.accSmooth[2] / 5); // unknown unit
#endif
}
void FrSkySport_sendAltVario()
{
#ifdef VARIOMETER
FrSkySport_sendValue(FRSKY_SPORT_VARIO_ID, ((uint32_t)alt.vario)); // unknown unit
#endif
}
void init_telemetry(void)
{
_currentGPSValue = 0;
SerialOpen(TELEMETRY_SERIAL,TELEMETRY_BAUD);
}
void run_telemetry(void)
{
static uint8_t lastRx = 0;
uint8_t c = SerialAvailable(TELEMETRY_SERIAL);
while (c--) {
int rx = SerialRead(TELEMETRY_SERIAL);
if (lastRx == FRSKY_START_STOP)
{
debug[1] = rx;
switch (rx)
{
case FRSKY_SPORT_DEVICE_4:
FrSkySport_sendA2voltage();
break;
case FRSKY_SPORT_DEVICE_8:
FrSkySport_sendACCX();
break;
case FRSKY_SPORT_DEVICE_9:
FrSkySport_sendACCY();
break;
case FRSKY_SPORT_DEVICE_10:
FrSkySport_sendACCZ();
break;
case FRSKY_SPORT_DEVICE_11:
FrSkySport_sendAltitude();
break;
case FRSKY_SPORT_DEVICE_12:
FrSkySport_sendAltVario();
break;
case FRSKY_SPORT_DEVICE_13:
FrSkySport_sendHeading();
break;
case FRSKY_SPORT_DEVICE_14:
FrSkySport_sendGPSSpeed();
break;
case FRSKY_SPORT_DEVICE_15:
FrSkySport_sendGPSAltitude();
break;
case FRSKY_SPORT_DEVICE_16:
FrSkySport_sendGPSCoordinate();
break;
}
}
lastRx = rx;
}
}
#endif // S.PORT telemetry