companion/src/simulation/telemetrysimu.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 "telemetrysimu.h"
  22 #include "ui_telemetrysimu.h"
  23 #include "appdata.h"
  24 #include "simulatorinterface.h"
  25 #include "radio/src/telemetry/frsky.h"
  26
  27 #include <QRegularExpression>
  28 #include <stdint.h>
  29
  30 TelemetrySimulator::TelemetrySimulator(QWidget * parent, SimulatorInterface * simulator):
  31   QWidget(parent),
  32   ui(new Ui::TelemetrySimulator),
  33   simulator(simulator),
  34   m_simuStarted(false),
  35   m_telemEnable(false),
  36   m_logReplayEnable(false)
  37 {
  38   ui->setupUi(this);
  39
  40   ui->A1->setSpecialValueText(" ");
  41   ui->A2->setSpecialValueText(" ");
  42   ui->A3->setSpecialValueText(" ");
  43   ui->A4->setSpecialValueText(" ");
  44   ui->rpm->setSpecialValueText(" ");
  45   ui->fuel->setSpecialValueText(" ");
  46
  47   ui->rxbt_ratio->setEnabled(false);
  48   ui->A1_ratio->setEnabled(false);
  49   ui->A2_ratio->setEnabled(false);
  50
  51
  52   timer.setInterval(10);
  53   connect(&timer, &QTimer::timeout, this, &TelemetrySimulator::generateTelemetryFrame);
  54
  55   connect(&logTimer, &QTimer::timeout, this, &TelemetrySimulator::onLogTimerEvent);
  56
  57   logPlayback = new LogPlaybackController(ui);
  58
  59   connect(ui->Simulate, SIGNAL(toggled(bool)), this, SLOT(onSimulateToggled(bool)));
  60   connect(ui->loadLogFile, SIGNAL(released()), this, SLOT(onLoadLogFile()));
  61   connect(ui->play, SIGNAL(released()), this, SLOT(onPlay()));
  62   connect(ui->rewind, SIGNAL(clicked()), this, SLOT(onRewind()));
  63   connect(ui->stepForward, SIGNAL(clicked()), this, SLOT(onStepForward()));
  64   connect(ui->stepBack, SIGNAL(clicked()), this, SLOT(onStepBack()));
  65   connect(ui->stop, SIGNAL(clicked()), this, SLOT(onStop()));
  66   connect(ui->positionIndicator, SIGNAL(valueChanged(int)), this, SLOT(onPositionIndicatorChanged(int)));
  67   connect(ui->replayRate, SIGNAL(valueChanged(int)), this, SLOT(onReplayRateChanged(int)));
  68
  69   connect(this, &TelemetrySimulator::telemetryDataChanged, simulator, &SimulatorInterface::sendTelemetry);
  70   connect(simulator, &SimulatorInterface::started, this, &TelemetrySimulator::onSimulatorStarted);
  71   connect(simulator, &SimulatorInterface::stopped, this, &TelemetrySimulator::onSimulatorStopped);
  72 }
  73
  74 TelemetrySimulator::~TelemetrySimulator()
  75 {
  76   timer.stop();
  77   logTimer.stop();
  78   delete logPlayback;
  79   delete ui;
  80 }
  81
  82 void TelemetrySimulator::onSimulatorStarted()
  83 {
  84   m_simuStarted = true;
  85   setupDataFields();
  86 }
  87
  88 void TelemetrySimulator::onSimulatorStopped()
  89 {
  90   m_simuStarted = false;
  91 }
  92
  93 void TelemetrySimulator::onSimulateToggled(bool isChecked)
  94 {
  95   if (isChecked) {
  96     timer.start();
  97   }
  98   else {
  99     timer.stop();
 100   }
 101 }
 102
 103 void TelemetrySimulator::onLogTimerEvent()
 104 {
 105   logPlayback->stepForward(false);
 106 }
 107
 108 void TelemetrySimulator::onLoadLogFile()
 109 {
 110   onStop(); // in case we are in playback mode
 111   logPlayback->loadLogFile();
 112 }
 113
 114 void TelemetrySimulator::onPlay()
 115 {
 116   if (logPlayback->isReady()) {
 117     logTimer.start(logPlayback->logFrequency * 1000 / SPEEDS[ui->replayRate->value()]);
 118     logPlayback->play();
 119   }
 120 }
 121
 122 void TelemetrySimulator::onRewind()
 123 {
 124   if (logPlayback->isReady()) {
 125     logTimer.stop();
 126     logPlayback->rewind();
 127   }
 128 }
 129
 130 void TelemetrySimulator::onStepForward()
 131 {
 132   if (logPlayback->isReady()) {
 133     logTimer.stop();
 134     logPlayback->stepForward(true);
 135   }
 136 }
 137
 138 void TelemetrySimulator::onStepBack()
 139 {
 140   if (logPlayback->isReady()) {
 141     logTimer.stop();
 142     logPlayback->stepBack();
 143   }
 144 }
 145
 146 void TelemetrySimulator::onStop()
 147 {
 148   if (logPlayback->isReady()) {
 149     logTimer.stop();
 150     logPlayback->stop();
 151   }
 152 }
 153
 154 void TelemetrySimulator::onPositionIndicatorChanged(int value)
 155 {
 156   if (logPlayback->isReady()) {
 157     logPlayback->updatePositionLabel(value);
 158     logPlayback->setUiDataValues();
 159   }
 160 }
 161
 162 void TelemetrySimulator::onReplayRateChanged(int value)
 163 {
 164   if (logTimer.isActive()) {
 165     logTimer.setInterval(logPlayback->logFrequency * 1000 / SPEEDS[ui->replayRate->value()]);
 166   }
 167 }
 168
 169 void TelemetrySimulator::hideEvent(QHideEvent *event)
 170 {
 171   m_telemEnable = ui->Simulate->isChecked();
 172   m_logReplayEnable = logTimer.isActive();
 173
 174   ui->Simulate->setChecked(false);
 175   ui->stop->click();
 176   event->accept();
 177 }
 178
 179 void TelemetrySimulator::showEvent(QShowEvent * event)
 180 {
 181   ui->Simulate->setChecked(m_telemEnable);
 182   if (m_logReplayEnable)
 183     ui->play->click();
 184 }
 185
 186 #define SET_INSTANCE(control, id, def)  ui->control->setText(QString::number(simulator->getSensorInstance(id, ((def) & 0x1F) + 1)))
 187
 188 void TelemetrySimulator::setupDataFields()
 189 {
 190   SET_INSTANCE(rxbt_inst,     BATT_ID,                0);
 191   SET_INSTANCE(rssi_inst,     RSSI_ID,                24);
 192   SET_INSTANCE(swr_inst,      RAS_ID,                 24);
 193   SET_INSTANCE(a1_inst,       ADC1_ID,                0);
 194   SET_INSTANCE(a2_inst,       ADC2_ID,                0);
 195   SET_INSTANCE(a3_inst,       A3_FIRST_ID,            0);
 196   SET_INSTANCE(a4_inst,       A4_FIRST_ID,            0);
 197   SET_INSTANCE(t1_inst,       T1_FIRST_ID,            0);
 198   SET_INSTANCE(t2_inst,       T2_FIRST_ID,            0);
 199   SET_INSTANCE(rpm_inst,      RPM_FIRST_ID,           DATA_ID_RPM);
 200   SET_INSTANCE(fuel_inst,     FUEL_FIRST_ID,          0);
 201   SET_INSTANCE(fuel_qty_inst, FUEL_QTY_FIRST_ID,      0);
 202   SET_INSTANCE(aspd_inst,     AIR_SPEED_FIRST_ID,     0);
 203   SET_INSTANCE(vvspd_inst,    VARIO_FIRST_ID,         DATA_ID_VARIO);
 204   SET_INSTANCE(valt_inst,     ALT_FIRST_ID,           DATA_ID_VARIO);
 205   SET_INSTANCE(fasv_inst,     VFAS_FIRST_ID,          DATA_ID_FAS);
 206   SET_INSTANCE(fasc_inst,     CURR_FIRST_ID,          DATA_ID_FAS);
 207   SET_INSTANCE(cells_inst,    CELLS_FIRST_ID,         DATA_ID_FLVSS);
 208   SET_INSTANCE(gpsa_inst,     GPS_ALT_FIRST_ID,       DATA_ID_GPS);
 209   SET_INSTANCE(gpss_inst,     GPS_SPEED_FIRST_ID,     DATA_ID_GPS);
 210   SET_INSTANCE(gpsc_inst,     GPS_COURS_FIRST_ID,     DATA_ID_GPS);
 211   SET_INSTANCE(gpst_inst,     GPS_TIME_DATE_FIRST_ID, DATA_ID_GPS);
 212   SET_INSTANCE(gpsll_inst,    GPS_LONG_LATI_FIRST_ID, DATA_ID_GPS);
 213   SET_INSTANCE(accx_inst,     ACCX_FIRST_ID,          0);
 214   SET_INSTANCE(accy_inst,     ACCY_FIRST_ID,          0);
 215   SET_INSTANCE(accz_inst,     ACCZ_FIRST_ID,          0);
 216
 217   refreshSensorRatios();
 218 }
 219
 220 void setSportPacketCrc(uint8_t * packet)
 221 {
 222   short crc = 0;
 223   for (int i=1; i<FRSKY_SPORT_PACKET_SIZE-1; i++) {
 224     crc += packet[i]; //0-1FF
 225     crc += crc >> 8; //0-100
 226     crc &= 0x00ff;
 227     crc += crc >> 8; //0-0FF
 228     crc &= 0x00ff;
 229   }
 230   packet[FRSKY_SPORT_PACKET_SIZE-1] = 0xFF - (crc & 0x00ff);
 231 }
 232
 233 uint8_t getBit(uint8_t position, uint8_t value)
 234 {
 235   return (value & (uint8_t)(1 << position)) ? 1 : 0;
 236 }
 237
 238 bool generateSportPacket(uint8_t * packet, uint8_t dataId, uint8_t prim, uint16_t appId, uint32_t data)
 239 {
 240   if (dataId > 0x1B ) return false;
 241
 242   // generate Data ID field
 243   uint8_t bit5 = getBit(0, dataId) ^ getBit(1, dataId) ^ getBit(2, dataId);
 244   uint8_t bit6 = getBit(2, dataId) ^ getBit(3, dataId) ^ getBit(4, dataId);
 245   uint8_t bit7 = getBit(0, dataId) ^ getBit(2, dataId) ^ getBit(4, dataId);
 246
 247   packet[0] = (bit7 << 7) + (bit6 << 6) + (bit5 << 5) + dataId;
 248   // qDebug("dataID: 0x%02x (%d)", packet[0], dataId);
 249   packet[1] = prim;
 250   *((uint16_t *)(packet+2)) = appId;
 251   *((int32_t *)(packet+4)) = data;
 252   setSportPacketCrc(packet);
 253   return true;
 254 }
 255
 256 void TelemetrySimulator::refreshSensorRatios()
 257 {
 258   ui->rxbt_ratio->setValue(simulator->getSensorRatio(BATT_ID) / 10.0);
 259   ui->A1_ratio->setValue(simulator->getSensorRatio(ADC1_ID) / 10.0);
 260   ui->A2_ratio->setValue(simulator->getSensorRatio(ADC2_ID) / 10.0);
 261 }
 262
 263 void TelemetrySimulator::generateTelemetryFrame()
 264 {
 265   static int item = 0;
 266   bool ok = true;
 267   uint8_t buffer[FRSKY_SPORT_PACKET_SIZE] = {0};
 268   static FlvssEmulator *flvss = new FlvssEmulator();
 269   static GPSEmulator *gps = new GPSEmulator();
 270
 271   if (!m_simuStarted)
 272     return;
 273
 274   switch (item++) {
 275     case 0:
 276 #if defined(XJT_VERSION_ID)
 277       generateSportPacket(buffer, 1, DATA_FRAME, XJT_VERSION_ID, 11);
 278 #endif
 279       refreshSensorRatios();    // placed here in order to call this less often
 280     break;
 281
 282     case 1:
 283       if (ui->rxbt->text().length()) {
 284         generateSportPacket(buffer, ui->rxbt_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, BATT_ID, LIMIT<uint32_t>(0, ui->rxbt->value() * 255.0 / ui->rxbt_ratio->value(), 0xFFFFFFFF));
 285       }
 286       break;
 287
 288     case 2:
 289       if (ui->Rssi->text().length())
 290         generateSportPacket(buffer, ui->rssi_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, RSSI_ID, LIMIT<uint32_t>(0, ui->Rssi->text().toInt(&ok, 0), 0xFF));
 291       break;
 292
 293     case 3:
 294       if (ui->Swr->text().length())
 295         generateSportPacket(buffer, ui->swr_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, RAS_ID, LIMIT<uint32_t>(0, ui->Swr->text().toInt(&ok, 0), 0xFFFF));
 296       break;
 297
 298     case 4:
 299       if (ui->A1->value() > 0)
 300         generateSportPacket(buffer, ui->a1_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, ADC1_ID, LIMIT<uint32_t>(0, ui->A1->value() * 255.0 / ui->A1_ratio->value(), 0xFF));
 301       break;
 302
 303     case 5:
 304       if (ui->A2->value() > 0)
 305         generateSportPacket(buffer, ui->a2_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, ADC2_ID, LIMIT<uint32_t>(0, ui->A2->value() * 255.0 / ui->A2_ratio->value(), 0xFF));
 306       break;
 307
 308     case 6:
 309       if (ui->A3->value() != 0)
 310         generateSportPacket(buffer, ui->a3_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, A3_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->A3->value() * 100.0, 0x7FFFFFFF));
 311       break;
 312
 313     case 7:
 314       if (ui->A4->value() != 0)
 315         generateSportPacket(buffer, ui->a4_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, A4_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->A4->value() * 100.0, 0x7FFFFFFF));
 316       break;
 317
 318     case 8:
 319       if (ui->T1->value() != 0)
 320         generateSportPacket(buffer, ui->t1_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, T1_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->T1->value(), 0x7FFFFFFF));
 321       break;
 322
 323     case 9:
 324       if (ui->T2->value() != 0)
 325         generateSportPacket(buffer, ui->t2_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, T2_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->T2->value(), 0x7FFFFFFF));
 326       break;
 327
 328     case 10:
 329       if (ui->rpm->value() > 0)
 330         generateSportPacket(buffer, ui->rpm_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, RPM_FIRST_ID, LIMIT<uint32_t>(0, ui->rpm->value(), 0x7FFFFFFF));
 331       break;
 332
 333     case 11:
 334       if (ui->fuel->value() > 0)
 335         generateSportPacket(buffer, ui->fuel_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, FUEL_FIRST_ID, LIMIT<uint32_t>(0, ui->fuel->value(), 0xFFFF));
 336       break;
 337
 338     case 12:
 339       if (ui->vspeed->value() != 0)
 340         generateSportPacket(buffer, ui->vvspd_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, VARIO_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->vspeed->value() * 100.0, 0x7FFFFFFF));
 341       break;
 342
 343     case 13:
 344       if (ui->valt->value() != 0)
 345         generateSportPacket(buffer, ui->valt_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, ALT_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->valt->value() * 100.0, 0x7FFFFFFF));
 346       break;
 347
 348     case 14:
 349       if (ui->vfas->value() != 0)
 350         generateSportPacket(buffer, ui->fasv_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, VFAS_FIRST_ID, LIMIT<uint32_t>(0, ui->vfas->value() * 100.0, 0xFFFFFFFF));
 351       break;
 352
 353     case 15:
 354       if (ui->curr->value() != 0)
 355         generateSportPacket(buffer, ui->fasc_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, CURR_FIRST_ID, LIMIT<uint32_t>(0, ui->curr->value() * 10.0, 0xFFFFFFFF));
 356       break;
 357
 358     case 16:
 359       double cellValues[FlvssEmulator::MAXCELLS];
 360       if (ui->cell1->value() > 0.009) { // ??? cell1 returning non-zero value when spin box is zero!
 361         cellValues[0] = ui->cell1->value();
 362         cellValues[1] = ui->cell2->value();
 363         cellValues[2] = ui->cell3->value();
 364         cellValues[3] = ui->cell4->value();
 365         cellValues[4] = ui->cell5->value();
 366         cellValues[5] = ui->cell6->value();
 367         generateSportPacket(buffer, ui->cells_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, CELLS_FIRST_ID, flvss->setAllCells_GetNextPair(cellValues));
 368       }
 369       else {
 370         cellValues[0] = 0;
 371         flvss->setAllCells_GetNextPair(cellValues);
 372       }
 373       break;
 374
 375     case 17:
 376       if (ui->aspeed->value() > 0)
 377         generateSportPacket(buffer, ui->aspd_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, AIR_SPEED_FIRST_ID, LIMIT<uint32_t>(0, ui->aspeed->value() * 5.39957, 0xFFFFFFFF));
 378       break;
 379
 380     case 18:
 381       if (ui->gps_alt->value() != 0) {
 382         gps->setGPSAltitude(ui->gps_alt->value());
 383         generateSportPacket(buffer, ui->gpsa_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, GPS_ALT_FIRST_ID, gps->getNextPacketData(GPS_ALT_FIRST_ID));
 384       }
 385       break;
 386
 387     case 19:
 388       if (ui->gps_speed->value() > 0) {
 389         gps->setGPSSpeedKMH(ui->gps_speed->value());
 390         generateSportPacket(buffer, ui->gpss_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, GPS_SPEED_FIRST_ID, gps->getNextPacketData(GPS_SPEED_FIRST_ID));
 391       }
 392       break;
 393
 394     case 20:
 395       if (ui->gps_course->value() != 0) {
 396         gps->setGPSCourse(ui->gps_course->value());
 397         generateSportPacket(buffer, ui->gpsc_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, GPS_COURS_FIRST_ID, gps->getNextPacketData(GPS_COURS_FIRST_ID));
 398       }
 399       break;
 400
 401     case 21:
 402       if (ui->gps_time->text().length()) {
 403         gps->setGPSDateTime(ui->gps_time->text());
 404         generateSportPacket(buffer, ui->gpst_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, GPS_TIME_DATE_FIRST_ID, gps->getNextPacketData(GPS_TIME_DATE_FIRST_ID));
 405       }
 406       break;
 407
 408     case 22:
 409       if (ui->gps_latlon->text().length()) {
 410         gps->setGPSLatLon(ui->gps_latlon->text());
 411         generateSportPacket(buffer, ui->gpsll_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, GPS_LONG_LATI_FIRST_ID, gps->getNextPacketData(GPS_LONG_LATI_FIRST_ID));
 412       }
 413       break;
 414
 415     case 23:
 416         if (ui->accx->value() != 0)
 417           generateSportPacket(buffer, ui->accx_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, ACCX_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->accx->value() * 100.0, 0x7FFFFFFF));
 418         break;
 419
 420     case 24:
 421       if (ui->accy->value() != 0)
 422         generateSportPacket(buffer, ui->accy_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, ACCY_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->accy->value() * 100.0, 0x7FFFFFFF));
 423       break;
 424
 425     case 25:
 426       if (ui->accz->value() != 0)
 427         generateSportPacket(buffer, ui->accz_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, ACCZ_FIRST_ID, LIMIT<int32_t>(-0x7FFFFFFF, ui->accz->value() * 100.0, 0x7FFFFFFF));
 428       break;
 429
 430     case 26:
 431       if (ui->fuel_qty->value() > 0)
 432         generateSportPacket(buffer, ui->fuel_qty_inst->text().toInt(&ok, 0) - 1, DATA_FRAME, FUEL_QTY_FIRST_ID, LIMIT<uint32_t>(0, ui->fuel_qty->value() * 100.0, 0xFFFFFF));
 433       break;
 434
 435     default:
 436       item = 0;
 437       return;
 438   }
 439
 440   if (ok && (buffer[2] || buffer[3])) {
 441     QByteArray ba((char *)buffer, FRSKY_SPORT_PACKET_SIZE);
 442     emit telemetryDataChanged(ba);
 443     //qDebug("%02X %02X %02X %02X %02X %02X %02X %02X %02X", buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], buffer[8]);
 444   }
 445   else {
 446     generateTelemetryFrame();
 447   }
 448 }
 449
 450 uint32_t TelemetrySimulator::FlvssEmulator::encodeCellPair(uint8_t cellNum, uint8_t firstCellNo, double cell1, double cell2)
 451 {
 452   uint16_t cell1Data = cell1 * 500.0;
 453   uint16_t cell2Data = cell2 * 500.0;
 454   uint32_t cellData = 0;
 455
 456   cellData = cell2Data & 0x0FFF;
 457   cellData <<= 12;
 458   cellData |= cell1Data & 0x0FFF;
 459   cellData <<= 4;
 460   cellData |= cellNum & 0x0F;
 461   cellData <<= 4;
 462   cellData |= firstCellNo & 0x0F;
 463
 464   return cellData;
 465 }
 466
 467 void TelemetrySimulator::FlvssEmulator::encodeAllCells()
 468 {
 469   cellData1 = encodeCellPair(numCells, 0, cellFloats[0], cellFloats[1]);
 470   if (numCells > 2) cellData2 = encodeCellPair(numCells, 2, cellFloats[2], cellFloats[3]); else cellData2 = 0;
 471   if (numCells > 4) cellData3 = encodeCellPair(numCells, 4, cellFloats[4], cellFloats[5]); else cellData3 = 0;
 472 }
 473
 474 void TelemetrySimulator::FlvssEmulator::splitIntoCells(double totalVolts)
 475 {
 476   numCells = qFloor((totalVolts / 3.7) + .5);
 477   double avgVolts = totalVolts / numCells;
 478   double remainder = (totalVolts - (avgVolts * numCells));
 479     for (uint32_t i = 0; (i < numCells) && ( i < MAXCELLS); i++) {
 480     cellFloats[i] = avgVolts;
 481   }
 482   for (uint32_t i = numCells; i < MAXCELLS; i++) {
 483     cellFloats[i] = 0;
 484   }
 485   cellFloats[0] += remainder;
 486   numCells = numCells > MAXCELLS ? MAXCELLS : numCells; // force into valid cell count in case of input out of range
 487 }
 488
 489 uint32_t TelemetrySimulator::FlvssEmulator::setAllCells_GetNextPair(double cellValues[6])
 490 {
 491   numCells = 0;
 492   for (uint32_t i = 0; i < MAXCELLS; i++) {
 493     if ((i == 0) && (cellValues[0] > 4.2)) {
 494       splitIntoCells(cellValues[0]);
 495       break;
 496     }
 497     if (cellValues[i] > 0) {
 498       cellFloats[i] = cellValues[i];
 499       numCells++;
 500     }
 501     else {
 502       // zero marks the last cell
 503       for (uint32_t x = i; x < MAXCELLS; x++) {
 504         cellFloats[x] = 0;
 505       }
 506       break;
 507     }
 508   }
 509
 510   // encode the double values into telemetry format
 511   encodeAllCells();
 512
 513   // return the value for the current pair
 514   uint32_t cellData = 0;
 515   if (nextCellNum >= numCells) {
 516     nextCellNum = 0;
 517   }
 518   switch (nextCellNum) {
 519   case 0:
 520     cellData = cellData1;
 521     break;
 522   case 2:
 523     cellData = cellData2;
 524     break;
 525   case 4:
 526     cellData = cellData3;
 527     break;
 528   }
 529   nextCellNum += 2;
 530   return cellData;
 531 }
 532
 533 TelemetrySimulator::GPSEmulator::GPSEmulator()
 534 {
 535   lat = 0;
 536   lon = 0;
 537   dt = QDateTime::currentDateTime();
 538   sendLat = true;
 539   sendDate = true;
 540 }
 541
 542
 543 uint32_t TelemetrySimulator::GPSEmulator::encodeLatLon(double latLon, bool isLat)
 544 {
 545   uint32_t data = (uint32_t)((latLon < 0 ? -latLon : latLon) * 60 * 10000) & 0x3FFFFFFF;
 546   if (isLat == false) {
 547     data |= 0x80000000;
 548   }
 549   if (latLon < 0) {
 550     data |= 0x40000000;
 551   }
 552   return data;
 553 }
 554
 555 uint32_t TelemetrySimulator::GPSEmulator::encodeDateTime(uint8_t yearOrHour, uint8_t monthOrMinute, uint8_t dayOrSecond, bool isDate)
 556 {
 557   uint32_t data = yearOrHour;
 558   data <<= 8;
 559   data |= monthOrMinute;
 560   data <<= 8;
 561   data |= dayOrSecond;
 562   data <<= 8;
 563   if (isDate == true) {
 564     data |= 0xFF;
 565   }
 566   return data;
 567 }
 568
 569 uint32_t TelemetrySimulator::GPSEmulator::getNextPacketData(uint32_t packetType)
 570 {
 571   switch (packetType) {
 572   case GPS_LONG_LATI_FIRST_ID:
 573     sendLat = !sendLat;
 574     return sendLat ? encodeLatLon(lat, true) : encodeLatLon(lon, false);
 575     break;
 576   case GPS_TIME_DATE_FIRST_ID:
 577     sendDate = !sendDate;
 578     return sendDate ? encodeDateTime(dt.date().year() - 2000, dt.date().month(), dt.date().day(), true) : encodeDateTime(dt.time().hour(), dt.time().minute(), dt.time().second(), false);
 579     break;
 580   case GPS_ALT_FIRST_ID:
 581     return (uint32_t) (altitude * 100);
 582     break;
 583   case GPS_SPEED_FIRST_ID:
 584     return speedKNTS * 1000;
 585     break;
 586   case GPS_COURS_FIRST_ID:
 587     return course * 100;
 588     break;
 589   }
 590   return 0;
 591 }
 592
 593 void TelemetrySimulator::GPSEmulator::setGPSDateTime(QString dateTime)
 594 {
 595   dt = QDateTime::currentDateTime().toTimeSpec(Qt::UTC); // default to current systemtime
 596   if (!dateTime.startsWith('*')) {
 597     QString format("dd-MM-yyyy hh:mm:ss");
 598     dt = QDateTime::fromString(dateTime, format);
 599   }
 600 }
 601
 602 void TelemetrySimulator::GPSEmulator::setGPSLatLon(QString latLon)
 603 {
 604   QStringList coords = latLon.split(",");
 605   lat = 0.0;
 606   lon = 0.0;
 607   if (coords.length() > 1)
 608   {
 609     lat = coords[0].toDouble();
 610     lon = coords[1].toDouble();
 611   }
 612 }
 613
 614 void TelemetrySimulator::GPSEmulator::setGPSCourse(double course)
 615 {
 616   this->course = course;
 617 }
 618
 619 void TelemetrySimulator::GPSEmulator::setGPSSpeedKMH(double speedKMH)
 620 {
 621   this->speedKNTS = speedKMH * 0.539957;
 622 }
 623
 624 void TelemetrySimulator::GPSEmulator::setGPSAltitude(double altitude)
 625 {
 626   this->altitude = altitude;
 627 }
 628
 629 TelemetrySimulator::LogPlaybackController::LogPlaybackController(Ui::TelemetrySimulator * ui)
 630 {
 631   TelemetrySimulator::LogPlaybackController::ui = ui;
 632   stepping = false;
 633   logFileGpsCordsInDecimalFormat = false;
 634   // initialize the map - TODO: how should this be localized?
 635   colToFuncMap.clear();
 636   colToFuncMap.insert("RxBt(V)", RXBT_V);
 637   colToFuncMap.insert("RSSI(dB)", RSSI);
 638   colToFuncMap.insert("RAS", RAS);
 639   colToFuncMap.insert("A1", A1);
 640   colToFuncMap.insert("A1(V)", A1);
 641   colToFuncMap.insert("A2", A2);
 642   colToFuncMap.insert("A2(V)", A2);
 643   colToFuncMap.insert("A3", A3);
 644   colToFuncMap.insert("A3(V)", A3);
 645   colToFuncMap.insert("A4", A4);
 646   colToFuncMap.insert("A4(V)", A4);
 647   colToFuncMap.insert("Tmp1(@C)", T1_DEGC);
 648   colToFuncMap.insert("Tmp1(@F)", T1_DEGF);
 649   colToFuncMap.insert("Tmp2(@C)", T2_DEGC);
 650   colToFuncMap.insert("Tmp2(@F)", T2_DEGF);
 651   colToFuncMap.insert("RPM(rpm)", RPM);
 652   colToFuncMap.insert("Fuel(%)", FUEL);
 653   colToFuncMap.insert("Fuel(ml)", FUEL_QTY);
 654   colToFuncMap.insert("VSpd(m/s)", VSPD_MS);
 655   colToFuncMap.insert("VSpd(f/s)", VSPD_FS);
 656   colToFuncMap.insert("Alt(ft)", ALT_FEET);
 657   colToFuncMap.insert("Alt(m)", ALT_METERS);
 658   colToFuncMap.insert("VFAS(V)", FASV);
 659   colToFuncMap.insert("Curr(A)", FASC);
 660   colToFuncMap.insert("Cels(gRe)", CELS_GRE);
 661   colToFuncMap.insert("Cels(V)", CELS_GRE);
 662   colToFuncMap.insert("ASpd(kts)", ASPD_KTS);
 663   colToFuncMap.insert("ASpd(kmh)", ASPD_KMH);
 664   colToFuncMap.insert("ASpd(mph)", ASPD_MPH);
 665   colToFuncMap.insert("GAlt(ft)", GALT_FEET);
 666   colToFuncMap.insert("GAlt(m)", GALT_METERS);
 667   colToFuncMap.insert("GSpd(kts)", GSPD_KNTS);
 668   colToFuncMap.insert("GSpd(kmh)", GSPD_KMH);
 669   colToFuncMap.insert("GSpd(mph)", GSPD_MPH);
 670   colToFuncMap.insert("Hdg(@)", GHDG_DEG);
 671   colToFuncMap.insert("Date", GDATE);
 672   colToFuncMap.insert("GPS", G_LATLON);
 673   colToFuncMap.insert("AccX(g)", ACCX);
 674   colToFuncMap.insert("AccY(g)", ACCY);
 675   colToFuncMap.insert("AccZ(g)", ACCZ);
 676
 677   // ACCX Y and Z
 678 }
 679
 680 QDateTime TelemetrySimulator::LogPlaybackController::parseTransmittterTimestamp(QString row)
 681 {
 682   QStringList rowParts = row.simplified().split(',');
 683   if (rowParts.size() < 2) {
 684     return QDateTime();
 685   }
 686   QString datePart = rowParts[0];
 687   QString timePart = rowParts[1];
 688   QDateTime result;
 689   QString format("yyyy-MM-dd hh:mm:ss.zzz"); // assume this format
 690   // hour can be 'missing'
 691   if (timePart.count(":") < 2) {
 692     timePart = "00:" + timePart;
 693   }
 694   if (datePart.contains("/")) { // happens when csv is edited by Excel
 695     format = "M/d/yyyy hh:mm:ss.z";
 696   }
 697   return QDateTime::fromString(datePart + " " + timePart, format);
 698 }
 699
 700 void TelemetrySimulator::LogPlaybackController::checkGpsFormat()
 701 {
 702   // sample the first record to check if cords are in decimal format
 703   logFileGpsCordsInDecimalFormat = false;
 704   if(csvRecords.count() > 1) {
 705     QStringList keys = csvRecords[0].split(',');
 706     if(keys.contains("GPS")) {
 707       int gpsColIndex = keys.indexOf("GPS");
 708       QStringList firstRowVlues = csvRecords[1].split(',');
 709       QString gpsSample = firstRowVlues[gpsColIndex];
 710       QStringList cords = gpsSample.simplified().split(' ');
 711       if (cords.count() == 2) {
 712         // frsky and TBS crossfire GPS sensor logs cords in decimal format with a precision of 6 places
 713         // if this format is met there is no need to call convertDegMin later on when processing the file
 714         QRegularExpression decimalCoordinateFormatRegex("^[-+]?\\d{1,2}[.]\\d{6}$");
 715         QRegularExpressionMatch latFormatMatch = decimalCoordinateFormatRegex.match(cords[0]);
 716         QRegularExpressionMatch lonFormatMatch = decimalCoordinateFormatRegex.match(cords[1]);
 717         if (lonFormatMatch.hasMatch() && latFormatMatch.hasMatch()) {
 718           logFileGpsCordsInDecimalFormat = true;
 719         }
 720       }
 721     }
 722   }
 723 }
 724
 725 void TelemetrySimulator::LogPlaybackController::calcLogFrequency()
 726 {
 727   // examine up to 20 rows to determine log frequency in seconds
 728   // Skip the first entry which contains the file open time
 729   logFrequency = 25.5; // default value
 730   QDateTime lastTime;
 731   for (int i = 2; (i < 21) && (i < csvRecords.count()); i++)
 732   {
 733     QDateTime logTime = parseTransmittterTimestamp(csvRecords[i]);
 734     // ugh - no timespan in this Qt version
 735     double timeDiff = (logTime.toMSecsSinceEpoch() - lastTime.toMSecsSinceEpoch()) / 1000.0;
 736     if ((timeDiff > 0.09) && (timeDiff < logFrequency)) {
 737       logFrequency = timeDiff;
 738     }
 739     lastTime = logTime;
 740   }
 741 }
 742
 743 bool TelemetrySimulator::LogPlaybackController::isReady()
 744 {
 745   return csvRecords.count() > 1;
 746 }
 747
 748 void TelemetrySimulator::LogPlaybackController::loadLogFile()
 749 {
 750   QString logFileNameAndPath = QFileDialog::getOpenFileName(NULL, tr("Log File"), g.logDir(), tr("LOG Files (*.csv)"));
 751   if (logFileNameAndPath.isEmpty())
 752     return;
 753
 754   g.logDir(logFileNameAndPath);
 755
 756   // reset the playback ui
 757   ui->play->setEnabled(false);
 758   ui->rewind->setEnabled(false);
 759   ui->stepBack->setEnabled(false);
 760   ui->stepForward->setEnabled(false);
 761   ui->stop->setEnabled(false);
 762   ui->positionIndicator->setEnabled(false);
 763   ui->replayRate->setEnabled(false);
 764   ui->positionLabel->setText("Row #\nTimestamp");
 765
 766   // clear existing data
 767   csvRecords.clear();
 768
 769   QFile file(logFileNameAndPath);
 770   if (!file.open(QIODevice::ReadOnly)) {
 771     ui->logFileLabel->setText(tr("ERROR - invalid file"));
 772     return;
 773   }
 774   while (!file.atEnd()) {
 775     QByteArray line = file.readLine();
 776     csvRecords.append(line.simplified());
 777   }
 778   file.close();
 779   if (csvRecords.count() > 1) {
 780     columnNames.clear();
 781     QStringList keys = csvRecords[0].split(',');
 782     // override the first two column names
 783     keys[0] = "LogDate";
 784     keys[1] = "LogTime";
 785     Q_FOREACH(QString key, keys) {
 786       columnNames.append(key.simplified());
 787     }
 788     ui->play->setEnabled(true);
 789     ui->rewind->setEnabled(true);
 790     ui->stepBack->setEnabled(true);
 791     ui->stepForward->setEnabled(true);
 792     ui->stop->setEnabled(true);
 793     ui->positionIndicator->setEnabled(true);
 794     ui->replayRate->setEnabled(true);
 795     supportedCols.clear();
 796     recordIndex = 1;
 797     calcLogFrequency();
 798     checkGpsFormat();
 799   }
 800   ui->logFileLabel->setText(QFileInfo(logFileNameAndPath).fileName());
 801   // iterate through all known mappings and add those that are used
 802   QMapIterator<QString, CONVERT_TYPE> it(colToFuncMap);
 803   while (it.hasNext()) {
 804     it.next();
 805     addColumnHash(it.key(), it.value());
 806   }
 807   rewind();
 808   return;
 809 }
 810
 811 void TelemetrySimulator::LogPlaybackController::addColumnHash(QString key, CONVERT_TYPE functionIndex)
 812 {
 813   DATA_TO_FUNC_XREF dfx;
 814   if (columnNames.contains(key)) {
 815     dfx.functionIndex = functionIndex;
 816     dfx.dataIndex = columnNames.indexOf(key);
 817     supportedCols.append(dfx);
 818   }
 819 }
 820
 821 void TelemetrySimulator::LogPlaybackController::play()
 822 {
 823 }
 824
 825 void TelemetrySimulator::LogPlaybackController::stop()
 826 {
 827 }
 828
 829 void TelemetrySimulator::LogPlaybackController::rewind()
 830 {
 831   stepping = true;
 832   recordIndex = 1;
 833   ui->stop->setChecked(true);
 834   updatePositionLabel(-1);
 835   setUiDataValues();
 836   stepping = false;
 837 }
 838
 839 void TelemetrySimulator::LogPlaybackController::stepForward(bool focusOnStop)
 840 {
 841   stepping = true;
 842   if (recordIndex < (csvRecords.count() - 1)) {
 843     recordIndex++;
 844     if (focusOnStop) {
 845       ui->stop->setChecked(true);
 846     }
 847     updatePositionLabel(-1);
 848     setUiDataValues();
 849   }
 850   else {
 851     rewind(); // always loop at the end
 852   }
 853   stepping = false;
 854 }
 855
 856 void TelemetrySimulator::LogPlaybackController::stepBack()
 857 {
 858   stepping = true;
 859   if (recordIndex > 1) {
 860     recordIndex--;
 861     ui->stop->setChecked(true);
 862     updatePositionLabel(-1);
 863     setUiDataValues();
 864   }
 865   stepping = false;
 866 }
 867
 868 double TelemetrySimulator::LogPlaybackController::convertFeetToMeters(QString input)
 869 {
 870   double meters100 = input.toDouble() * 0.3048;
 871   return qFloor(meters100 + .005);
 872 }
 873
 874 QString TelemetrySimulator::LogPlaybackController::convertGPSDate(QString input)
 875 {
 876   QStringList dateTime = input.simplified().split(' ');
 877   if (dateTime.size() < 2) {
 878     return ""; // invalid format
 879   }
 880   QStringList dateParts = dateTime[0].split('-'); // input as yy-mm-dd
 881   if (dateParts.size() < 3) {
 882     return ""; // invalid format
 883   }
 884   // output is dd-MM-yyyy hh:mm:ss
 885   QString localDateString = dateParts[2] + "-" + dateParts[1] + "-20" + dateParts[0] + " " + dateTime[1];
 886   QString format("dd-MM-yyyy hh:mm:ss");
 887   QDateTime utcDate = QDateTime::fromString(localDateString, format).toTimeSpec(Qt::UTC);
 888   return utcDate.toString(format);
 889 }
 890
 891 double TelemetrySimulator::LogPlaybackController::convertDegMin(QString input)
 892 {
 893   double fInput = input.mid(0, input.length() - 1).toDouble();
 894   double degrees = qFloor(fInput / 100.0);
 895   double minutes = fInput - (degrees * 100);
 896   int32_t sign = ((input.endsWith('E')) || (input.endsWith('N'))) ? 1 : -1;
 897   return (degrees + (minutes / 60)) * sign;
 898 }
 899
 900 QString TelemetrySimulator::LogPlaybackController::convertGPS(QString input)
 901 {
 902   // input format is DDmm.mmmmH DDDmm.mmmmH (longitude latitude - degrees (2 places) minutes (2 places) decimal minutes (4 places))
 903   QStringList lonLat = input.simplified().split(' ');
 904   if (lonLat.count() < 2) {
 905     return ""; // invalid format
 906   }
 907   double lon, lat;
 908   if (logFileGpsCordsInDecimalFormat) {
 909     lat = lonLat[0].toDouble();
 910     lon = lonLat[1].toDouble();
 911   }
 912   else {
 913     lon = convertDegMin(lonLat[0]);
 914     lat = convertDegMin(lonLat[1]);
 915   }
 916   return QString::number(lat, 'f', 6) + ", " + QString::number(lon, 'f', 6);
 917 }
 918
 919 void TelemetrySimulator::LogPlaybackController::updatePositionLabel(int32_t percentage)
 920 {
 921   if ((percentage > 0) && (!stepping)) {
 922     recordIndex = qFloor((double)csvRecords.count() / 100.0 * percentage);
 923     if (recordIndex == 0) {
 924       recordIndex = 1; // record 0 is column labels
 925     }
 926   }
 927   // format the transmitter date info
 928   QDateTime transmitterTimestamp = parseTransmittterTimestamp(csvRecords[recordIndex]);
 929   QString format("yyyy-MM-dd hh:mm:ss.z");
 930   ui->positionLabel->setText("Row " + QString::number(recordIndex) + " of " + QString::number(csvRecords.count() - 1)
 931               + "\n" + transmitterTimestamp.toString(format));
 932   if (percentage < 0) { // did we step past a threshold?
 933     uint32_t posPercent = (recordIndex / (double)(csvRecords.count() - 1)) * 100;
 934     ui->positionIndicator->setValue(posPercent);
 935   }
 936 }
 937
 938 double TelemetrySimulator::LogPlaybackController::convertFahrenheitToCelsius(QString input)
 939 {
 940   return (input.toDouble() - 32.0) * 0.5556;
 941 }
 942
 943 void TelemetrySimulator::LogPlaybackController::setUiDataValues()
 944 {
 945   QStringList columnData = csvRecords[recordIndex].split(',');
 946   Q_FOREACH(DATA_TO_FUNC_XREF info, supportedCols) {
 947     if (info.dataIndex < columnData.size()) {
 948       switch (info.functionIndex) {
 949       case RXBT_V:
 950         ui->rxbt->setValue(columnData[info.dataIndex].toDouble());
 951         break;
 952       case RSSI:
 953         ui->Rssi->setValue(columnData[info.dataIndex].toDouble());
 954         break;
 955       case RAS:
 956         ui->Swr->setValue(columnData[info.dataIndex].toDouble());
 957         break;
 958       case A1:
 959         ui->A1->setValue(columnData[info.dataIndex].toDouble());
 960         break;
 961       case A2:
 962         ui->A2->setValue(columnData[info.dataIndex].toDouble());
 963         break;
 964       case A3:
 965         ui->A3->setValue(columnData[info.dataIndex].toDouble());
 966         break;
 967       case A4:
 968         ui->A4->setValue(columnData[info.dataIndex].toDouble());
 969         break;
 970       case T1_DEGC:
 971         ui->T1->setValue(columnData[info.dataIndex].toDouble());
 972         break;
 973       case T2_DEGC:
 974         ui->T2->setValue(columnData[info.dataIndex].toDouble());
 975         break;
 976       case T1_DEGF:
 977         ui->T1->setValue(convertFahrenheitToCelsius(columnData[info.dataIndex]));
 978         break;
 979       case T2_DEGF:
 980         ui->T2->setValue(convertFahrenheitToCelsius(columnData[info.dataIndex]));
 981         break;
 982       case RPM:
 983         ui->rpm->setValue(columnData[info.dataIndex].toDouble());
 984         break;
 985       case FUEL:
 986         ui->fuel->setValue(columnData[info.dataIndex].toDouble());
 987         break;
 988       case FUEL_QTY:
 989         ui->fuel_qty->setValue(columnData[info.dataIndex].toDouble());
 990         break;
 991       case VSPD_MS:
 992         ui->vspeed->setValue(columnData[info.dataIndex].toDouble());
 993         break;
 994       case VSPD_FS:
 995         ui->vspeed->setValue(columnData[info.dataIndex].toDouble() * 0.3048);
 996         break;
 997       case ALT_FEET:
 998         ui->valt->setValue(convertFeetToMeters(columnData[info.dataIndex]));
 999         break;
1000       case ALT_METERS:
1001         ui->valt->setValue(columnData[info.dataIndex].toDouble());
1002         break;
1003       case FASV:
1004         ui->vfas->setValue(columnData[info.dataIndex].toDouble());
1005         break;
1006       case FASC:
1007         ui->curr->setValue(columnData[info.dataIndex].toDouble());
1008         break;
1009       case CELS_GRE:
1010         ui->cell1->setValue(columnData[info.dataIndex].toDouble());
1011         break;
1012       case ASPD_KTS:
1013         ui->aspeed->setValue(columnData[info.dataIndex].toDouble() * 1.8520008892119);
1014         break;
1015       case ASPD_KMH:
1016         ui->aspeed->setValue(columnData[info.dataIndex].toDouble());
1017         break;
1018       case ASPD_MPH:
1019         ui->aspeed->setValue(columnData[info.dataIndex].toDouble() * 1.60934);
1020         break;
1021       case GALT_FEET:
1022         ui->gps_alt->setValue(convertFeetToMeters(columnData[info.dataIndex]));
1023         break;
1024       case GALT_METERS:
1025         ui->gps_alt->setValue(columnData[info.dataIndex].toDouble());
1026         break;
1027       case GSPD_KNTS:
1028         ui->gps_speed->setValue(columnData[info.dataIndex].toDouble() * 1.852);
1029         break;
1030       case GSPD_KMH:
1031         ui->gps_speed->setValue(columnData[info.dataIndex].toDouble());
1032         break;
1033       case GSPD_MPH:
1034         ui->gps_speed->setValue(columnData[info.dataIndex].toDouble() * 1.60934);
1035         break;
1036       case GHDG_DEG:
1037         ui->gps_course->setValue(columnData[info.dataIndex].toDouble());
1038         break;
1039       case GDATE:
1040         ui->gps_time->setText(convertGPSDate(columnData[info.dataIndex]));
1041         break;
1042       case G_LATLON:
1043         ui->gps_latlon->setText(convertGPS(columnData[info.dataIndex]));
1044         break;
1045       case ACCX:
1046         ui->accx->setValue(columnData[info.dataIndex].toDouble());
1047         break;
1048       case ACCY:
1049         ui->accy->setValue(columnData[info.dataIndex].toDouble());
1050         break;
1051       case ACCZ:
1052         ui->accz->setValue(columnData[info.dataIndex].toDouble());
1053         break;
1054       }
1055     }
1056     else {
1057       // file is corrupt - shut down with open logs, or log format changed mid-day
1058     }
1059   }
1060 }