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