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
  23 uint8_t telemetryStreaming = 0;
  24 uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];   // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
  25 uint8_t telemetryRxBufferCount = 0;
  26
  27 #if defined(WS_HOW_HIGH)
  28 uint8_t wshhStreaming = 0;
  29 #endif
  30
  31 uint8_t link_counter = 0;
  32
  33 #if defined(CPUARM)
  34 uint8_t telemetryState = TELEMETRY_INIT;
  35 #endif
  36
  37 TelemetryData telemetryData;
  38
  39 #if defined(CPUARM)
  40 uint8_t telemetryProtocol = 255;
  41 #endif
  42
  43 #if defined(PCBSKY9X) && defined(REVX)
  44 uint8_t serialInversion = 0;
  45 #endif
  46
  47 #if !defined(CPUARM)
  48 uint16_t getChannelRatio(source_t channel)
  49 {
  50   return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier;
  51 }
  52
  53 lcdint_t applyChannelRatio(source_t channel, lcdint_t val)
  54 {
  55   return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51;
  56 }
  57 #endif
  58
  59 void processTelemetryData(uint8_t data)
  60 {
  61 #if defined(CROSSFIRE)
  62   if (telemetryProtocol == PROTOCOL_PULSES_CROSSFIRE) {
  63     processCrossfireTelemetryData(data);
  64     return;
  65   }
  66 #endif
  67 #if defined(MULTIMODULE)
  68   if (telemetryProtocol == PROTOCOL_SPEKTRUM) {
  69     processSpektrumTelemetryData(data);
  70     return;
  71   }
  72   if (telemetryProtocol == PROTOCOL_FLYSKY_IBUS) {
  73     processFlySkyTelemetryData(data);
  74     return;
  75   }
  76   if (telemetryProtocol == PROTOCOL_MULTIMODULE) {
  77     processMultiTelemetryData(data);
  78     return;
  79   }
  80 #endif
  81   processFrskyTelemetryData(data);
  82 }
  83
  84 void telemetryWakeup()
  85 {
  86 #if defined(CPUARM)
  87   uint8_t requiredTelemetryProtocol = modelTelemetryProtocol();
  88 #if defined(REVX)
  89   uint8_t requiredSerialInversion = g_model.moduleData[EXTERNAL_MODULE].invertedSerial;
  90   if (telemetryProtocol != requiredTelemetryProtocol || serialInversion != requiredSerialInversion) {
  91     serialInversion = requiredSerialInversion;
  92 #else
  93    if (telemetryProtocol != requiredTelemetryProtocol) {
  94 #endif
  95     telemetryInit(requiredTelemetryProtocol);
  96   }
  97 #endif
  98
  99 #if defined(STM32)
 100   uint8_t data;
 101   if (telemetryGetByte(&data)) {
 102     LOG_TELEMETRY_WRITE_START();
 103     do {
 104       processTelemetryData(data);
 105       LOG_TELEMETRY_WRITE_BYTE(data);
 106     } while (telemetryGetByte(&data));
 107   }
 108 #elif defined(PCBSKY9X)
 109   if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
 110     uint8_t data;
 111     while (telemetrySecondPortReceive(data)) {
 112       processTelemetryData(data);
 113     }
 114   }
 115   else {
 116     // Receive serial data here
 117     rxPdcUsart(processTelemetryData);
 118   }
 119 #endif
 120
 121 #if !defined(CPUARM)
 122   if (IS_FRSKY_D_PROTOCOL()) {
 123     // Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
 124     static uint8_t frskyTxDelay = 5;
 125     if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
 126       frskyTxDelay = 5; // 50ms
 127 #if !defined(SIMU)
 128       frskyDSendNextAlarm();
 129 #endif
 130     }
 131   }
 132 #endif
 133
 134 #if defined(CPUARM)
 135   for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
 136     const TelemetrySensor & sensor = g_model.telemetrySensors[i];
 137     if (sensor.type == TELEM_TYPE_CALCULATED) {
 138       telemetryItems[i].eval(sensor);
 139     }
 140   }
 141 #endif
 142
 143 #if defined(VARIO)
 144   if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
 145     varioWakeup();
 146   }
 147 #endif
 148
 149 #define FRSKY_BAD_ANTENNA()            (IS_SWR_VALUE_VALID() && telemetryData.swr.value > 0x33)
 150
 151 #if defined(CPUARM)
 152   static tmr10ms_t alarmsCheckTime = 0;
 153   #define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds))
 154   if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
 155
 156     SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/);
 157
 158     bool sensor_lost = false;
 159     for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
 160       if (isTelemetryFieldAvailable(i)) {
 161         TelemetryItem & item = telemetryItems[i];
 162         if (item.hasReceiveTime() && item.getDelaySinceLastValue() > TELEMETRY_VALUE_OLD_THRESHOLD) {
 163           TelemetrySensor * sensor = & g_model.telemetrySensors[i];
 164           if (sensor->unit != UNIT_DATETIME) {
 165             item.setOld();
 166             sensor_lost = true;
 167           }
 168         }
 169       }
 170     }
 171     if (sensor_lost && TELEMETRY_STREAMING() &&  !g_model.rssiAlarms.disabled) {
 172       audioEvent(AU_SENSOR_LOST);
 173     }
 174
 175 #if defined(PCBTARANIS) || defined(PCBHORUS)
 176     if ((IS_MODULE_PXX(INTERNAL_MODULE) || IS_MODULE_PXX(EXTERNAL_MODULE)) && FRSKY_BAD_ANTENNA()) {
 177       AUDIO_SWR_RED();
 178       POPUP_WARNING(STR_WARNING);
 179       const char * w = STR_ANTENNAPROBLEM;
 180       SET_WARNING_INFO(w, strlen(w), 0);
 181       SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
 182     }
 183 #endif
 184
 185     if (!g_model.rssiAlarms.disabled) {
 186       if (TELEMETRY_STREAMING()) {
 187         if (TELEMETRY_RSSI() < g_model.rssiAlarms.getCriticalRssi() ) {
 188           AUDIO_RSSI_RED();
 189           SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
 190         }
 191         else if (TELEMETRY_RSSI() < g_model.rssiAlarms.getWarningRssi() ) {
 192           AUDIO_RSSI_ORANGE();
 193           SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
 194         }
 195       }
 196
 197       if (TELEMETRY_STREAMING()) {
 198         if (telemetryState == TELEMETRY_KO) {
 199           AUDIO_TELEMETRY_BACK();
 200         }
 201         telemetryState = TELEMETRY_OK;
 202       }
 203       else if (telemetryState == TELEMETRY_OK) {
 204         telemetryState = TELEMETRY_KO;
 205         AUDIO_TELEMETRY_LOST();
 206       }
 207     }
 208   }
 209 #endif
 210 }
 211
 212 void telemetryInterrupt10ms()
 213 {
 214 #if defined(FRSKY_HUB) && !defined(CPUARM)
 215   uint16_t voltage = 0; /* unit: 1/10 volts */
 216   for (uint8_t i=0; i<telemetryData.hub.cellsCount; i++)
 217     voltage += telemetryData.hub.cellVolts[i];
 218   voltage /= (10 / TELEMETRY_CELL_VOLTAGE_MUTLIPLIER);
 219   telemetryData.hub.cellsSum = voltage;
 220   if (telemetryData.hub.cellsSum < telemetryData.hub.minCells) {
 221     telemetryData.hub.minCells = telemetryData.hub.cellsSum;
 222   }
 223 #endif
 224
 225   if (TELEMETRY_STREAMING()) {
 226     if (!TELEMETRY_OPENXSENSOR()) {
 227 #if defined(CPUARM)
 228       for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
 229         const TelemetrySensor & sensor = g_model.telemetrySensors[i];
 230         if (sensor.type == TELEM_TYPE_CALCULATED) {
 231           telemetryItems[i].per10ms(sensor);
 232         }
 233       }
 234 #else
 235       // power calculation
 236       uint8_t channel = g_model.frsky.voltsSource;
 237       if (channel <= FRSKY_VOLTS_SOURCE_A2) {
 238         voltage = applyChannelRatio(channel, telemetryData.analog[channel].value) / 10;
 239       }
 240
 241 #if defined(FRSKY_HUB)
 242       else if (channel == FRSKY_VOLTS_SOURCE_FAS) {
 243         voltage = telemetryData.hub.vfas;
 244       }
 245 #endif
 246
 247 #if defined(FRSKY_HUB)
 248       uint16_t current = telemetryData.hub.current; /* unit: 1/10 amps */
 249 #else
 250       uint16_t current = 0;
 251 #endif
 252
 253       channel = g_model.frsky.currentSource - FRSKY_CURRENT_SOURCE_A1;
 254       if (channel < MAX_FRSKY_A_CHANNELS) {
 255         current = applyChannelRatio(channel, telemetryData.analog[channel].value) / 10;
 256       }
 257
 258       telemetryData.hub.power = ((current>>1) * (voltage>>1)) / 25;
 259
 260       telemetryData.hub.currentPrescale += current;
 261       if (telemetryData.hub.currentPrescale >= 3600) {
 262         telemetryData.hub.currentConsumption += 1;
 263         telemetryData.hub.currentPrescale -= 3600;
 264       }
 265 #endif
 266     }
 267
 268 #if !defined(CPUARM)
 269     if (telemetryData.hub.power > telemetryData.hub.maxPower) {
 270       telemetryData.hub.maxPower = telemetryData.hub.power;
 271     }
 272 #endif
 273   }
 274
 275 #if defined(WS_HOW_HIGH)
 276   if (wshhStreaming > 0) {
 277     wshhStreaming--;
 278   }
 279 #endif
 280
 281   if (telemetryStreaming > 0) {
 282     telemetryStreaming--;
 283   }
 284   else {
 285 #if !defined(SIMU)
 286 #if defined(CPUARM)
 287     telemetryData.rssi.reset();
 288 #else
 289     telemetryData.rssi[0].set(0);
 290     telemetryData.rssi[1].set(0);
 291 #endif
 292 #endif
 293   }
 294 }
 295
 296 void telemetryReset()
 297 {
 298   memclear(&telemetryData, sizeof(telemetryData));
 299
 300 #if defined(CPUARM)
 301   for (int index=0; index<MAX_TELEMETRY_SENSORS; index++) {
 302     telemetryItems[index].clear();
 303   }
 304 #endif
 305
 306   telemetryStreaming = 0; // reset counter only if valid frsky packets are being detected
 307   link_counter = 0;
 308
 309 #if defined(CPUARM)
 310   telemetryState = TELEMETRY_INIT;
 311 #endif
 312
 313 #if defined(FRSKY_HUB) && !defined(CPUARM)
 314   telemetryData.hub.gpsLatitude_bp = 2;
 315   telemetryData.hub.gpsLongitude_bp = 2;
 316   telemetryData.hub.gpsFix = -1;
 317 #endif
 318
 319 #if defined(SIMU) && !defined(CPUARM)
 320   telemetryData.rssi[0].value = 75;
 321   telemetryData.rssi[1].value = 75;
 322   telemetryData.analog[TELEM_ANA_A1].set(120, UNIT_VOLTS);
 323   telemetryData.analog[TELEM_ANA_A2].set(240, UNIT_VOLTS);
 324
 325   telemetryData.hub.fuelLevel = 75;
 326   telemetryData.hub.rpm = 12000;
 327   telemetryData.hub.vfas = 100;
 328   telemetryData.hub.minVfas = 90;
 329
 330 #if defined(GPS)
 331   telemetryData.hub.gpsFix = 1;
 332   telemetryData.hub.gpsLatitude_bp = 4401;
 333   telemetryData.hub.gpsLatitude_ap = 7710;
 334   telemetryData.hub.gpsLongitude_bp = 1006;
 335   telemetryData.hub.gpsLongitude_ap = 8872;
 336   telemetryData.hub.gpsSpeed_bp = 200;  //in knots
 337   telemetryData.hub.gpsSpeed_ap = 0;
 338   getGpsPilotPosition();
 339
 340   telemetryData.hub.gpsLatitude_bp = 4401;
 341   telemetryData.hub.gpsLatitude_ap = 7455;
 342   telemetryData.hub.gpsLongitude_bp = 1006;
 343   telemetryData.hub.gpsLongitude_ap = 9533;
 344   getGpsDistance();
 345 #endif
 346
 347   telemetryData.hub.airSpeed = 1000; // 185.1 km/h
 348
 349   telemetryData.hub.cellsCount = 6;
 350   telemetryData.hub.cellVolts[0] = 410/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 351   telemetryData.hub.cellVolts[1] = 420/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 352   telemetryData.hub.cellVolts[2] = 430/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 353   telemetryData.hub.cellVolts[3] = 440/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 354   telemetryData.hub.cellVolts[4] = 450/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 355   telemetryData.hub.cellVolts[5] = 460/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 356   telemetryData.hub.minCellVolts = 250/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
 357   telemetryData.hub.minCell = 300;    //unit 10mV
 358   telemetryData.hub.minCells = 220;  //unit 100mV
 359   //telemetryData.hub.cellsSum = 261;    //calculated from cellVolts[]
 360
 361   telemetryData.hub.gpsAltitude_bp = 50;
 362   telemetryData.hub.baroAltitude_bp = 50;
 363   telemetryData.hub.minAltitude = 10;
 364   telemetryData.hub.maxAltitude = 500;
 365
 366   telemetryData.hub.accelY = 100;
 367   telemetryData.hub.temperature1 = -30;
 368   telemetryData.hub.maxTemperature1 = 100;
 369
 370   telemetryData.hub.current = 55;
 371   telemetryData.hub.maxCurrent = 65;
 372 #endif
 373 }
 374
 375 #if defined(CPUARM)
 376 // we don't reset the telemetry here as we would also reset the consumption after model load
 377 void telemetryInit(uint8_t protocol)
 378 {
 379   telemetryProtocol = protocol;
 380
 381   if (protocol == PROTOCOL_FRSKY_D) {
 382     telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
 383   }
 384
 385 #if defined(MULTIMODULE)
 386   else if (protocol == PROTOCOL_MULTIMODULE || protocol == PROTOCOL_FLYSKY_IBUS) {
 387     // The DIY Multi module always speaks 100000 baud regardless of the telemetry protocol in use
 388     telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2);
 389 #if defined(LUA)
 390     outputTelemetryBufferSize = 0;
 391     outputTelemetryBufferTrigger = 0x7E;
 392 #endif
 393   }
 394   else if (protocol == PROTOCOL_SPEKTRUM) {
 395     // Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same since there is no real standard
 396     telemetryPortInit(125000, TELEMETRY_SERIAL_DEFAULT);
 397   }
 398 #endif
 399
 400 #if defined(CROSSFIRE)
 401   else if (protocol == PROTOCOL_PULSES_CROSSFIRE) {
 402     telemetryPortInit(CROSSFIRE_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
 403 #if defined(LUA)
 404     outputTelemetryBufferSize = 0;
 405     outputTelemetryBufferTrigger = 0;
 406 #endif
 407     telemetryPortSetDirectionOutput();
 408   }
 409 #endif
 410
 411 #if defined(SERIAL2) || defined(PCBSKY9X)
 412   else if (protocol == PROTOCOL_FRSKY_D_SECONDARY) {
 413     telemetryPortInit(0, TELEMETRY_SERIAL_DEFAULT);
 414     serial2TelemetryInit(PROTOCOL_FRSKY_D_SECONDARY);
 415   }
 416 #endif
 417
 418   else {
 419     telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
 420 #if defined(LUA)
 421     outputTelemetryBufferSize = 0;
 422     outputTelemetryBufferTrigger = 0x7E;
 423 #endif
 424   }
 425
 426 #if defined(REVX) && !defined(SIMU)
 427   if (serialInversion) {
 428     setMFP();
 429   }
 430   else {
 431     clearMFP();
 432   }
 433 #endif
 434 }
 435 #else
 436 void telemetryInit()
 437 {
 438   telemetryPortInit();
 439 }
 440 #endif
 441
 442 #if !defined(CPUARM)
 443 NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
 444 {
 445   return (45 - 3*alarm + g_model.frsky.rssiAlarms[alarm].value);
 446 }
 447 #endif
 448
 449 #if defined(LOG_TELEMETRY) && !defined(SIMU)
 450 extern FIL g_telemetryFile;
 451 void logTelemetryWriteStart()
 452 {
 453   static tmr10ms_t lastTime = 0;
 454   tmr10ms_t newTime = get_tmr10ms();
 455   if (lastTime != newTime) {
 456     struct gtm utm;
 457     gettime(&utm);
 458     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);
 459     lastTime = newTime;
 460   }
 461 }
 462
 463 void logTelemetryWriteByte(uint8_t data)
 464 {
 465   f_printf(&g_telemetryFile, " %02X", data);
 466 }
 467 #endif
 468
 469 uint8_t outputTelemetryBuffer[TELEMETRY_OUTPUT_FIFO_SIZE] __DMA;
 470 uint8_t outputTelemetryBufferSize = 0;
 471 uint8_t outputTelemetryBufferTrigger = 0;
 472
 473 #if defined(LUA)
 474 Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
 475 #endif