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<uint32_t>(0, ui->A3->value() * 100.0, 0xFFFFFFFF));
 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<uint32_t>(0, ui->A4->value() * 100.0, 0xFFFFFFFF));
 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("ASpd(kts)", ASPD_KTS);
 662   colToFuncMap.insert("ASpd(kmh)", ASPD_KMH);
 663   colToFuncMap.insert("ASpd(mph)", ASPD_MPH);
 664   colToFuncMap.insert("GAlt(ft)", GALT_FEET);
 665   colToFuncMap.insert("GAlt(m)", GALT_METERS);
 666   colToFuncMap.insert("GSpd(kts)", GSPD_KNTS);
 667   colToFuncMap.insert("GSpd(kmh)", GSPD_KMH);
 668   colToFuncMap.insert("GSpd(mph)", GSPD_MPH);
 669   colToFuncMap.insert("Hdg(@)", GHDG_DEG);
 670   colToFuncMap.insert("Date", GDATE);
 671   colToFuncMap.insert("GPS", G_LATLON);
 672   colToFuncMap.insert("AccX(g)", ACCX);
 673   colToFuncMap.insert("AccY(g)", ACCY);
 674   colToFuncMap.insert("AccZ(g)", ACCZ);
 675
 676   // ACCX Y and Z
 677 }
 678
 679 QDateTime TelemetrySimulator::LogPlaybackController::parseTransmittterTimestamp(QString row)
 680 {
 681   QStringList rowParts = row.simplified().split(',');
 682   if (rowParts.size() < 2) {
 683     return QDateTime();
 684   }
 685   QString datePart = rowParts[0];
 686   QString timePart = rowParts[1];
 687   QDateTime result;
 688   QString format("yyyy-MM-dd hh:mm:ss.zzz"); // assume this format
 689   // hour can be 'missing'
 690   if (timePart.count(":") < 2) {
 691     timePart = "00:" + timePart;
 692   }
 693   if (datePart.contains("/")) { // happens when csv is edited by Excel
 694     format = "M/d/yyyy hh:mm:ss.z";
 695   }
 696   return QDateTime::fromString(datePart + " " + timePart, format);
 697 }
 698
 699 void TelemetrySimulator::LogPlaybackController::checkGpsFormat()
 700 {
 701   // sample the first record to check if cords are in decimal format
 702   logFileGpsCordsInDecimalFormat = false;
 703   if(csvRecords.count() > 1) {
 704     QStringList keys = csvRecords[0].split(',');
 705     if(keys.contains("GPS")) {
 706       int gpsColIndex = keys.indexOf("GPS");
 707       QStringList firstRowVlues = csvRecords[1].split(',');
 708       QString gpsSample = firstRowVlues[gpsColIndex];
 709       QStringList cords = gpsSample.simplified().split(' ');
 710       if (cords.count() == 2) {
 711         // frsky and TBS crossfire GPS sensor logs cords in decimal format with a precision of 6 places
 712         // if this format is met there is no need to call convertDegMin later on when processing the file
 713         QRegularExpression decimalCoordinateFormatRegex("^[-+]?\\d{1,2}[.]\\d{6}$");
 714         QRegularExpressionMatch latFormatMatch = decimalCoordinateFormatRegex.match(cords[0]);
 715         QRegularExpressionMatch lonFormatMatch = decimalCoordinateFormatRegex.match(cords[1]);
 716         if (lonFormatMatch.hasMatch() && latFormatMatch.hasMatch()) {
 717           logFileGpsCordsInDecimalFormat = true;
 718         }
 719       }
 720     }
 721   }
 722 }
 723
 724 void TelemetrySimulator::LogPlaybackController::calcLogFrequency()
 725 {
 726   // examine up to 20 rows to determine log frequency in seconds
 727   // Skip the first entry which contains the file open time
 728   logFrequency = 25.5; // default value
 729   QDateTime lastTime;
 730   for (int i = 2; (i < 21) && (i < csvRecords.count()); i++)
 731   {
 732     QDateTime logTime = parseTransmittterTimestamp(csvRecords[i]);
 733     // ugh - no timespan in this Qt version
 734     double timeDiff = (logTime.toMSecsSinceEpoch() - lastTime.toMSecsSinceEpoch()) / 1000.0;
 735     if ((timeDiff > 0.09) && (timeDiff < logFrequency)) {
 736       logFrequency = timeDiff;
 737     }
 738     lastTime = logTime;
 739   }
 740 }
 741
 742 bool TelemetrySimulator::LogPlaybackController::isReady()
 743 {
 744   return csvRecords.count() > 1;
 745 }
 746
 747 void TelemetrySimulator::LogPlaybackController::loadLogFile()
 748 {
 749   QString logFileNameAndPath = QFileDialog::getOpenFileName(NULL, tr("Log File"), g.logDir(), tr("LOG Files (*.csv)"));
 750   if (logFileNameAndPath.isEmpty())
 751     return;
 752
 753   g.logDir(logFileNameAndPath);
 754
 755   // reset the playback ui
 756   ui->play->setEnabled(false);
 757   ui->rewind->setEnabled(false);
 758   ui->stepBack->setEnabled(false);
 759   ui->stepForward->setEnabled(false);
 760   ui->stop->setEnabled(false);
 761   ui->positionIndicator->setEnabled(false);
 762   ui->replayRate->setEnabled(false);
 763   ui->positionLabel->setText("Row #\nTimestamp");
 764
 765   // clear existing data
 766   csvRecords.clear();
 767
 768   QFile file(logFileNameAndPath);
 769   if (!file.open(QIODevice::ReadOnly)) {
 770     ui->logFileLabel->setText(tr("ERROR - invalid file"));
 771     return;
 772   }
 773   while (!file.atEnd()) {
 774     QByteArray line = file.readLine();
 775     csvRecords.append(line.simplified());
 776   }
 777   file.close();
 778   if (csvRecords.count() > 1) {
 779     columnNames.clear();
 780     QStringList keys = csvRecords[0].split(',');
 781     // override the first two column names
 782     keys[0] = "LogDate";
 783     keys[1] = "LogTime";
 784     Q_FOREACH(QString key, keys) {
 785       columnNames.append(key.simplified());
 786     }
 787     ui->play->setEnabled(true);
 788     ui->rewind->setEnabled(true);
 789     ui->stepBack->setEnabled(true);
 790     ui->stepForward->setEnabled(true);
 791     ui->stop->setEnabled(true);
 792     ui->positionIndicator->setEnabled(true);
 793     ui->replayRate->setEnabled(true);
 794     supportedCols.clear();
 795     recordIndex = 1;
 796     calcLogFrequency();
 797     checkGpsFormat();
 798   }
 799   ui->logFileLabel->setText(QFileInfo(logFileNameAndPath).fileName());
 800   // iterate through all known mappings and add those that are used
 801   QMapIterator<QString, CONVERT_TYPE> it(colToFuncMap);
 802   while (it.hasNext()) {
 803     it.next();
 804     addColumnHash(it.key(), it.value());
 805   }
 806   rewind();
 807   return;
 808 }
 809
 810 void TelemetrySimulator::LogPlaybackController::addColumnHash(QString key, CONVERT_TYPE functionIndex)
 811 {
 812   DATA_TO_FUNC_XREF dfx;
 813   if (columnNames.contains(key)) {
 814     dfx.functionIndex = functionIndex;
 815     dfx.dataIndex = columnNames.indexOf(key);
 816     supportedCols.append(dfx);
 817   }
 818 }
 819
 820 void TelemetrySimulator::LogPlaybackController::play()
 821 {
 822 }
 823
 824 void TelemetrySimulator::LogPlaybackController::stop()
 825 {
 826 }
 827
 828 void TelemetrySimulator::LogPlaybackController::rewind()
 829 {
 830   stepping = true;
 831   recordIndex = 1;
 832   ui->stop->setChecked(true);
 833   updatePositionLabel(-1);
 834   setUiDataValues();
 835   stepping = false;
 836 }
 837
 838 void TelemetrySimulator::LogPlaybackController::stepForward(bool focusOnStop)
 839 {
 840   stepping = true;
 841   if (recordIndex < (csvRecords.count() - 1)) {
 842     recordIndex++;
 843     if (focusOnStop) {
 844       ui->stop->setChecked(true);
 845     }
 846     updatePositionLabel(-1);
 847     setUiDataValues();
 848   }
 849   else {
 850     rewind(); // always loop at the end
 851   }
 852   stepping = false;
 853 }
 854
 855 void TelemetrySimulator::LogPlaybackController::stepBack()
 856 {
 857   stepping = true;
 858   if (recordIndex > 1) {
 859     recordIndex--;
 860     ui->stop->setChecked(true);
 861     updatePositionLabel(-1);
 862     setUiDataValues();
 863   }
 864   stepping = false;
 865 }
 866
 867 double TelemetrySimulator::LogPlaybackController::convertFeetToMeters(QString input)
 868 {
 869   double meters100 = input.toDouble() * 0.3048;
 870   return qFloor(meters100 + .005);
 871 }
 872
 873 QString TelemetrySimulator::LogPlaybackController::convertGPSDate(QString input)
 874 {
 875   QStringList dateTime = input.simplified().split(' ');
 876   if (dateTime.size() < 2) {
 877     return ""; // invalid format
 878   }
 879   QStringList dateParts = dateTime[0].split('-'); // input as yy-mm-dd
 880   if (dateParts.size() < 3) {
 881     return ""; // invalid format
 882   }
 883   // output is dd-MM-yyyy hh:mm:ss
 884   QString localDateString = dateParts[2] + "-" + dateParts[1] + "-20" + dateParts[0] + " " + dateTime[1];
 885   QString format("dd-MM-yyyy hh:mm:ss");
 886   QDateTime utcDate = QDateTime::fromString(localDateString, format).toTimeSpec(Qt::UTC);
 887   return utcDate.toString(format);
 888 }
 889
 890 double TelemetrySimulator::LogPlaybackController::convertDegMin(QString input)
 891 {
 892   double fInput = input.mid(0, input.length() - 1).toDouble();
 893   double degrees = qFloor(fInput / 100.0);
 894   double minutes = fInput - (degrees * 100);
 895   int32_t sign = ((input.endsWith('E')) || (input.endsWith('N'))) ? 1 : -1;
 896   return (degrees + (minutes / 60)) * sign;
 897 }
 898
 899 QString TelemetrySimulator::LogPlaybackController::convertGPS(QString input)
 900 {
 901   // input format is DDmm.mmmmH DDDmm.mmmmH (longitude latitude - degrees (2 places) minutes (2 places) decimal minutes (4 places))
 902   QStringList lonLat = input.simplified().split(' ');
 903   if (lonLat.count() < 2) {
 904     return ""; // invalid format
 905   }
 906   double lon, lat;
 907   if (logFileGpsCordsInDecimalFormat) {
 908     lat = lonLat[0].toDouble();
 909     lon = lonLat[1].toDouble();
 910   }
 911   else {
 912     lon = convertDegMin(lonLat[0]);
 913     lat = convertDegMin(lonLat[1]);
 914   }
 915   return QString::number(lat, 'f', 6) + ", " + QString::number(lon, 'f', 6);
 916 }
 917
 918 void TelemetrySimulator::LogPlaybackController::updatePositionLabel(int32_t percentage)
 919 {
 920   if ((percentage > 0) && (!stepping)) {
 921     recordIndex = qFloor((double)csvRecords.count() / 100.0 * percentage);
 922     if (recordIndex == 0) {
 923       recordIndex = 1; // record 0 is column labels
 924     }
 925   }
 926   // format the transmitter date info
 927   QDateTime transmitterTimestamp = parseTransmittterTimestamp(csvRecords[recordIndex]);
 928   QString format("yyyy-MM-dd hh:mm:ss.z");
 929   ui->positionLabel->setText("Row " + QString::number(recordIndex) + " of " + QString::number(csvRecords.count() - 1)
 930               + "\n" + transmitterTimestamp.toString(format));
 931   if (percentage < 0) { // did we step past a threshold?
 932     uint32_t posPercent = (recordIndex / (double)(csvRecords.count() - 1)) * 100;
 933     ui->positionIndicator->setValue(posPercent);
 934   }
 935 }
 936
 937 double TelemetrySimulator::LogPlaybackController::convertFahrenheitToCelsius(QString input)
 938 {
 939   return (input.toDouble() - 32.0) * 0.5556;
 940 }
 941
 942 void TelemetrySimulator::LogPlaybackController::setUiDataValues()
 943 {
 944   QStringList columnData = csvRecords[recordIndex].split(',');
 945   Q_FOREACH(DATA_TO_FUNC_XREF info, supportedCols) {
 946     if (info.dataIndex < columnData.size()) {
 947       switch (info.functionIndex) {
 948       case RXBT_V:
 949         ui->rxbt->setValue(columnData[info.dataIndex].toDouble());
 950         break;
 951       case RSSI:
 952         ui->Rssi->setValue(columnData[info.dataIndex].toDouble());
 953         break;
 954       case RAS:
 955         ui->Swr->setValue(columnData[info.dataIndex].toDouble());
 956         break;
 957       case A1:
 958         ui->A1->setValue(columnData[info.dataIndex].toDouble());
 959         break;
 960       case A2:
 961         ui->A2->setValue(columnData[info.dataIndex].toDouble());
 962         break;
 963       case A3:
 964         ui->A3->setValue(columnData[info.dataIndex].toDouble());
 965         break;
 966       case A4:
 967         ui->A4->setValue(columnData[info.dataIndex].toDouble());
 968         break;
 969       case T1_DEGC:
 970         ui->T1->setValue(columnData[info.dataIndex].toDouble());
 971         break;
 972       case T2_DEGC:
 973         ui->T2->setValue(columnData[info.dataIndex].toDouble());
 974         break;
 975       case T1_DEGF:
 976         ui->T1->setValue(convertFahrenheitToCelsius(columnData[info.dataIndex]));
 977         break;
 978       case T2_DEGF:
 979         ui->T2->setValue(convertFahrenheitToCelsius(columnData[info.dataIndex]));
 980         break;
 981       case RPM:
 982         ui->rpm->setValue(columnData[info.dataIndex].toDouble());
 983         break;
 984       case FUEL:
 985         ui->fuel->setValue(columnData[info.dataIndex].toDouble());
 986         break;
 987       case FUEL_QTY:
 988         ui->fuel_qty->setValue(columnData[info.dataIndex].toDouble());
 989         break;
 990       case VSPD_MS:
 991         ui->vspeed->setValue(columnData[info.dataIndex].toDouble());
 992         break;
 993       case VSPD_FS:
 994         ui->vspeed->setValue(columnData[info.dataIndex].toDouble() * 0.3048);
 995         break;
 996       case ALT_FEET:
 997         ui->valt->setValue(convertFeetToMeters(columnData[info.dataIndex]));
 998         break;
 999       case ALT_METERS:
1000         ui->valt->setValue(columnData[info.dataIndex].toDouble());
1001         break;
1002       case FASV:
1003         ui->vfas->setValue(columnData[info.dataIndex].toDouble());
1004         break;
1005       case FASC:
1006         ui->curr->setValue(columnData[info.dataIndex].toDouble());
1007         break;
1008       case CELS_GRE:
1009         ui->cell1->setValue(columnData[info.dataIndex].toDouble());
1010         break;
1011       case ASPD_KTS:
1012         ui->aspeed->setValue(columnData[info.dataIndex].toDouble() * 1.8520008892119);
1013         break;
1014       case ASPD_KMH:
1015         ui->aspeed->setValue(columnData[info.dataIndex].toDouble());
1016         break;
1017       case ASPD_MPH:
1018         ui->aspeed->setValue(columnData[info.dataIndex].toDouble() * 1.60934);
1019         break;
1020       case GALT_FEET:
1021         ui->gps_alt->setValue(convertFeetToMeters(columnData[info.dataIndex]));
1022         break;
1023       case GALT_METERS:
1024         ui->gps_alt->setValue(columnData[info.dataIndex].toDouble());
1025         break;
1026       case GSPD_KNTS:
1027         ui->gps_speed->setValue(columnData[info.dataIndex].toDouble() * 1.852);
1028         break;
1029       case GSPD_KMH:
1030         ui->gps_speed->setValue(columnData[info.dataIndex].toDouble());
1031         break;
1032       case GSPD_MPH:
1033         ui->gps_speed->setValue(columnData[info.dataIndex].toDouble() * 1.60934);
1034         break;
1035       case GHDG_DEG:
1036         ui->gps_course->setValue(columnData[info.dataIndex].toDouble());
1037         break;
1038       case GDATE:
1039         ui->gps_time->setText(convertGPSDate(columnData[info.dataIndex]));
1040         break;
1041       case G_LATLON:
1042         ui->gps_latlon->setText(convertGPS(columnData[info.dataIndex]));
1043         break;
1044       case ACCX:
1045         ui->accx->setValue(columnData[info.dataIndex].toDouble());
1046         break;
1047       case ACCY:
1048         ui->accy->setValue(columnData[info.dataIndex].toDouble());
1049         break;
1050       case ACCZ:
1051         ui->accz->setValue(columnData[info.dataIndex].toDouble());
1052         break;
1053       }
1054     }
1055     else {
1056       // file is corrupt - shut down with open logs, or log format changed mid-day
1057     }
1058   }
1059 }