radio/src/telemetry/telemetry.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22 #include "multi.h"
  23
  24 uint8_t telemetryStreaming = 0;
  25 uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];   // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
  26 uint8_t telemetryRxBufferCount = 0;
  27
  28 uint8_t telemetryState = TELEMETRY_INIT;
  29
  30 TelemetryData telemetryData;
  31
  32 uint8_t telemetryProtocol = 255;
  33
  34 #if defined(PCBSKY9X) && defined(REVX)
  35 uint8_t serialInversion = 0;
  36 #endif
  37
  38 void processTelemetryData(uint8_t data)
  39 {
  40 #if defined(CROSSFIRE)
  41   if (telemetryProtocol == PROTOCOL_TELEMETRY_CROSSFIRE) {
  42     processCrossfireTelemetryData(data);
  43     return;
  44   }
  45 #endif
  46
  47 #if defined(MULTIMODULE)
  48   if (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
  49     processSpektrumTelemetryData(EXTERNAL_MODULE, data, telemetryRxBuffer, telemetryRxBufferCount);
  50     return;
  51   }
  52   if (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) {
  53     processFlySkyTelemetryData(data, telemetryRxBuffer, telemetryRxBufferCount);
  54     return;
  55   }
  56   if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
  57     processMultiTelemetryData(data, EXTERNAL_MODULE);
  58     return;
  59   }
  60 #endif
  61
  62   processFrskyTelemetryData(data);
  63 }
  64
  65 inline bool isBadAntennaDetected()
  66 {
  67   if (!isRasValueValid())
  68     return false;
  69
  70   if (telemetryData.swrInternal.isFresh() && telemetryData.swrInternal.value() > FRSKY_BAD_ANTENNA_THRESHOLD)
  71     return true;
  72
  73   if (telemetryData.swrExternal.isFresh() && telemetryData.swrExternal.value() > FRSKY_BAD_ANTENNA_THRESHOLD)
  74     return true;
  75
  76   return false;
  77 }
  78
  79 void telemetryWakeup()
  80 {
  81   uint8_t requiredTelemetryProtocol = modelTelemetryProtocol();
  82   uint8_t data;
  83
  84 #if defined(REVX)
  85   uint8_t requiredSerialInversion = g_model.moduleData[EXTERNAL_MODULE].invertedSerial;
  86   if (telemetryProtocol != requiredTelemetryProtocol || serialInversion != requiredSerialInversion) {
  87     serialInversion = requiredSerialInversion;
  88     telemetryInit(requiredTelemetryProtocol);
  89   }
  90 #else
  91   if (telemetryProtocol != requiredTelemetryProtocol) {
  92     telemetryInit(requiredTelemetryProtocol);
  93   }
  94 #endif
  95
  96 #if defined(INTERNAL_MODULE_PXX2) || defined(EXTMODULE_USART)
  97   uint8_t frame[PXX2_FRAME_MAXLENGTH];
  98
  99   #if defined(INTERNAL_MODULE_PXX2)
 100   while (intmoduleFifo.getFrame(frame)) {
 101     processPXX2Frame(INTERNAL_MODULE, frame);
 102   }
 103   #endif
 104
 105   #if defined(EXTMODULE_USART)
 106   while (extmoduleFifo.getFrame(frame)) {
 107     processPXX2Frame(EXTERNAL_MODULE, frame);
 108   }
 109   #endif
 110 #endif
 111
 112 #if defined(INTERNAL_MODULE_MULTI)
 113   if (intmoduleFifo.pop(data)) {
 114     LOG_TELEMETRY_WRITE_START();
 115     do {
 116       processMultiTelemetryData(data, INTERNAL_MODULE);
 117       LOG_TELEMETRY_WRITE_BYTE(data);
 118     } while (intmoduleFifo.pop(data));
 119   }
 120 #endif
 121
 122 #if defined(STM32)
 123   if (telemetryGetByte(&data)) {
 124     LOG_TELEMETRY_WRITE_START();
 125     do {
 126       processTelemetryData(data);
 127       LOG_TELEMETRY_WRITE_BYTE(data);
 128     } while (telemetryGetByte(&data));
 129   }
 130 #elif defined(PCBSKY9X)
 131   if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
 132     uint8_t data;
 133     while (telemetrySecondPortReceive(data)) {
 134       processTelemetryData(data);
 135     }
 136   }
 137   else {
 138     // Receive serial data here
 139     rxPdcUsart(processTelemetryData);
 140   }
 141 #endif
 142
 143   for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
 144     const TelemetrySensor & sensor = g_model.telemetrySensors[i];
 145     if (sensor.type == TELEM_TYPE_CALCULATED) {
 146       telemetryItems[i].eval(sensor);
 147     }
 148   }
 149
 150 #if defined(VARIO)
 151   if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
 152     varioWakeup();
 153   }
 154 #endif
 155
 156   static tmr10ms_t alarmsCheckTime = 0;
 157   #define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds))
 158   if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
 159
 160     SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/);
 161
 162     bool sensorLost = false;
 163     for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
 164       if (isTelemetryFieldAvailable(i)) {
 165         TelemetryItem & item = telemetryItems[i];
 166         if (item.timeout == 0) {
 167           TelemetrySensor * sensor = & g_model.telemetrySensors[i];
 168           if (sensor->unit != UNIT_DATETIME) {
 169             item.setOld();
 170             sensorLost = true;
 171           }
 172         }
 173       }
 174     }
 175
 176     if (sensorLost && TELEMETRY_STREAMING() && !g_model.rssiAlarms.disabled) {
 177       audioEvent(AU_SENSOR_LOST);
 178     }
 179
 180 #if defined(PCBTARANIS) || defined(PCBHORUS)
 181     if (isBadAntennaDetected()) {
 182       AUDIO_RAS_RED();
 183       POPUP_WARNING(STR_WARNING);
 184       const char * w = STR_ANTENNAPROBLEM;
 185       SET_WARNING_INFO(w, strlen(w), 0);
 186       SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
 187     }
 188 #endif
 189
 190     if (!g_model.rssiAlarms.disabled) {
 191       if (TELEMETRY_STREAMING()) {
 192         if (TELEMETRY_RSSI() < g_model.rssiAlarms.getCriticalRssi() ) {
 193           AUDIO_RSSI_RED();
 194           SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
 195         }
 196         else if (TELEMETRY_RSSI() < g_model.rssiAlarms.getWarningRssi() ) {
 197           AUDIO_RSSI_ORANGE();
 198           SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
 199         }
 200       }
 201
 202       if (TELEMETRY_STREAMING()) {
 203         if (telemetryState == TELEMETRY_KO) {
 204           AUDIO_TELEMETRY_BACK();
 205         }
 206         telemetryState = TELEMETRY_OK;
 207       }
 208       else if (telemetryState == TELEMETRY_OK) {
 209         telemetryState = TELEMETRY_KO;
 210         if (!isModuleInBeepMode()) {
 211           AUDIO_TELEMETRY_LOST();
 212         }
 213       }
 214     }
 215   }
 216 }
 217
 218 void telemetryInterrupt10ms()
 219 {
 220   if (telemetryStreaming > 0) {
 221     bool tick160ms = (telemetryStreaming & 0x0F) == 0;
 222     for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
 223       const TelemetrySensor & sensor = g_model.telemetrySensors[i];
 224       if (sensor.type == TELEM_TYPE_CALCULATED) {
 225         telemetryItems[i].per10ms(sensor);
 226       }
 227       if (tick160ms && telemetryItems[i].timeout > 0) {
 228         telemetryItems[i].timeout--;
 229       }
 230     }
 231     telemetryStreaming--;
 232   }
 233   else {
 234 #if !defined(SIMU)
 235     telemetryData.rssi.reset();
 236 #endif
 237     for (auto & telemetryItem: telemetryItems) {
 238       if (telemetryItem.isAvailable()) {
 239         telemetryItem.setOld();
 240       }
 241     }
 242   }
 243 }
 244
 245 void telemetryReset()
 246 {
 247   telemetryData.clear();
 248
 249   for (auto & telemetryItem : telemetryItems) {
 250     telemetryItem.clear();
 251   }
 252
 253   telemetryStreaming = 0; // reset counter only if valid telemetry packets are being detected
 254
 255   telemetryState = TELEMETRY_INIT;
 256 }
 257
 258 // we don't reset the telemetry here as we would also reset the consumption after model load
 259 void telemetryInit(uint8_t protocol)
 260 {
 261   telemetryProtocol = protocol;
 262
 263   if (protocol == PROTOCOL_TELEMETRY_FRSKY_D) {
 264     telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
 265   }
 266
 267 #if defined(MULTIMODULE)
 268   else if (protocol == PROTOCOL_TELEMETRY_MULTIMODULE || protocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) {
 269     // The DIY Multi module always speaks 100000 baud regardless of the telemetry protocol in use
 270     telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2);
 271 #if defined(LUA)
 272     outputTelemetryBuffer.reset();
 273 #endif
 274   }
 275   else if (protocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
 276     // Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same since there is no real standard
 277     telemetryPortInit(125000, TELEMETRY_SERIAL_DEFAULT);
 278   }
 279 #endif
 280
 281 #if defined(CROSSFIRE)
 282   else if (protocol == PROTOCOL_TELEMETRY_CROSSFIRE) {
 283     telemetryPortInit(CROSSFIRE_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
 284 #if defined(LUA)
 285     outputTelemetryBuffer.reset();
 286 #endif
 287     telemetryPortSetDirectionOutput();
 288   }
 289 #endif
 290
 291 #if defined(AUX_SERIAL) || defined(PCBSKY9X)
 292   else if (protocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
 293     telemetryPortInit(0, TELEMETRY_SERIAL_DEFAULT);
 294     auxSerialTelemetryInit(PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY);
 295   }
 296 #endif
 297
 298   else {
 299     telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
 300 #if defined(LUA)
 301     outputTelemetryBuffer.reset();
 302 #endif
 303   }
 304
 305 #if defined(REVX) && !defined(SIMU)
 306   if (serialInversion) {
 307     setMFP();
 308   }
 309   else {
 310     clearMFP();
 311   }
 312 #endif
 313 }
 314
 315
 316 #if defined(LOG_TELEMETRY) && !defined(SIMU)
 317 extern FIL g_telemetryFile;
 318 void logTelemetryWriteStart()
 319 {
 320   static tmr10ms_t lastTime = 0;
 321   tmr10ms_t newTime = get_tmr10ms();
 322   if (lastTime != newTime) {
 323     struct gtm utm;
 324     gettime(&utm);
 325     f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0:", utm.tm_year+TM_YEAR_BASE, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
 326     lastTime = newTime;
 327   }
 328 }
 329
 330 void logTelemetryWriteByte(uint8_t data)
 331 {
 332   f_printf(&g_telemetryFile, " %02X", data);
 333 }
 334 #endif
 335
 336 OutputTelemetryBuffer outputTelemetryBuffer __DMA;
 337
 338 #if defined(LUA)
 339 Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
 340 #endif