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