radio/src/targets/simu/opentxsimulator.cpp

   1 /*
   2  * Author - Bertrand Songis <bsongis@gmail.com>
   3  *
   4  * Based on th9x -> http://code.google.com/p/th9x/
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  */
  16
  17 #include "opentxsimulator.h"
  18 #include "opentx.h"
  19 #include "simulcd.h"
  20
  21 #include <QDebug>
  22 #include <QElapsedTimer>
  23
  24 #if !defined(MAX_LOGICAL_SWITCHES) && defined(NUM_CSW)
  25   #define MAX_LOGICAL_SWITCHES    NUM_CSW
  26 #endif
  27
  28 #define GET_SWITCH_BOOL(sw__)    getSwitch((sw__), 0);
  29
  30 #define OTXS_DBG    qDebug() << "(" << simuTimerMicros() << "us)"
  31
  32 int16_t g_anas[Analogs::NUM_ANALOGS];
  33 QVector<QIODevice *> OpenTxSimulator::tracebackDevices;
  34
  35 uint16_t anaIn(uint8_t chan)
  36 {
  37   return g_anas[chan];
  38 }
  39
  40 uint16_t getAnalogValue(uint8_t index)
  41 {
  42   return anaIn(index);
  43 }
  44
  45 void firmwareTraceCb(const char * text)
  46 {
  47   foreach (QIODevice * dev, OpenTxSimulator::tracebackDevices) {
  48     if (dev)
  49       dev->write(text);
  50   }
  51 }
  52
  53 OpenTxSimulator::OpenTxSimulator() :
  54   SimulatorInterface(),
  55   m_timer10ms(nullptr),
  56   m_resetOutputsData(true),
  57   m_stopRequested(false)
  58 {
  59   tracebackDevices.clear();
  60   traceCallback = firmwareTraceCb;
  61 }
  62
  63 OpenTxSimulator::~OpenTxSimulator()
  64 {
  65   traceCallback = nullptr;
  66   tracebackDevices.clear();
  67
  68   if (m_timer10ms)
  69     delete m_timer10ms;
  70
  71   if (isRunning()) {
  72     stop();
  73     QElapsedTimer tmout;
  74     tmout.start();
  75     while (isRunning() && !tmout.hasExpired(1000))
  76       ;
  77   }
  78   //qDebug() << "Deleting OpenTxSimulator";
  79 }
  80
  81 QString OpenTxSimulator::name()
  82 {
  83   return QString(SIMULATOR_FLAVOUR);
  84 }
  85
  86 bool OpenTxSimulator::isRunning()
  87 {
  88   QMutexLocker lckr(&m_mtxSimuMain);
  89   return simuIsRunning();
  90 }
  91
  92 void OpenTxSimulator::init()
  93 {
  94   if (isRunning())
  95     return;
  96
  97   OTXS_DBG;
  98
  99   if (!m_timer10ms) {
 100     // make sure we create & control the timer from current thread
 101     m_timer10ms = new QTimer();
 102     m_timer10ms->setInterval(10);
 103     connect(m_timer10ms, &QTimer::timeout, this, &OpenTxSimulator::run);
 104     connect(this, SIGNAL(started()), m_timer10ms, SLOT(start()));
 105     connect(this, SIGNAL(stopped()), m_timer10ms, SLOT(stop()));
 106   }
 107
 108   m_resetOutputsData = true;
 109   setStopRequested(false);
 110
 111   QMutexLocker lckr(&m_mtxSimuMain);
 112   memset(g_anas, 0, sizeof(g_anas));
 113
 114 #if defined(PCBTARANIS)
 115   g_anas[TX_RTC_VOLTAGE] = 800;  // 2,34V
 116 #endif
 117
 118   simuInit();
 119 }
 120
 121 void OpenTxSimulator::start(const char * filename, bool tests)
 122 {
 123   if (isRunning())
 124     return;
 125   OTXS_DBG << "file:" << filename << "tests:" << tests;
 126
 127   QMutexLocker lckr(&m_mtxSimuMain);
 128   QMutexLocker slckr(&m_mtxSettings);
 129   StartEepromThread(filename);
 130   StartAudioThread(volumeGain);
 131   StartSimu(tests, simuSdDirectory.toLatin1().constData(), simuSettingsDirectory.toLatin1().constData());
 132
 133   emit started();
 134   QTimer::singleShot(0, this, SLOT(run()));  // old style for Qt < 5.4
 135 }
 136
 137 void OpenTxSimulator::stop()
 138 {
 139   if (!isRunning())
 140     return;
 141   OTXS_DBG;
 142
 143   setStopRequested(true);
 144
 145   QMutexLocker lckr(&m_mtxSimuMain);
 146   StopSimu();
 147   StopAudioThread();
 148   StopEepromThread();
 149
 150   emit stopped();
 151 }
 152
 153 void OpenTxSimulator::setSdPath(const QString & sdPath, const QString & settingsPath)
 154 {
 155   QMutexLocker lckr(&m_mtxSettings);
 156   simuSdDirectory = sdPath;
 157   simuSettingsDirectory = settingsPath;
 158 }
 159
 160 void OpenTxSimulator::setVolumeGain(const int value)
 161 {
 162   QMutexLocker lckr(&m_mtxSettings);
 163   volumeGain = value;
 164 }
 165
 166 void OpenTxSimulator::setRadioData(const QByteArray & data)
 167 {
 168 #if defined(EEPROM_SIZE)
 169   QMutexLocker lckr(&m_mtxRadioData);
 170   eeprom = (uint8_t *)malloc(qMin<int>(EEPROM_SIZE, data.size()));
 171   memcpy(eeprom, data.data(), qMin<int>(EEPROM_SIZE, data.size()));
 172 #endif
 173 }
 174
 175 void OpenTxSimulator::readRadioData(QByteArray & dest)
 176 {
 177 #if defined(EEPROM_SIZE)
 178   QMutexLocker lckr(&m_mtxRadioData);
 179   memcpy(dest.data(), eeprom, std::min<int>(EEPROM_SIZE, dest.size()));
 180 #endif
 181 }
 182
 183 uint8_t * OpenTxSimulator::getLcd()
 184 {
 185   return (uint8_t *)simuLcdBuf;
 186 }
 187
 188 void OpenTxSimulator::setAnalogValue(uint8_t index, int16_t value)
 189 {
 190   static int dim = DIM(g_anas);
 191   if (index < dim)
 192     g_anas[index] = value;
 193 }
 194
 195 void OpenTxSimulator::setSwitch(uint8_t swtch, int8_t state)
 196 {
 197   simuSetSwitch(swtch, state);
 198 }
 199
 200 void OpenTxSimulator::setKey(uint8_t key, bool state)
 201 {
 202   simuSetKey(key, state);
 203 }
 204
 205 void OpenTxSimulator::setTrimSwitch(uint8_t trim, bool state)
 206 {
 207   simuSetTrim(trim, state);
 208 }
 209
 210 void OpenTxSimulator::setTrim(unsigned int idx, int value)
 211 {
 212   unsigned i = idx;
 213   if (i < 4)  // swap axes
 214     i = modn12x3[4 * getStickMode() + idx];
 215   uint8_t phase = getTrimFlightMode(getFlightMode(), i);
 216
 217   if (!setTrimValue(phase, i, value)) {
 218     QTimer *timer = new QTimer(this);
 219     timer->setSingleShot(true);
 220     connect(timer, &QTimer::timeout, [=]() {
 221       emit trimValueChange(idx, 0);
 222       emit outputValueChange(OUTPUT_SRC_TRIM_VALUE, idx, 0);
 223       timer->deleteLater();
 224     });
 225     timer->start(350);
 226   }
 227 }
 228
 229 void OpenTxSimulator::setTrainerInput(unsigned int inputNumber, int16_t value)
 230 {
 231   static unsigned dim = DIM(ppmInput);
 232   //setTrainerTimeout(100);
 233   if (inputNumber < dim)
 234     ppmInput[inputNumber] = qMin(qMax((int16_t)-512, value), (int16_t)512);
 235 }
 236
 237 void OpenTxSimulator::setInputValue(int type, uint8_t index, int16_t value)
 238 {
 239   //qDebug() << type << index << value << this->thread();
 240   switch (type) {
 241     case INPUT_SRC_ANALOG :
 242     case INPUT_SRC_STICK :
 243       setAnalogValue(index, value);
 244       break;
 245     case INPUT_SRC_KNOB :
 246       setAnalogValue(index + NUM_STICKS, value);
 247       break;
 248     case INPUT_SRC_SLIDER :
 249       setAnalogValue(index + NUM_STICKS + NUM_POTS, value);
 250       break;
 251     case INPUT_SRC_TXVIN :
 252       setAnalogValue(Analogs::TX_VOLTAGE, voltageToAdc(value));
 253       break;
 254     case INPUT_SRC_SWITCH :
 255       setSwitch(index, (int8_t)value);
 256       break;
 257     case INPUT_SRC_TRIM_SW :
 258       setTrimSwitch(index, (bool)value);
 259       break;
 260     case INPUT_SRC_TRIM :
 261       setTrim(index, value);
 262       break;
 263     case INPUT_SRC_KEY :
 264       setKey(index, (bool)value);
 265       break;
 266     case INPUT_SRC_TRAINER :
 267       setTrainerInput(index, value);
 268       break;
 269     case INPUT_SRC_ROTENC :  // TODO
 270     default:
 271       return;
 272   }
 273 }
 274
 275 void OpenTxSimulator::rotaryEncoderEvent(int steps)
 276 {
 277 #if defined(ROTARY_ENCODER_NAVIGATION)
 278   ROTARY_ENCODER_NAVIGATION_VALUE += steps * ROTARY_ENCODER_GRANULARITY;
 279 #else
 280   // TODO : this should probably be handled in the GUI
 281   int key;
 282 #if defined(PCBXLITE)
 283   if (steps > 0)
 284     key = KEY_DOWN;
 285   else if (steps < 0)
 286     key = KEY_UP;
 287 #else
 288   if (steps > 0)
 289     key = KEY_MINUS;
 290   else if (steps < 0)
 291     key = KEY_PLUS;
 292 #endif
 293   else
 294     // Should not happen but Clang complains that key is unset otherwise
 295     return;
 296
 297   setKey(key, 1);
 298 #if (QT_VERSION >= QT_VERSION_CHECK(5, 4, 0))
 299   QTimer::singleShot(10, [this, key]() { setKey(key, 0); });
 300 #else
 301   QTimer *timer = new QTimer(this);
 302   timer->setSingleShot(true);
 303   connect(timer, &QTimer::timeout, [=]() {
 304     setKey(key, 0);
 305     timer->deleteLater();
 306   } );
 307   timer->start(10);
 308 #endif
 309 #endif  // defined(ROTARY_ENCODER_NAVIGATION)
 310 }
 311
 312 void OpenTxSimulator::setTrainerTimeout(uint16_t ms)
 313 {
 314   ppmInputValidityTimer = ms;
 315 }
 316
 317 void OpenTxSimulator::sendTelemetry(const QByteArray data)
 318 {
 319   //OTXS_DBG << data;
 320   sportProcessTelemetryPacket((uint8_t *)data.constData());
 321 }
 322
 323 uint8_t OpenTxSimulator::getSensorInstance(uint16_t id, uint8_t defaultValue)
 324 {
 325   for (int i = 0; i < MAX_TELEMETRY_SENSORS; i++) {
 326     if (isTelemetryFieldAvailable(i)) {
 327       TelemetrySensor * sensor = &g_model.telemetrySensors[i];
 328       if (sensor->id == id) {
 329         return sensor->instance;
 330       }
 331     }
 332   }
 333   return defaultValue;
 334 }
 335
 336 uint16_t OpenTxSimulator::getSensorRatio(uint16_t id)
 337 {
 338   for (int i = 0; i < MAX_TELEMETRY_SENSORS; i++) {
 339     if (isTelemetryFieldAvailable(i)) {
 340       TelemetrySensor * sensor = &g_model.telemetrySensors[i];
 341       if (sensor->id == id) {
 342         return sensor->custom.ratio;
 343       }
 344     }
 345   }
 346   return 0;
 347 }
 348
 349 const int OpenTxSimulator::getCapability(Capability cap)
 350 {
 351   int ret = 0;
 352   switch(cap) {
 353     case CAP_LUA :
 354       #ifdef LUA
 355         ret = 1;
 356       #endif
 357       break;
 358
 359     case CAP_ROTARY_ENC :
 360       break;
 361
 362     case CAP_ROTARY_ENC_NAV :
 363       #ifdef ROTARY_ENCODER_NAVIGATION
 364         ret = 1;
 365       #endif
 366       break;
 367
 368     case CAP_TELEM_FRSKY_SPORT :
 369         ret = 1;
 370       break;
 371   }
 372   return ret;
 373 }
 374
 375 void OpenTxSimulator::setLuaStateReloadPermanentScripts()
 376 {
 377 #if defined(LUA)
 378   luaState = INTERPRETER_RELOAD_PERMANENT_SCRIPTS;
 379 #endif
 380 }
 381
 382 void OpenTxSimulator::addTracebackDevice(QIODevice * device)
 383 {
 384   QMutexLocker lckr(&m_mtxTbDevices);
 385   if (device && !tracebackDevices.contains(device))
 386     tracebackDevices.append(device);
 387 }
 388
 389 void OpenTxSimulator::removeTracebackDevice(QIODevice * device)
 390 {
 391   if (device) {
 392     QMutexLocker lckr(&m_mtxTbDevices);
 393     // no QVector::removeAll() in Qt < 5.4
 394     int i = 0;
 395     foreach (QIODevice * d, tracebackDevices) {
 396       if (d == device)
 397         tracebackDevices.remove(i);
 398       ++i;
 399     }
 400   }
 401 }
 402
 403
 404 /*** Protected functions ***/
 405
 406 void OpenTxSimulator::run()
 407 {
 408   static uint32_t loops = 0;
 409   static QElapsedTimer ts;
 410
 411   if (!loops)
 412     ts.start();
 413
 414   if (isStopRequested()) {
 415     return;
 416   }
 417   if (!isRunning()) {
 418     QString err(getError());
 419     emit runtimeError(err);
 420     emit stopped();
 421     return;
 422   }
 423
 424   ++loops;
 425
 426   per10ms();
 427
 428   checkLcdChanged();
 429
 430   if (!(loops % 5)) {
 431     checkOutputsChanged();
 432   }
 433
 434   if (!(loops % (SIMULATOR_INTERFACE_HEARTBEAT_PERIOD / 10))) {
 435     emit heartbeat(loops, simuTimerMicros() / 1000);
 436   }
 437 }
 438
 439 bool OpenTxSimulator::isStopRequested()
 440 {
 441   QMutexLocker lckr(&m_mtxStopReq);
 442   return m_stopRequested;
 443 }
 444
 445 void OpenTxSimulator::setStopRequested(bool stop)
 446 {
 447   QMutexLocker lckr(&m_mtxStopReq);
 448   m_stopRequested = stop;
 449 }
 450
 451 bool OpenTxSimulator::checkLcdChanged()
 452 {
 453   if (simuLcdRefresh) {
 454     simuLcdRefresh = false;
 455     emit lcdChange(isBacklightEnabled());
 456     return true;
 457   }
 458   return false;
 459 }
 460
 461 void OpenTxSimulator::checkOutputsChanged()
 462 {
 463   static TxOutputs lastOutputs;
 464   static size_t chansDim = DIM(channelOutputs);
 465   const static int16_t limit = 512 * 2;
 466   qint32 tmpVal;
 467   uint8_t i, idx;
 468   const uint8_t phase = getFlightMode();  // opentx.cpp
 469   const uint8_t mode = getStickMode();
 470
 471   for (i=0; i < chansDim; i++) {
 472     if (lastOutputs.chans[i] != channelOutputs[i] || m_resetOutputsData) {
 473       emit channelOutValueChange(i, channelOutputs[i], (g_model.extendedLimits ? limit * LIMIT_EXT_PERCENT / 100 : limit));
 474       emit outputValueChange(OUTPUT_SRC_CHAN_OUT, i, channelOutputs[i]);
 475       lastOutputs.chans[i] = channelOutputs[i];
 476     }
 477     if (lastOutputs.ex_chans[i] != ex_chans[i] || m_resetOutputsData) {
 478       emit channelMixValueChange(i, ex_chans[i], limit * 2);
 479       emit outputValueChange(OUTPUT_SRC_CHAN_MIX, i, ex_chans[i]);
 480       lastOutputs.ex_chans[i] = ex_chans[i];
 481     }
 482   }
 483
 484   for (i=0; i < MAX_LOGICAL_SWITCHES; i++) {
 485     tmpVal = (qint32)GET_SWITCH_BOOL(SWSRC_SW1+i);
 486     if (lastOutputs.vsw[i] != (bool)tmpVal || m_resetOutputsData) {
 487       emit virtualSwValueChange(i, tmpVal);
 488       emit outputValueChange(OUTPUT_SRC_VIRTUAL_SW, i, tmpVal);
 489       lastOutputs.vsw[i] = tmpVal;
 490     }
 491   }
 492
 493   for (i=0; i < Board::TRIM_AXIS_COUNT; i++) {
 494     if (i < 4)  // swap axes
 495       idx = modn12x3[4 * mode + i];
 496     else
 497       idx = i;
 498
 499     tmpVal = getTrimValue(getTrimFlightMode(phase, idx), idx);
 500     if (lastOutputs.trims[i] != tmpVal || m_resetOutputsData) {
 501       emit trimValueChange(i, tmpVal);
 502       emit outputValueChange(OUTPUT_SRC_TRIM_VALUE, i, tmpVal);
 503       lastOutputs.trims[i] = tmpVal;
 504     }
 505   }
 506
 507   tmpVal = g_model.extendedTrims ? TRIM_EXTENDED_MAX : TRIM_MAX;
 508   if (lastOutputs.trimRange != tmpVal || m_resetOutputsData) {
 509     emit trimRangeChange(Board::TRIM_AXIS_COUNT, -tmpVal, tmpVal);
 510     emit outputValueChange(OUTPUT_SRC_TRIM_RANGE, Board::TRIM_AXIS_COUNT, tmpVal);
 511     lastOutputs.trimRange = tmpVal;
 512   }
 513
 514   if (lastOutputs.phase != phase || m_resetOutputsData) {
 515     emit phaseChanged(phase, getCurrentPhaseName());
 516     emit outputValueChange(OUTPUT_SRC_PHASE, 0, qint16(phase));
 517     lastOutputs.phase = phase;
 518   }
 519
 520 #if defined(GVAR_VALUE) && defined(GVARS)
 521   gVarMode_t gvar;
 522   for (uint8_t gv=0; gv < MAX_GVARS; gv++) {
 523     gvar.prec = g_model.gvars[gv].prec;
 524     gvar.unit = g_model.gvars[gv].unit;
 525     for (uint8_t fm=0; fm < MAX_FLIGHT_MODES; fm++) {
 526       gvar.mode = fm;
 527       gvar.value = (int16_t)GVAR_VALUE(gv, getGVarFlightMode(fm, gv));
 528       tmpVal = gvar;
 529       if (lastOutputs.gvars[fm][gv] != tmpVal || m_resetOutputsData) {
 530         lastOutputs.gvars[fm][gv] = tmpVal;
 531         emit gVarValueChange(gv, tmpVal);
 532         emit outputValueChange(OUTPUT_SRC_GVAR, gv, tmpVal);
 533       }
 534     }
 535   }
 536 #endif
 537
 538   m_resetOutputsData = false;
 539 }
 540
 541 uint8_t OpenTxSimulator::getStickMode()
 542 {
 543   return limit<uint8_t>(0, g_eeGeneral.stickMode, 3);
 544 }
 545
 546 const char * OpenTxSimulator::getPhaseName(unsigned int phase)
 547 {
 548   static char buff[sizeof(g_model.flightModeData[0].name)+1];
 549   zchar2str(buff, g_model.flightModeData[phase].name, sizeof(g_model.flightModeData[0].name));
 550   return buff;
 551 }
 552
 553 const QString OpenTxSimulator::getCurrentPhaseName()
 554 {
 555   unsigned phase = getFlightMode();
 556   QString name(getPhaseName(phase));
 557   if (name.isEmpty())
 558     name = QString::number(phase);
 559   return name;
 560 }
 561
 562 const char * OpenTxSimulator::getError()
 563 {
 564   return main_thread_error;
 565 }
 566
 567 const int OpenTxSimulator::voltageToAdc(const int volts)
 568 {
 569   int ret = 0;
 570 #if defined(PCBHORUS) || defined(PCBX7)
 571   ret = (float)volts * 16.2f;
 572 #elif defined(PCBTARANIS) || defined(PCBSKY9X)
 573   ret = (float)volts * 13.3f;
 574 #else
 575   ret = (float)volts * 14.15f;
 576 #endif
 577   return ret;
 578 }
 579
 580
 581 /*
 582  * OpenTxSimulatorFactory
 583  */
 584
 585 class OpenTxSimulatorFactory: public SimulatorFactory
 586 {
 587   public:
 588     OpenTxSimulatorFactory()
 589     {
 590     }
 591
 592     virtual SimulatorInterface * create()
 593     {
 594       return new OpenTxSimulator();
 595     }
 596
 597     virtual QString name()
 598     {
 599       return QString(SIMULATOR_FLAVOUR);
 600     }
 601
 602     virtual Board::Type type()
 603     {
 604 #if defined(PCBX12S)
 605       return Board::BOARD_HORUS_X12S;
 606 #elif defined(PCBX10)
 607       return Board::BOARD_X10;
 608 #elif defined(PCBX7)
 609       return Board::BOARD_TARANIS_X7;
 610 #elif defined(PCBX9LITES)
 611       return Board::BOARD_TARANIS_X9LITES;
 612 #elif defined(PCBX9LITE)
 613       return Board::BOARD_TARANIS_X9LITE;
 614 #elif defined(PCBTARANIS)
 615       return Board::BOARD_TARANIS_X9D;
 616 #else
 617       return Board::BOARD_9X_M64;
 618 #endif
 619     }
 620 };
 621
 622 extern "C" DLLEXPORT SimulatorFactory * registerSimu()
 623 {
 624   return new OpenTxSimulatorFactory();
 625 }