frsky.c 14 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
19
20
 * Initial FrSky Telemetry implementation by silpstream @ rcgroups.
 * Addition protocol work by airmamaf @ github.
21
22
23
24
25
26
27
28
29
30
31
32
33
 */
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>

#include "platform.h"

#ifdef TELEMETRY

#include "common/maths.h"
#include "common/axis.h"

#include "drivers/system.h"
34
#include "drivers/sensor.h"
35
36
37
38
39
40
41
#include "drivers/accgyro.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/serial.h"


#include "sensors/sensors.h"
42
#include "sensors/acceleration.h"
43
44
45
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
46
47
48

#include "io/serial.h"
#include "io/rc_controls.h"
49
50
#include "io/gps.h"

51
52
53
#include "rx/rx.h"

#include "flight/mixer.h"
54
#include "flight/pid.h"
55
56
57
58
59
60
#include "flight/imu.h"
#include "flight/altitudehold.h"

#include "config/runtime_config.h"
#include "config/config.h"

61
62
63
#include "telemetry/telemetry.h"
#include "telemetry/frsky.h"

64
static serialPort_t *frskyPort = NULL;
65
66
static serialPortConfig_t *portConfig;

67
#define FRSKY_BAUDRATE 9600
68
69
70
#define FRSKY_INITIAL_PORT_MODE MODE_TX

static telemetryConfig_t *telemetryConfig;
71
72
73
static bool frskyTelemetryEnabled =  false;
static portSharing_e frskyPortSharing;

74

75
76
extern batteryConfig_t *batteryConfig;

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
extern int16_t telemTemperature1; // FIXME dependency on mw.c

#define CYCLETIME             125

#define PROTOCOL_HEADER       0x5E
#define PROTOCOL_TAIL         0x5E

// Data Ids  (bp = before decimal point; af = after decimal point)
// Official data IDs
#define ID_GPS_ALTIDUTE_BP    0x01
#define ID_GPS_ALTIDUTE_AP    0x09
#define ID_TEMPRATURE1        0x02
#define ID_RPM                0x03
#define ID_FUEL_LEVEL         0x04
#define ID_TEMPRATURE2        0x05
#define ID_VOLT               0x06
#define ID_ALTITUDE_BP        0x10
#define ID_ALTITUDE_AP        0x21
#define ID_GPS_SPEED_BP       0x11
#define ID_GPS_SPEED_AP       0x19
#define ID_LONGITUDE_BP       0x12
#define ID_LONGITUDE_AP       0x1A
#define ID_E_W                0x22
#define ID_LATITUDE_BP        0x13
#define ID_LATITUDE_AP        0x1B
#define ID_N_S                0x23
#define ID_COURSE_BP          0x14
#define ID_COURSE_AP          0x1C
#define ID_DATE_MONTH         0x15
#define ID_YEAR               0x16
#define ID_HOUR_MINUTE        0x17
#define ID_SECOND             0x18
#define ID_ACC_X              0x24
#define ID_ACC_Y              0x25
#define ID_ACC_Z              0x26
112
#define ID_VOLTAGE_AMP        0x39
113
114
115
116
117
118
119
120
121
122
#define ID_VOLTAGE_AMP_BP     0x3A
#define ID_VOLTAGE_AMP_AP     0x3B
#define ID_CURRENT            0x28
// User defined data IDs
#define ID_GYRO_X             0x40
#define ID_GYRO_Y             0x41
#define ID_GYRO_Z             0x42

#define ID_VERT_SPEED         0x30 //opentx vario

Airmamaf's avatar
Airmamaf committed
123
124
125
#define GPS_BAD_QUALITY       300
#define GPS_MAX_HDOP_VAL      9999
#define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s
126
#define BLADE_NUMBER_DIVIDER  5 // should set 12 blades in Taranis
Airmamaf's avatar
Airmamaf committed
127
128
129

static uint32_t lastCycleTime = 0;
static uint8_t cycleNum = 0;
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
static void sendDataHead(uint8_t id)
{
    serialWrite(frskyPort, PROTOCOL_HEADER);
    serialWrite(frskyPort, id);
}

static void sendTelemetryTail(void)
{
    serialWrite(frskyPort, PROTOCOL_TAIL);
}

static void serializeFrsky(uint8_t data)
{
    // take care of byte stuffing
    if (data == 0x5e) {
        serialWrite(frskyPort, 0x5d);
        serialWrite(frskyPort, 0x3e);
    } else if (data == 0x5d) {
        serialWrite(frskyPort, 0x5d);
        serialWrite(frskyPort, 0x3d);
    } else
        serialWrite(frskyPort, data);
}

static void serialize16(int16_t a)
{
    uint8_t t;
    t = a;
    serializeFrsky(t);
    t = a >> 8 & 0xff;
    serializeFrsky(t);
}

static void sendAccel(void)
{
    int i;

    for (i = 0; i < 3; i++) {
        sendDataHead(ID_ACC_X + i);
        serialize16(((float)accSmooth[i] / acc_1G) * 1000);
    }
}

static void sendBaro(void)
{
    sendDataHead(ID_ALTITUDE_BP);
    serialize16(BaroAlt / 100);
    sendDataHead(ID_ALTITUDE_AP);
178
    serialize16(ABS(BaroAlt % 100));
179
180
}

181
#ifdef GPS
Airmamaf's avatar
Airmamaf committed
182
183
184
185
static void sendGpsAltitude(void)
{
    uint16_t altitude = GPS_altitude;
    //Send real GPS altitude only if it's reliable (there's a GPS fix)
186
    if (!STATE(GPS_FIX)) {
Airmamaf's avatar
Airmamaf committed
187
188
189
190
191
192
193
        altitude = 0;
    }
    sendDataHead(ID_GPS_ALTIDUTE_BP);
    serialize16(altitude);
    sendDataHead(ID_GPS_ALTIDUTE_AP);
    serialize16(0);
}
194
#endif
Airmamaf's avatar
Airmamaf committed
195

pinkywafer's avatar
pinkywafer committed
196
static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
Airmamaf's avatar
Airmamaf committed
197
{
pinkywafer's avatar
pinkywafer committed
198
    uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
Airmamaf's avatar
Airmamaf committed
199
    sendDataHead(ID_RPM);
200
    if (ARMING_FLAG(ARMED)) {
pinkywafer's avatar
pinkywafer committed
201
202
203
204
        throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
        if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
                    throttleForRPM = 0;
        serialize16(throttleForRPM);
Airmamaf's avatar
Airmamaf committed
205
    } else {
206
        serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
Airmamaf's avatar
Airmamaf committed
207
    }
208

Airmamaf's avatar
Airmamaf committed
209
}
210

211
212
213
static void sendTemperature1(void)
{
    sendDataHead(ID_TEMPRATURE1);
214
#ifdef BARO
Airmamaf's avatar
Airmamaf committed
215
    serialize16((baroTemperature + 50)/ 100); //Airmamaf
216
#else
217
    serialize16(telemTemperature1 / 10);
218
#endif
219
220
}

221
#ifdef GPS
222
static void sendSatalliteSignalQualityAsTemperature2(void)
Airmamaf's avatar
Airmamaf committed
223
224
{
    uint16_t satellite = GPS_numSat;
225
    if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
Airmamaf's avatar
Airmamaf committed
226
227
228
229
        satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
    }
    sendDataHead(ID_TEMPRATURE2);

230
    if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
Airmamaf's avatar
Airmamaf committed
231
232
        serialize16(satellite);
    } else {
233
        float tmp = (satellite - 32) / 1.8f;
Airmamaf's avatar
Airmamaf committed
234
        //Round the value
Petr Ledvina's avatar
Petr Ledvina committed
235
        tmp += (tmp < 0) ? -0.5f : 0.5f;
Airmamaf's avatar
Airmamaf committed
236
237
238
        serialize16(tmp);
    }
}
239

Airmamaf's avatar
Airmamaf committed
240
241
static void sendSpeed(void)
{
242
243
    if (!STATE(GPS_FIX)) {
        return;
Airmamaf's avatar
Airmamaf committed
244
    }
Samuel Brucksch's avatar
Samuel Brucksch committed
245
    //Speed should be sent in knots (GPS speed is in cm/s)
246
    sendDataHead(ID_GPS_SPEED_BP);
Samuel Brucksch's avatar
Samuel Brucksch committed
247
    //convert to knots: 1cm/s = 0.0194384449 knots
Samuel Brucksch's avatar
Samuel Brucksch committed
248
    serialize16(GPS_speed * 1944 / 10000);
249
    sendDataHead(ID_GPS_SPEED_AP);
Samuel Brucksch's avatar
Samuel Brucksch committed
250
    serialize16((GPS_speed * 1944 / 100) % 100);
Airmamaf's avatar
Airmamaf committed
251
}
252
#endif
253

254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
static void sendTime(void)
{
    uint32_t seconds = millis() / 1000;
    uint8_t minutes = (seconds / 60) % 60;

    // if we fly for more than an hour, something's wrong anyway
    sendDataHead(ID_HOUR_MINUTE);
    serialize16(minutes << 8);
    sendDataHead(ID_SECOND);
    serialize16(seconds % 60);
}

// Frsky pdf: dddmm.mmmm
// .mmmm is returned in decimal fraction of minutes.
static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
{
    int32_t absgps, deg, min;
271
    absgps = ABS(mwiigps);
272
273
274
    deg    = absgps / GPS_DEGREES_DIVIDER;
    absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;        // absgps = Minutes left * 10^7
    min    = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
275

276
    if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
Airmamaf's avatar
Airmamaf committed
277
278
        result->dddmm = deg * 100 + min;
    } else {
279
        result->dddmm = deg * 60 + min;
Airmamaf's avatar
Airmamaf committed
280
281
    }

282
    result->mmmm  = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
283
284
}

285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
static void sendLatLong(int32_t coord[2])
{
    gpsCoordinateDDDMMmmmm_t coordinate;
    GPStoDDDMM_MMMM(coord[LAT], &coordinate);
    sendDataHead(ID_LATITUDE_BP);
    serialize16(coordinate.dddmm);
    sendDataHead(ID_LATITUDE_AP);
    serialize16(coordinate.mmmm);
    sendDataHead(ID_N_S);
    serialize16(coord[LAT] < 0 ? 'S' : 'N');

    GPStoDDDMM_MMMM(coord[LON], &coordinate);
    sendDataHead(ID_LONGITUDE_BP);
    serialize16(coordinate.dddmm);
    sendDataHead(ID_LONGITUDE_AP);
    serialize16(coordinate.mmmm);
    sendDataHead(ID_E_W);
    serialize16(coord[LON] < 0 ? 'W' : 'E');
}

305
static void sendFakeLatLong(void)
306
{
307
    // Heading is only displayed on OpenTX if non-zero lat/long is also sent
308
    int32_t coord[2] = {0,0};
309
    
310
311
312
313
314
315
    coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
    coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);

    sendLatLong(coord);
}

316
static void sendFakeLatLongThatAllowsHeadingDisplay(void)
317
318
319
320
321
322
{
    // Heading is only displayed on OpenTX if non-zero lat/long is also sent
    int32_t coord[2] = {
        1 * GPS_DEGREES_DIVIDER,
        1 * GPS_DEGREES_DIVIDER
    };
323
324
325
326

    sendLatLong(coord);
}

327
328
#ifdef GPS
static void sendGPSLatLong(void)
329
{
330
331
332
    static uint8_t gpsFixOccured = 0;

    if (STATE(GPS_FIX) || gpsFixOccured == 1) {
333
        // If we have ever had a fix, send the last known lat/long
Airmamaf's avatar
Airmamaf committed
334
        gpsFixOccured = 1;
335
        sendLatLong(GPS_coord);
Airmamaf's avatar
Airmamaf committed
336
    } else {
337
        // otherwise send fake lat/long in order to display compass value
338
        sendFakeLatLong();
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
}
#endif

/*
 * Send vertical speed for opentx. ID_VERT_SPEED
 * Unit is cm/s
 */
static void sendVario(void)
{
    sendDataHead(ID_VERT_SPEED);
    serialize16(vario);
}

/*
 * Send voltage via ID_VOLT
 *
 * NOTE: This sends voltage divided by batteryCellCount. To get the real
 * battery voltage, you need to multiply the value by batteryCellCount.
 */
static void sendVoltage(void)
{
    static uint16_t currentCell = 0;
    uint32_t cellVoltage;
    uint16_t payload;

    /*
366
     * Format for Voltage Data for single cells is like this:
367
368
369
370
371
     *
     *  llll llll cccc hhhh
     *  l: Low voltage bits
     *  h: High voltage bits
     *  c: Cell number (starting at 0)
372
373
374
     *
     * The actual value sent for cell voltage has resolution of 0.002 volts
     * Since vbat has resolution of 0.1 volts it has to be multiplied by 50
375
     */
376
    cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
377
378

    // Cell number is at bit 9-12
379
    payload = (currentCell << 4);
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398

    // Lower voltage bits are at bit 0-8
    payload |= ((cellVoltage & 0x0ff) << 8);

    // Higher voltage bits are at bits 13-15
    payload |= ((cellVoltage & 0xf00) >> 8);

    sendDataHead(ID_VOLT);
    serialize16(payload);

    currentCell++;
    currentCell %= batteryCellCount;
}

/*
 * Send voltage with ID_VOLTAGE_AMP
 */
static void sendVoltageAmp(void)
{
399
    if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
400
401
402
403
404
405
406
        /*
         * Use new ID 0x39 to send voltage directly in 0.1 volts resolution
         */
        sendDataHead(ID_VOLTAGE_AMP);
        serialize16(vbat);
    } else {
        uint16_t voltage = (vbat * 110) / 21;
407

408
409
410
411
412
        sendDataHead(ID_VOLTAGE_AMP_BP);
        serialize16(voltage / 100);
        sendDataHead(ID_VOLTAGE_AMP_AP);
        serialize16(((voltage % 100) + 5) / 10);
    }
413
414
415
416
417
418
419
420
421
422
423
}

static void sendAmperage(void)
{
    sendDataHead(ID_CURRENT);
    serialize16((uint16_t)(amperage / 10));
}

static void sendFuelLevel(void)
{
    sendDataHead(ID_FUEL_LEVEL);
424

425
426
    if (batteryConfig->batteryCapacity > 0) {
        serialize16((uint16_t)calculateBatteryCapacityRemainingPercentage());
Airmamaf's avatar
Airmamaf committed
427
428
429
    } else {
        serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
    }
430
431
432
433
434
435
436
437
438
439
440
441
442
}

static void sendHeading(void)
{
    sendDataHead(ID_COURSE_BP);
    serialize16(heading);
    sendDataHead(ID_COURSE_AP);
    serialize16(0);
}

void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig)
{
    telemetryConfig = initialTelemetryConfig;
443
444
    portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY);
    frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY);
445
446
447
448
}

void freeFrSkyTelemetryPort(void)
{
449
450
    closeSerialPort(frskyPort);
    frskyPort = NULL;
451
    frskyTelemetryEnabled = false;
452
453
454
455
}

void configureFrSkyTelemetryPort(void)
{
456
457
    if (!portConfig) {
        return;
458
    }
459

460
    frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
461
462
463
464
465
    if (!frskyPort) {
        return;
    }

    frskyTelemetryEnabled = true;
466
467
468
469
470
471
472
}

bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
{
    return currentMillis - lastCycleTime >= CYCLETIME;
}

473
474
void checkFrSkyTelemetryState(void)
{
475
    bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing);
476
477
478
479
480
481
482
483
484
485
486

    if (newTelemetryEnabledValue == frskyTelemetryEnabled) {
        return;
    }

    if (newTelemetryEnabledValue)
        configureFrSkyTelemetryPort();
    else
        freeFrSkyTelemetryPort();
}

pinkywafer's avatar
pinkywafer committed
487
void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
488
{
489
490
491
492
    if (!frskyTelemetryEnabled) {
        return;
    }

493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
    uint32_t now = millis();

    if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
        return;
    }

    lastCycleTime = now;

    cycleNum++;

    // Sent every 125ms
    sendAccel();
    sendVario();
    sendTelemetryTail();

    if ((cycleNum % 4) == 0) {      // Sent every 500ms
509
        if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
Airmamaf's avatar
Airmamaf committed
510
511
            sendBaro();
        }
512
513
514
515
516
517
        sendHeading();
        sendTelemetryTail();
    }

    if ((cycleNum % 8) == 0) {      // Sent every 1s
        sendTemperature1();
pinkywafer's avatar
pinkywafer committed
518
        sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle);
519
520
521
522
523
524
525
526
527

        if (feature(FEATURE_VBAT)) {
            sendVoltage();
            sendVoltageAmp();
            sendAmperage();
            sendFuelLevel();
        }

#ifdef GPS
Airmamaf's avatar
Airmamaf committed
528
        if (sensors(SENSOR_GPS)) {
529
530
531
            sendSpeed();
            sendGpsAltitude();
            sendSatalliteSignalQualityAsTemperature2();
532
            sendGPSLatLong();
533
        }
534
        else {
535
            sendFakeLatLongThatAllowsHeadingDisplay();
536
537
        }
#else
538
        sendFakeLatLongThatAllowsHeadingDisplay();
539
540
#endif

541
542
543
544
545
546
547
548
549
550
551
        sendTelemetryTail();
    }

    if (cycleNum == 40) {     //Frame 3: Sent every 5s
        cycleNum = 0;
        sendTime();
        sendTelemetryTail();
    }
}

#endif