src/main/telemetry/hott.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /*
  19  * telemetry_hott.c
  20  *
  21  * Authors:
  22  * Dominic Clifton - Hydra - Software Serial, Electronics, Hardware Integration and debugging, HoTT Code cleanup and fixes, general telemetry improvements.
  23  * Carsten Giesen - cGiesen - Baseflight port
  24  * Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
  25  * Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed.
  26  *
  27  * https://github.com/obayer/MultiWii-HoTT
  28  * https://github.com/oBayer/MultiHoTT-Module
  29  * https://code.google.com/p/hott-for-ardupilot
  30  *
  31  * HoTT is implemented in Graupner equipment using a bi-directional protocol over a single wire.
  32  *
  33  * Generally the receiver sends a single request byte out using normal uart signals, then waits a short period for a
  34  * multiple byte response and checksum byte before it sends out the next request byte.
  35  * Each response byte must be send with a protocol specific delay between them.
  36  *
  37  * Serial ports use two wires but HoTT uses a single wire so some electronics are required so that
  38  * the signals don't get mixed up.  When cleanflight transmits it should not receive it's own transmission.
  39  *
  40  * Connect as follows:
  41  * HoTT TX/RX -> Serial RX (connect directly)
  42  * Serial TX -> 1N4148 Diode -(|  )-> HoTT TX/RX (connect via diode)
  43  *
  44  * The diode should be arranged to allow the data signals to flow the right way
  45  * -(|  )- == Diode, | indicates cathode marker.
  46  *
  47  * 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.
  48  *
  49  * Note: The softserial ports are not listed as 5V tolerant in the STM32F103xx data sheet pinouts and pin description
  50  * section.  Verify if you require a 5v/3.3v level shifters.  The softserial port should not be inverted.
  51  *
  52  * There is a technical discussion (in German) about HoTT here
  53  * http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
  54  */
  55 #include <stdbool.h>
  56 #include <stdint.h>
  57 #include <string.h>
  58
  59 #include "platform.h"
  60 #include "build_config.h"
  61 #include "debug.h"
  62
  63 #ifdef TELEMETRY
  64
  65 #include "common/axis.h"
  66
  67 #include "drivers/system.h"
  68
  69 #include "drivers/serial.h"
  70 #include "io/serial.h"
  71
  72 #include "config/runtime_config.h"
  73
  74 #include "sensors/sensors.h"
  75 #include "sensors/battery.h"
  76
  77 #include "flight/pid.h"
  78 #include "flight/navigation.h"
  79 #include "io/gps.h"
  80
  81 #include "telemetry/telemetry.h"
  82 #include "telemetry/hott.h"
  83
  84 //#define HOTT_DEBUG
  85
  86 #define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5)
  87 #define HOTT_RX_SCHEDULE 4000
  88 #define HOTT_TX_DELAY_US 3000
  89 #define MILLISECONDS_IN_A_SECOND 1000
  90
  91 static uint32_t lastHoTTRequestCheckAt = 0;
  92 static uint32_t lastMessagesPreparedAt = 0;
  93 static uint32_t lastHottAlarmSoundTime = 0;
  94
  95 static bool hottIsSending = false;
  96
  97 static uint8_t *hottMsg = NULL;
  98 static uint8_t hottMsgRemainingBytesToSendCount;
  99 static uint8_t hottMsgCrc;
 100
 101 #define HOTT_CRC_SIZE (sizeof(hottMsgCrc))
 102
 103 #define HOTT_BAUDRATE 19200
 104 #define HOTT_INITIAL_PORT_MODE MODE_RX
 105
 106 static serialPort_t *hottPort = NULL;
 107 static serialPortConfig_t *portConfig;
 108
 109 static telemetryConfig_t *telemetryConfig;
 110 static bool hottTelemetryEnabled =  false;
 111 static portSharing_e hottPortSharing;
 112
 113 static HOTT_GPS_MSG_t hottGPSMessage;
 114 static HOTT_EAM_MSG_t hottEAMMessage;
 115
 116 static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
 117 {
 118     memset(msg, 0, size);
 119     msg->start_byte = 0x7C;
 120     msg->eam_sensor_id = HOTT_TELEMETRY_EAM_SENSOR_ID;
 121     msg->sensor_id = HOTT_EAM_SENSOR_TEXT_ID;
 122     msg->stop_byte = 0x7D;
 123 }
 124
 125 #ifdef GPS
 126 typedef enum {
 127     GPS_FIX_CHAR_NONE = '-',
 128     GPS_FIX_CHAR_2D = '2',
 129     GPS_FIX_CHAR_3D = '3',
 130     GPS_FIX_CHAR_DGPS = 'D',
 131 } gpsFixChar_e;
 132
 133 static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
 134 {
 135     memset(msg, 0, size);
 136     msg->start_byte = 0x7C;
 137     msg->gps_sensor_id = HOTT_TELEMETRY_GPS_SENSOR_ID;
 138     msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID;
 139     msg->stop_byte = 0x7D;
 140 }
 141 #endif
 142
 143 static void initialiseMessages(void)
 144 {
 145     initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage));
 146 #ifdef GPS
 147     initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
 148 #endif
 149 }
 150
 151 #ifdef GPS
 152 void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
 153 {
 154     int16_t deg = latitude / GPS_DEGREES_DIVIDER;
 155     int32_t sec = (latitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
 156     int8_t min = sec / 1000000L;
 157     sec = (sec % 1000000L) / 100L;
 158     uint16_t degMin = (deg * 100L) + min;
 159
 160     hottGPSMessage->pos_NS = (latitude < 0);
 161     hottGPSMessage->pos_NS_dm_L = degMin;
 162     hottGPSMessage->pos_NS_dm_H = degMin >> 8;
 163     hottGPSMessage->pos_NS_sec_L = sec;
 164     hottGPSMessage->pos_NS_sec_H = sec >> 8;
 165
 166     deg = longitude / GPS_DEGREES_DIVIDER;
 167     sec = (longitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
 168     min = sec / 1000000L;
 169     sec = (sec % 1000000L) / 100L;
 170     degMin = (deg * 100L) + min;
 171
 172     hottGPSMessage->pos_EW = (longitude < 0);
 173     hottGPSMessage->pos_EW_dm_L = degMin;
 174     hottGPSMessage->pos_EW_dm_H = degMin >> 8;
 175     hottGPSMessage->pos_EW_sec_L = sec;
 176     hottGPSMessage->pos_EW_sec_H = sec >> 8;
 177 }
 178
 179 void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
 180 {
 181     hottGPSMessage->gps_satelites = GPS_numSat;
 182
 183     if (!STATE(GPS_FIX)) {
 184         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
 185         return;
 186     }
 187
 188     if (GPS_numSat >= 5) {
 189         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
 190     } else {
 191         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
 192     }
 193
 194     addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);
 195
 196     // GPS Speed in km/h
 197     uint16_t speed = (GPS_speed * 36) / 100; // 0->1m/s * 0->36 = km/h
 198     hottGPSMessage->gps_speed_L = speed & 0x00FF;
 199     hottGPSMessage->gps_speed_H = speed >> 8;
 200
 201     hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
 202     hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
 203
 204     uint16_t hottGpsAltitude = (GPS_altitude / 10) + HOTT_GPS_ALTITUDE_OFFSET; // 1 / 0.1f == 10, GPS_altitude of 1 == 0.1m
 205
 206     hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
 207     hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
 208
 209     hottGPSMessage->home_direction = GPS_directionToHome;
 210 }
 211 #endif
 212
 213 static bool shouldTriggerBatteryAlarmNow(void)
 214 {
 215     return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND));
 216 }
 217
 218 static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
 219 {
 220     batteryState_e batteryState;
 221
 222     if (shouldTriggerBatteryAlarmNow()){
 223         lastHottAlarmSoundTime = millis();
 224         batteryState = getBatteryState();
 225         if (batteryState == BATTERY_WARNING  || batteryState == BATTERY_CRITICAL){
 226             hottEAMMessage->warning_beeps = 0x10;
 227             hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1;
 228         }
 229         else {
 230             hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE;
 231             hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE;
 232         }
 233     }
 234 }
 235
 236 static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
 237 {
 238     hottEAMMessage->main_voltage_L = vbat & 0xFF;
 239     hottEAMMessage->main_voltage_H = vbat >> 8;
 240     hottEAMMessage->batt1_voltage_L = vbat & 0xFF;
 241     hottEAMMessage->batt1_voltage_H = vbat >> 8;
 242
 243     updateAlarmBatteryStatus(hottEAMMessage);
 244 }
 245
 246 static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
 247 {
 248     int32_t amp = amperage / 10;
 249     hottEAMMessage->current_L = amp & 0xFF;
 250     hottEAMMessage->current_H = amp >> 8;
 251 }
 252
 253 static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
 254 {
 255     int32_t mAh = mAhDrawn / 10;
 256     hottEAMMessage->batt_cap_L = mAh & 0xFF;
 257     hottEAMMessage->batt_cap_H = mAh >> 8;
 258 }
 259
 260 void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
 261 {
 262     // Reset alarms
 263     hottEAMMessage->warning_beeps = 0x0;
 264     hottEAMMessage->alarm_invers1 = 0x0;
 265
 266     hottEAMUpdateBattery(hottEAMMessage);
 267     hottEAMUpdateCurrentMeter(hottEAMMessage);
 268     hottEAMUpdateBatteryDrawnCapacity(hottEAMMessage);
 269 }
 270
 271 static void hottSerialWrite(uint8_t c)
 272 {
 273     static uint8_t serialWrites = 0;
 274     serialWrites++;
 275     serialWrite(hottPort, c);
 276 }
 277
 278 void freeHoTTTelemetryPort(void)
 279 {
 280     closeSerialPort(hottPort);
 281     hottPort = NULL;
 282     hottTelemetryEnabled = false;
 283 }
 284
 285 void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
 286 {
 287     telemetryConfig = initialTelemetryConfig;
 288     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT);
 289     hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT);
 290
 291     initialiseMessages();
 292 }
 293
 294 void configureHoTTTelemetryPort(void)
 295 {
 296     if (!portConfig) {
 297         return;
 298     }
 299
 300     hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
 301
 302     if (!hottPort) {
 303         return;
 304     }
 305
 306     hottTelemetryEnabled = true;
 307 }
 308
 309 static void hottSendResponse(uint8_t *buffer, int length)
 310 {
 311     if(hottIsSending) {
 312         return;
 313     }
 314
 315     hottMsg = buffer;
 316     hottMsgRemainingBytesToSendCount = length + HOTT_CRC_SIZE;
 317 }
 318
 319 static inline void hottSendGPSResponse(void)
 320 {
 321     hottSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
 322 }
 323
 324 static inline void hottSendEAMResponse(void)
 325 {
 326     hottSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
 327 }
 328
 329 static void hottPrepareMessages(void) {
 330     hottPrepareEAMResponse(&hottEAMMessage);
 331 #ifdef GPS
 332     hottPrepareGPSResponse(&hottGPSMessage);
 333 #endif
 334 }
 335
 336 static void processBinaryModeRequest(uint8_t address) {
 337
 338 #ifdef HOTT_DEBUG
 339     static uint8_t hottBinaryRequests = 0;
 340     static uint8_t hottGPSRequests = 0;
 341     static uint8_t hottEAMRequests = 0;
 342 #endif
 343
 344     switch (address) {
 345 #ifdef GPS
 346         case 0x8A:
 347 #ifdef HOTT_DEBUG
 348             hottGPSRequests++;
 349 #endif
 350             if (sensors(SENSOR_GPS)) {
 351                 hottSendGPSResponse();
 352             }
 353             break;
 354 #endif
 355         case 0x8E:
 356 #ifdef HOTT_DEBUG
 357             hottEAMRequests++;
 358 #endif
 359             hottSendEAMResponse();
 360             break;
 361     }
 362
 363
 364 #ifdef HOTT_DEBUG
 365     hottBinaryRequests++;
 366     debug[0] = hottBinaryRequests;
 367 #ifdef GPS
 368     debug[1] = hottGPSRequests;
 369 #endif
 370     debug[2] = hottEAMRequests;
 371 #endif
 372
 373 }
 374
 375 static void flushHottRxBuffer(void)
 376 {
 377     while (serialRxBytesWaiting(hottPort) > 0) {
 378         serialRead(hottPort);
 379     }
 380 }
 381
 382 static void hottCheckSerialData(uint32_t currentMicros)
 383 {
 384     static bool lookingForRequest = true;
 385
 386     uint8_t bytesWaiting = serialRxBytesWaiting(hottPort);
 387
 388     if (bytesWaiting <= 1) {
 389         return;
 390     }
 391
 392     if (bytesWaiting != 2) {
 393         flushHottRxBuffer();
 394         lookingForRequest = true;
 395         return;
 396     }
 397
 398     if (lookingForRequest) {
 399         lastHoTTRequestCheckAt = currentMicros;
 400         lookingForRequest = false;
 401         return;
 402     } else {
 403         bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE;
 404
 405         if (!enoughTimePassed) {
 406             return;
 407         }
 408         lookingForRequest = true;
 409     }
 410
 411     uint8_t requestId = serialRead(hottPort);
 412     uint8_t address = serialRead(hottPort);
 413
 414     if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) {
 415     /*
 416      * FIXME the first byte of the HoTT request frame is ONLY either 0x80 (binary mode) or 0x7F (text mode).
 417      * The binary mode is read as 0x00 (error reading the upper bit) while the text mode is correctly decoded.
 418      * The (requestId == 0) test is a workaround for detecting the binary mode with no ambiguity as there is only
 419      * one other valid value (0x7F) for text mode.
 420      * The error reading for the upper bit should nevertheless be fixed
 421      */
 422         processBinaryModeRequest(address);
 423     }
 424 }
 425
 426 static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) {
 427         closeSerialPort(hottPort);
 428         hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED);
 429 }
 430
 431 static void hottSendTelemetryData(void) {
 432     if (!hottIsSending) {
 433         hottIsSending = true;
 434         // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
 435         if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
 436                 workAroundForHottTelemetryOnUsart(hottPort, MODE_TX);
 437         else
 438                 serialSetMode(hottPort, MODE_TX);
 439         hottMsgCrc = 0;
 440         return;
 441     }
 442
 443     if (hottMsgRemainingBytesToSendCount == 0) {
 444         hottMsg = NULL;
 445         hottIsSending = false;
 446         // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
 447         if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
 448                 workAroundForHottTelemetryOnUsart(hottPort, MODE_RX);
 449         else
 450                 serialSetMode(hottPort, MODE_RX);
 451         flushHottRxBuffer();
 452         return;
 453     }
 454
 455     --hottMsgRemainingBytesToSendCount;
 456     if(hottMsgRemainingBytesToSendCount == 0) {
 457         hottSerialWrite(hottMsgCrc++);
 458         return;
 459     }
 460
 461     hottMsgCrc += *hottMsg;
 462     hottSerialWrite(*hottMsg++);
 463 }
 464
 465 static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros)
 466 {
 467     return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ;
 468 }
 469
 470 static inline bool shouldCheckForHoTTRequest()
 471 {
 472     if (hottIsSending) {
 473         return false;
 474     }
 475     return true;
 476 }
 477
 478 void checkHoTTTelemetryState(void)
 479 {
 480     bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing);
 481
 482     if (newTelemetryEnabledValue == hottTelemetryEnabled) {
 483         return;
 484     }
 485
 486     if (newTelemetryEnabledValue)
 487         configureHoTTTelemetryPort();
 488     else
 489         freeHoTTTelemetryPort();
 490 }
 491
 492 void handleHoTTTelemetry(void)
 493 {
 494     static uint32_t serialTimer;
 495
 496     if (!hottTelemetryEnabled) {
 497         return;
 498     }
 499
 500     uint32_t now = micros();
 501
 502     if (shouldPrepareHoTTMessages(now)) {
 503         hottPrepareMessages();
 504         lastMessagesPreparedAt = now;
 505     }
 506
 507     if (shouldCheckForHoTTRequest()) {
 508         hottCheckSerialData(now);
 509     }
 510
 511     if (!hottMsg)
 512         return;
 513
 514     if (hottIsSending) {
 515         if(now - serialTimer < HOTT_TX_DELAY_US) {
 516             return;
 517         }
 518     }
 519     hottSendTelemetryData();
 520     serialTimer = now;
 521 }
 522
 523 #endif