hott.c 13.8 KB
Newer Older
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
/*
 * 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/>.
 */

/*
 * telemetry_hott.c
 *
 * Authors:
 * Dominic Clifton - Hydra - Software Serial, Electronics, Hardware Integration and debugging, HoTT Code cleanup and fixes, general telemetry improvements.
 * Carsten Giesen - cGiesen - Baseflight port
 * Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
 * Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed.
 *
 * https://github.com/obayer/MultiWii-HoTT
 * https://github.com/oBayer/MultiHoTT-Module
 * https://code.google.com/p/hott-for-ardupilot
 *
 * HoTT is implemented in Graupner equipment using a bi-directional protocol over a single wire.
 *
 * Generally the receiver sends a single request byte out using normal uart signals, then waits a short period for a
 * multiple byte response and checksum byte before it sends out the next request byte.
 * Each response byte must be send with a protocol specific delay between them.
 *
 * Serial ports use two wires but HoTT uses a single wire so some electronics are required so that
 * the signals don't get mixed up.  When cleanflight transmits it should not receive it's own transmission.
 *
 * Connect as follows:
 * HoTT TX/RX -> Serial RX (connect directly)
 * Serial TX -> 1N4148 Diode -(|  )-> HoTT TX/RX (connect via diode)
 *
 * The diode should be arranged to allow the data signals to flow the right way
 * -(|  )- == Diode, | indicates cathode marker.
 *
 * As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins.
 *
 * Note: The softserial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description
 * section.  Verify if you require a 5v/3.3v level shifters.  The softserial port should not be inverted.
 *
 * There is a technical discussion (in German) about HoTT here
 * http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
 */
#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "platform.h"
60
#include "build_config.h"
Dominic Clifton's avatar
Dominic Clifton committed
61
#include "debug.h"
62

63
64
65
66
67
68
69
70
71
72
73
74
75
76
#ifdef TELEMETRY

#include "common/axis.h"

#include "drivers/system.h"

#include "drivers/serial.h"
#include "io/serial.h"

#include "config/runtime_config.h"

#include "sensors/sensors.h"
#include "sensors/battery.h"

77
#include "flight/pid.h"
78
79
80
81
82
83
84
85
86
87
88
#include "flight/navigation.h"
#include "io/gps.h"

#include "telemetry/telemetry.h"
#include "telemetry/hott.h"

//#define HOTT_DEBUG

#define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5)
#define HOTT_RX_SCHEDULE 4000
#define HOTT_TX_DELAY_US 3000
Pierre-A's avatar
Pierre-A committed
89
#define MILLISECONDS_IN_A_SECOND 1000
90
91
92

static uint32_t lastHoTTRequestCheckAt = 0;
static uint32_t lastMessagesPreparedAt = 0;
Pierre-A's avatar
Pierre-A committed
93
static uint32_t lastHottAlarmSoundTime = 0;
94
95
96
97
98
99
100
101
102

static bool hottIsSending = false;

static uint8_t *hottMsg = NULL;
static uint8_t hottMsgRemainingBytesToSendCount;
static uint8_t hottMsgCrc;

#define HOTT_CRC_SIZE (sizeof(hottMsgCrc))

103
#define HOTT_BAUDRATE 19200
104
105
#define HOTT_INITIAL_PORT_MODE MODE_RX

106
static serialPort_t *hottPort = NULL;
107
static serialPortConfig_t *portConfig;
108
109

static telemetryConfig_t *telemetryConfig;
110
111
static bool hottTelemetryEnabled =  false;
static portSharing_e hottPortSharing;
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

static HOTT_GPS_MSG_t hottGPSMessage;
static HOTT_EAM_MSG_t hottEAMMessage;

static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
{
    memset(msg, 0, size);
    msg->start_byte = 0x7C;
    msg->eam_sensor_id = HOTT_TELEMETRY_EAM_SENSOR_ID;
    msg->sensor_id = HOTT_EAM_SENSOR_TEXT_ID;
    msg->stop_byte = 0x7D;
}

#ifdef GPS
typedef enum {
    GPS_FIX_CHAR_NONE = '-',
    GPS_FIX_CHAR_2D = '2',
    GPS_FIX_CHAR_3D = '3',
    GPS_FIX_CHAR_DGPS = 'D',
} gpsFixChar_e;

static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
{
    memset(msg, 0, size);
    msg->start_byte = 0x7C;
    msg->gps_sensor_id = HOTT_TELEMETRY_GPS_SENSOR_ID;
    msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID;
    msg->stop_byte = 0x7D;
}
#endif

static void initialiseMessages(void)
{
    initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage));
#ifdef GPS
    initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
#endif
}

#ifdef GPS
void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
{
154
155
    int16_t deg = latitude / GPS_DEGREES_DIVIDER;
    int32_t sec = (latitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
156
157
158
159
160
161
162
163
164
165
    int8_t min = sec / 1000000L;
    sec = (sec % 1000000L) / 100L;
    uint16_t degMin = (deg * 100L) + min;

    hottGPSMessage->pos_NS = (latitude < 0);
    hottGPSMessage->pos_NS_dm_L = degMin;
    hottGPSMessage->pos_NS_dm_H = degMin >> 8;
    hottGPSMessage->pos_NS_sec_L = sec;
    hottGPSMessage->pos_NS_sec_H = sec >> 8;

166
167
    deg = longitude / GPS_DEGREES_DIVIDER;
    sec = (longitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
    min = sec / 1000000L;
    sec = (sec % 1000000L) / 100L;
    degMin = (deg * 100L) + min;

    hottGPSMessage->pos_EW = (longitude < 0);
    hottGPSMessage->pos_EW_dm_L = degMin;
    hottGPSMessage->pos_EW_dm_H = degMin >> 8;
    hottGPSMessage->pos_EW_sec_L = sec;
    hottGPSMessage->pos_EW_sec_H = sec >> 8;
}

void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
    hottGPSMessage->gps_satelites = GPS_numSat;

183
    if (!STATE(GPS_FIX)) {
184
185
186
187
188
189
190
191
192
193
194
195
196
        hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
        return;
    }

    if (GPS_numSat >= 5) {
        hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
    } else {
        hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
    }

    addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);

    // GPS Speed in km/h
197
    uint16_t speed = (GPS_speed * 36) / 100; // 0->1m/s * 0->36 = km/h
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
    hottGPSMessage->gps_speed_L = speed & 0x00FF;
    hottGPSMessage->gps_speed_H = speed >> 8;

    hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
    hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;

    uint16_t hottGpsAltitude = (GPS_altitude / 10) + HOTT_GPS_ALTITUDE_OFFSET; // 1 / 0.1f == 10, GPS_altitude of 1 == 0.1m

    hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
    hottGPSMessage->altitude_H = hottGpsAltitude >> 8;

    hottGPSMessage->home_direction = GPS_directionToHome;
}
#endif

213
static bool shouldTriggerBatteryAlarmNow(void)
214
{
215
    return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND));
216
217
}

Pierre-A's avatar
Pierre-A committed
218
static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
219
{
220
221
    if (shouldTriggerBatteryAlarmNow()){
        lastHottAlarmSoundTime = millis();
222
        if (vbat <= batteryWarningVoltage){
223
224
            hottEAMMessage->warning_beeps = 0x10;
            hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1;
225
226
        }
        else {
227
228
            hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE;
            hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE;
229
230
231
232
        }
    }
}

233
234
235
236
237
238
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{
    hottEAMMessage->main_voltage_L = vbat & 0xFF;
    hottEAMMessage->main_voltage_H = vbat >> 8;
    hottEAMMessage->batt1_voltage_L = vbat & 0xFF;
    hottEAMMessage->batt1_voltage_H = vbat >> 8;
239

Pierre-A's avatar
Pierre-A committed
240
    updateAlarmBatteryStatus(hottEAMMessage);
241
242
}

Pierre-A's avatar
Pierre-A committed
243
244
static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
{
Petr Ledvina's avatar
Petr Ledvina committed
245
246
    int32_t amp = amperage / 10;
    hottEAMMessage->current_L = amp & 0xFF;
Pierre-A's avatar
Pierre-A committed
247
248
249
250
251
    hottEAMMessage->current_H = amp >> 8;
}

static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
{
Petr Ledvina's avatar
Petr Ledvina committed
252
253
    int32_t mAh = mAhDrawn / 10;
    hottEAMMessage->batt_cap_L = mAh & 0xFF;
Pierre-A's avatar
Pierre-A committed
254
255
256
    hottEAMMessage->batt_cap_H = mAh >> 8;
}

257
258
259
260
261
262
263
void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
{
    // Reset alarms
    hottEAMMessage->warning_beeps = 0x0;
    hottEAMMessage->alarm_invers1 = 0x0;

    hottEAMUpdateBattery(hottEAMMessage);
Pierre-A's avatar
Pierre-A committed
264
265
    hottEAMUpdateCurrentMeter(hottEAMMessage);
    hottEAMUpdateBatteryDrawnCapacity(hottEAMMessage);
266
267
268
269
270
271
272
273
274
275
276
}

static void hottSerialWrite(uint8_t c)
{
    static uint8_t serialWrites = 0;
    serialWrites++;
    serialWrite(hottPort, c);
}

void freeHoTTTelemetryPort(void)
{
277
278
    closeSerialPort(hottPort);
    hottPort = NULL;
279
    hottTelemetryEnabled = false;
280
281
282
283
284
}

void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
{
    telemetryConfig = initialTelemetryConfig;
285
286
    portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT);
    hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT);
287
288
289
290
291
292

    initialiseMessages();
}

void configureHoTTTelemetryPort(void)
{
293
294
295
    if (!portConfig) {
        return;
    }
296

297
    hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
298

299
300
301
    if (!hottPort) {
        return;
    }
302

303
    hottTelemetryEnabled = true;
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
}

static void hottSendResponse(uint8_t *buffer, int length)
{
    if(hottIsSending) {
        return;
    }

    hottMsg = buffer;
    hottMsgRemainingBytesToSendCount = length + HOTT_CRC_SIZE;
}

static inline void hottSendGPSResponse(void)
{
    hottSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
}

static inline void hottSendEAMResponse(void)
{
    hottSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
}

static void hottPrepareMessages(void) {
    hottPrepareEAMResponse(&hottEAMMessage);
#ifdef GPS
    hottPrepareGPSResponse(&hottGPSMessage);
#endif
}

static void processBinaryModeRequest(uint8_t address) {

#ifdef HOTT_DEBUG
    static uint8_t hottBinaryRequests = 0;
    static uint8_t hottGPSRequests = 0;
    static uint8_t hottEAMRequests = 0;
#endif

    switch (address) {
#ifdef GPS
        case 0x8A:
#ifdef HOTT_DEBUG
            hottGPSRequests++;
#endif
            if (sensors(SENSOR_GPS)) {
                hottSendGPSResponse();
            }
            break;
#endif
        case 0x8E:
#ifdef HOTT_DEBUG
            hottEAMRequests++;
#endif
            hottSendEAMResponse();
            break;
    }


#ifdef HOTT_DEBUG
    hottBinaryRequests++;
    debug[0] = hottBinaryRequests;
#ifdef GPS
    debug[1] = hottGPSRequests;
#endif
    debug[2] = hottEAMRequests;
#endif

}

static void flushHottRxBuffer(void)
{
    while (serialTotalBytesWaiting(hottPort) > 0) {
        serialRead(hottPort);
    }
}

379
380
static void hottCheckSerialData(uint32_t currentMicros)
{
381
382
    static bool lookingForRequest = true;

383
    uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407

    if (bytesWaiting <= 1) {
        return;
    }

    if (bytesWaiting != 2) {
        flushHottRxBuffer();
        lookingForRequest = true;
        return;
    }

    if (lookingForRequest) {
        lastHoTTRequestCheckAt = currentMicros;
        lookingForRequest = false;
        return;
    } else {
        bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;

        if (!enoughTimePassed) {
            return;
        }
        lookingForRequest = true;
    }

408
409
    uint8_t requestId = serialRead(hottPort);
    uint8_t address = serialRead(hottPort);
410

Pierre-A's avatar
Pierre-A committed
411
    if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) {
412
413
414
415
416
417
418
    /*
     * FIXME the first byte of the HoTT request frame is ONLY either 0x80 (binary mode) or 0x7F (text mode).
     * The binary mode is read as 0x00 (error reading the upper bit) while the text mode is correctly decoded.
     * The (requestId == 0) test is a workaround for detecting the binary mode with no ambiguity as there is only
     * one other valid value (0x7F) for text mode.
     * The error reading for the upper bit should nevertheless be fixed
     */
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
        processBinaryModeRequest(address);
    }
}

static void hottSendTelemetryData(void) {
    if (!hottIsSending) {
        hottIsSending = true;
        serialSetMode(hottPort, MODE_TX);
        hottMsgCrc = 0;
        return;
    }

    if (hottMsgRemainingBytesToSendCount == 0) {
        hottMsg = NULL;
        hottIsSending = false;

        serialSetMode(hottPort, MODE_RX);
        flushHottRxBuffer();
        return;
    }

    --hottMsgRemainingBytesToSendCount;
    if(hottMsgRemainingBytesToSendCount == 0) {
        hottSerialWrite(hottMsgCrc++);
        return;
    }

    hottMsgCrc += *hottMsg;
    hottSerialWrite(*hottMsg++);
}

static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros)
{
    return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ;
}

static inline bool shouldCheckForHoTTRequest()
{
    if (hottIsSending) {
        return false;
    }
    return true;
}

463
464
void checkHoTTTelemetryState(void)
{
465
    bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing);
466
467
468
469
470
471
472
473
474
475
476

    if (newTelemetryEnabledValue == hottTelemetryEnabled) {
        return;
    }

    if (newTelemetryEnabledValue)
        configureHoTTTelemetryPort();
    else
        freeHoTTTelemetryPort();
}

477
478
479
480
void handleHoTTTelemetry(void)
{
    static uint32_t serialTimer;

481
482
483
484
485
    if (!hottTelemetryEnabled) {
        return;
    }

    uint32_t now = micros();
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508

    if (shouldPrepareHoTTMessages(now)) {
        hottPrepareMessages();
        lastMessagesPreparedAt = now;
    }

    if (shouldCheckForHoTTRequest()) {
        hottCheckSerialData(now);
    }

    if (!hottMsg)
        return;

    if (hottIsSending) {
        if(now - serialTimer < HOTT_TX_DELAY_US) {
            return;
        }
    }
    hottSendTelemetryData();
    serialTimer = now;
}

#endif