radio/src/opentx.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 "opentx.h"
  22
  23 RadioData  g_eeGeneral;
  24 ModelData  g_model;
  25
  26 #if defined(SDCARD)
  27 Clipboard clipboard;
  28 #endif
  29
  30 #if !defined(CPUARM)
  31 uint8_t g_tmr1Latency_max;
  32 uint8_t g_tmr1Latency_min;
  33 uint16_t lastMixerDuration;
  34 #endif
  35
  36 uint8_t unexpectedShutdown = 0;
  37
  38 /* AVR: mixer duration in 1/16ms */
  39 /* ARM: mixer duration in 0.5us */
  40 uint16_t maxMixerDuration;
  41
  42 #if defined(AUDIO) && !defined(CPUARM)
  43 audioQueue  audio;
  44 #endif
  45
  46 uint8_t heartbeat;
  47
  48 #if defined(OVERRIDE_CHANNEL_FUNCTION)
  49 safetych_t safetyCh[MAX_OUTPUT_CHANNELS];
  50 #endif
  51
  52 union ReusableBuffer reusableBuffer;
  53
  54 const pm_uint8_t bchout_ar[] PROGMEM = {
  55     0x1B, 0x1E, 0x27, 0x2D, 0x36, 0x39,
  56     0x4B, 0x4E, 0x63, 0x6C, 0x72, 0x78,
  57     0x87, 0x8D, 0x93, 0x9C, 0xB1, 0xB4,
  58     0xC6, 0xC9, 0xD2, 0xD8, 0xE1, 0xE4 };
  59
  60 uint8_t channel_order(uint8_t x)
  61 {
  62   return ( ((pgm_read_byte(bchout_ar + g_eeGeneral.templateSetup) >> (6-(x-1) * 2)) & 3 ) + 1 );
  63 }
  64
  65 /*
  66 mode1 rud ele thr ail
  67 mode2 rud thr ele ail
  68 mode3 ail ele thr rud
  69 mode4 ail thr ele rud
  70 */
  71 const pm_uint8_t modn12x3[] PROGMEM = {
  72     0, 1, 2, 3,
  73     0, 2, 1, 3,
  74     3, 1, 2, 0,
  75     3, 2, 1, 0 };
  76
  77 volatile tmr10ms_t g_tmr10ms;
  78
  79 #if defined(CPUARM)
  80 volatile uint8_t rtc_count = 0;
  81 uint32_t watchdogTimeout = 0;
  82
  83 void watchdogSuspend(uint32_t timeout)
  84 {
  85   watchdogTimeout = timeout;
  86 }
  87 #endif
  88
  89 void per10ms()
  90 {
  91   g_tmr10ms++;
  92
  93 #if defined(CPUARM)
  94   if (watchdogTimeout) {
  95     watchdogTimeout -= 1;
  96     wdt_reset();  // Retrigger hardware watchdog
  97   }
  98 #endif
  99
 100 #if defined(GUI)
 101   if (lightOffCounter) lightOffCounter--;
 102   if (flashCounter) flashCounter--;
 103   if (noHighlightCounter) noHighlightCounter--;
 104 #endif
 105
 106   if (trimsCheckTimer) trimsCheckTimer--;
 107   if (ppmInputValidityTimer) ppmInputValidityTimer--;
 108
 109 #if defined(CPUARM)
 110   if (trimsDisplayTimer)
 111     trimsDisplayTimer--;
 112   else
 113     trimsDisplayMask = 0;
 114 #endif
 115
 116 #if defined(RTCLOCK)
 117   /* Update global Date/Time every 100 per10ms cycles */
 118   if (++g_ms100 == 100) {
 119     g_rtcTime++;   // inc global unix timestamp one second
 120 #if defined(COPROCESSOR)
 121     if (g_rtcTime < 60 || rtc_count<5) {
 122       rtcInit();
 123       rtc_count++;
 124     }
 125     else {
 126       coprocReadData(true);
 127     }
 128 #endif
 129     g_ms100 = 0;
 130   }
 131 #endif
 132
 133   readKeysAndTrims();
 134
 135 #if defined(ROTARY_ENCODER_NAVIGATION)
 136   if (IS_ROTARY_ENCODER_NAVIGATION_ENABLE()) {
 137     static rotenc_t rePreviousValue;
 138     rotenc_t reNewValue = (ROTARY_ENCODER_NAVIGATION_VALUE / ROTARY_ENCODER_GRANULARITY);
 139     int8_t scrollRE = reNewValue - rePreviousValue;
 140     if (scrollRE) {
 141       rePreviousValue = reNewValue;
 142       putEvent(scrollRE < 0 ? EVT_ROTARY_LEFT : EVT_ROTARY_RIGHT);
 143     }
 144 #if defined(CPUARM)
 145     // rotary encoder navigation speed (acceleration) detection/calculation
 146     if (scrollRE) {
 147       static uint32_t lastTick = 0;
 148       static uint32_t delay = 0;
 149       delay = (((get_tmr10ms() - lastTick) << 3) + delay) >> 1;   // Modified moving average filter used for smoother change of speed
 150       lastTick = get_tmr10ms();
 151       if (delay < ROTENC_DELAY_HIGHSPEED)
 152         rotencSpeed = ROTENC_HIGHSPEED;
 153       else if (delay < ROTENC_DELAY_MIDSPEED)
 154         rotencSpeed = ROTENC_MIDSPEED;
 155       else
 156         rotencSpeed = ROTENC_LOWSPEED;
 157     }
 158 #endif
 159   }
 160 #endif
 161
 162 #if defined(TELEMETRY_FRSKY) || defined(TELEMETRY_JETI)
 163   if (!IS_DSM2_SERIAL_PROTOCOL(s_current_protocol[0])) {
 164     telemetryInterrupt10ms();
 165   }
 166 #endif
 167
 168   // These moved here from evalFlightModeMixes() to improve beep trigger reliability.
 169 #if defined(PWM_BACKLIGHT)
 170   if ((g_tmr10ms&0x03) == 0x00)
 171     backlightFade(); // increment or decrement brightness until target brightness is reached
 172 #endif
 173
 174 #if !defined(AUDIO)
 175   if (mixWarning & 1) if(((g_tmr10ms&0xFF)==  0)) AUDIO_MIX_WARNING(1);
 176   if (mixWarning & 2) if(((g_tmr10ms&0xFF)== 64) || ((g_tmr10ms&0xFF)== 72)) AUDIO_MIX_WARNING(2);
 177   if (mixWarning & 4) if(((g_tmr10ms&0xFF)==128) || ((g_tmr10ms&0xFF)==136) || ((g_tmr10ms&0xFF)==144)) AUDIO_MIX_WARNING(3);
 178 #endif
 179
 180 #if defined(SDCARD)
 181   sdPoll10ms();
 182 #endif
 183
 184   heartbeat |= HEART_TIMER_10MS;
 185 }
 186
 187 FlightModeData *flightModeAddress(uint8_t idx)
 188 {
 189   return &g_model.flightModeData[idx];
 190 }
 191
 192 ExpoData *expoAddress(uint8_t idx )
 193 {
 194   return &g_model.expoData[idx];
 195 }
 196
 197 MixData *mixAddress(uint8_t idx)
 198 {
 199   return &g_model.mixData[idx];
 200 }
 201
 202 LimitData *limitAddress(uint8_t idx)
 203 {
 204   return &g_model.limitData[idx];
 205 }
 206
 207 #if defined(CPUM64)
 208 void memclear(void *ptr, uint8_t size)
 209 {
 210   memset(ptr, 0, size);
 211 }
 212 #endif
 213
 214 void memswap(void * a, void * b, uint8_t size)
 215 {
 216   uint8_t * x = (uint8_t *)a;
 217   uint8_t * y = (uint8_t *)b;
 218   uint8_t temp ;
 219
 220   while (size--) {
 221     temp = *x;
 222     *x++ = *y;
 223     *y++ = temp;
 224   }
 225 }
 226
 227 void generalDefault()
 228 {
 229   memclear(&g_eeGeneral, sizeof(g_eeGeneral));
 230   g_eeGeneral.version  = EEPROM_VER;
 231   g_eeGeneral.variant = EEPROM_VARIANT;
 232
 233 #if !defined(PCBHORUS)
 234   g_eeGeneral.contrast = LCD_CONTRAST_DEFAULT;
 235 #endif
 236
 237 #if defined(PCBHORUS)
 238   #if PCBREV >= 13
 239     g_eeGeneral.potsConfig = 0x1B;  // S1 = pot, 6P = multipos, S2 = pot with detent
 240   #else
 241     g_eeGeneral.potsConfig = 0x19;  // S1 = pot without detent, 6P = multipos, S2 = pot with detent
 242   #endif
 243   g_eeGeneral.slidersConfig = 0x0f; // 4 sliders
 244   g_eeGeneral.blOffBright = 20;
 245 #elif defined(PCBX7)
 246   g_eeGeneral.potsConfig = 0x07;    // S1 = pot without detent, S2 = pot with detent
 247 #elif defined(PCBTARANIS)
 248   g_eeGeneral.potsConfig = 0x05;    // S1 and S2 = pots with detent
 249   g_eeGeneral.slidersConfig = 0x03; // LS and RS = sliders with detent
 250 #endif
 251
 252 #if defined(PCBX7)
 253   g_eeGeneral.switchConfig = 0x000006ff; // 4x3POS, 1x2POS, 1xTOGGLE
 254 #elif defined(PCBTARANIS) || defined(PCBHORUS)
 255   g_eeGeneral.switchConfig = 0x00007bff; // 6x3POS, 1x2POS, 1xTOGGLE
 256 #endif
 257
 258 // vBatWarn is voltage in 100mV, vBatMin is in 100mV but with -9V offset, vBatMax has a -12V offset
 259 #if defined(PCBX9E) || defined(PCBX12S)
 260   // NI-MH 9.6V
 261   g_eeGeneral.vBatWarn = 87;
 262   g_eeGeneral.vBatMin = -5;   //8,5V
 263   g_eeGeneral.vBatMax = -5;   //11,5V
 264 #elif defined(PCBX10)
 265   // Lipo 2V
 266   g_eeGeneral.vBatWarn = 66;
 267   g_eeGeneral.vBatMin = -28; // 6.2V
 268   g_eeGeneral.vBatMax = -38;   // 8.2V
 269 #elif defined(PCBTARANIS)
 270   // NI-MH 7.2V, X9D, X9D+ and X7
 271   g_eeGeneral.vBatWarn = 65;
 272   g_eeGeneral.vBatMin = -30; //6V
 273   g_eeGeneral.vBatMax = -40; //8V
 274 #else
 275   g_eeGeneral.vBatWarn = 90;
 276 #endif
 277
 278 #if defined(DEFAULT_MODE)
 279   g_eeGeneral.stickMode = DEFAULT_MODE-1;
 280 #endif
 281
 282 #if defined(PCBTARANIS)
 283   g_eeGeneral.templateSetup = 17; /* TAER */
 284 #endif
 285
 286 #if !defined(CPUM64)
 287   g_eeGeneral.backlightMode = e_backlight_mode_all;
 288   g_eeGeneral.lightAutoOff = 2;
 289   g_eeGeneral.inactivityTimer = 10;
 290 #endif
 291
 292 #if defined(CPUARM)
 293   g_eeGeneral.ttsLanguage[0] = 'e';
 294   g_eeGeneral.ttsLanguage[1] = 'n';
 295   g_eeGeneral.wavVolume = 2;
 296   g_eeGeneral.backgroundVolume = 1;
 297 #endif
 298
 299 #if defined(CPUARM)
 300   for (int i=0; i<NUM_STICKS; ++i) {
 301     g_eeGeneral.trainer.mix[i].mode = 2;
 302     g_eeGeneral.trainer.mix[i].srcChn = channel_order(i+1) - 1;
 303     g_eeGeneral.trainer.mix[i].studWeight = 100;
 304   }
 305 #endif
 306
 307 #if defined(PCBX9E)
 308   const int8_t defaultName[] = { 20, -1, -18, -1, -14, -9, -19 };
 309   memcpy(g_eeGeneral.bluetoothName, defaultName, sizeof(defaultName));
 310 #endif
 311
 312 #if !defined(EEPROM)
 313   strcpy(g_eeGeneral.currModelFilename, DEFAULT_MODEL_FILENAME);
 314 #endif
 315
 316 #if defined(PCBHORUS)
 317   strcpy(g_eeGeneral.themeName, theme->getName());
 318   theme->init();
 319 #endif
 320
 321   g_eeGeneral.chkSum = 0xFFFF;
 322 }
 323
 324 uint16_t evalChkSum()
 325 {
 326   uint16_t sum = 0;
 327   const int16_t *calibValues = (const int16_t *) &g_eeGeneral.calib[0];
 328   for (int i=0; i<12; i++)
 329     sum += calibValues[i];
 330   return sum;
 331 }
 332
 333 #if defined(VIRTUAL_INPUTS)
 334 void clearInputs()
 335 {
 336   memset(g_model.expoData, 0, sizeof(g_model.expoData)); // clear all expos
 337 }
 338
 339 void defaultInputs()
 340 {
 341   clearInputs();
 342
 343   for (int i=0; i<NUM_STICKS; i++) {
 344     uint8_t stick_index = channel_order(i+1);
 345     ExpoData *expo = expoAddress(i);
 346     expo->srcRaw = MIXSRC_Rud - 1 + stick_index;
 347     expo->curve.type = CURVE_REF_EXPO;
 348     expo->chn = i;
 349     expo->weight = 100;
 350     expo->mode = 3; // TODO constant
 351 #if defined(TRANSLATIONS_CZ)
 352     for (int c=0; c<4; c++) {
 353       g_model.inputNames[i][c] = char2idx(STR_INPUTNAMES[1+4*(stick_index-1)+c]);
 354     }
 355 #else
 356     for (int c=0; c<3; c++) {
 357       g_model.inputNames[i][c] = char2idx(STR_VSRCRAW[2+4*stick_index+c]);
 358     }
 359 #if LEN_INPUT_NAME > 3
 360     g_model.inputNames[i][3] = '\0';
 361 #endif
 362 #endif
 363   }
 364   storageDirty(EE_MODEL);
 365 }
 366 #endif
 367
 368 #if defined(TEMPLATES)
 369 inline void applyDefaultTemplate()
 370 {
 371   applyTemplate(TMPL_SIMPLE_4CH); // calls storageDirty internally
 372 }
 373 #else
 374 void applyDefaultTemplate()
 375 {
 376 #if defined(VIRTUAL_INPUTS)
 377   defaultInputs(); // calls storageDirty internally
 378 #else
 379   storageDirty(EE_MODEL);
 380 #endif
 381
 382   for (int i=0; i<NUM_STICKS; i++) {
 383     MixData * mix = mixAddress(i);
 384     mix->destCh = i;
 385     mix->weight = 100;
 386 #if defined(VIRTUAL_INPUTS)
 387     mix->srcRaw = i+1;
 388 #else
 389     mix->srcRaw = MIXSRC_Rud - 1 + channel_order(i+1);
 390 #endif
 391   }
 392 }
 393 #endif
 394
 395 #if defined(CPUARM) && defined(EEPROM)
 396 void checkModelIdUnique(uint8_t index, uint8_t module)
 397 {
 398   uint8_t modelId = g_model.header.modelId[module];
 399   uint8_t additionalOnes = 0;
 400   char * name = reusableBuffer.msgbuf.msg;
 401
 402   memset(reusableBuffer.msgbuf.msg, 0, sizeof(reusableBuffer.msgbuf.msg));
 403
 404   if (modelId != 0) {
 405     for (uint8_t i = 0; i < MAX_MODELS; i++) {
 406       if (i != index) {
 407         if (modelId == modelHeaders[i].modelId[module]) {
 408           if ((WARNING_LINE_LEN - 4 - (name - reusableBuffer.msgbuf.msg)) > (signed)(modelHeaders[i].name[0] ? zlen(modelHeaders[i].name, LEN_MODEL_NAME) : sizeof(TR_MODEL) + 2)) { // you cannot rely exactly on WARNING_LINE_LEN so using WARNING_LINE_LEN-2 (-2 for the ",")
 409             if (reusableBuffer.msgbuf.msg[0] != 0) {
 410               name = strAppend(name, ", ");
 411             }
 412             if (modelHeaders[i].name[0] == 0) {
 413               name = strAppend(name, STR_MODEL);
 414               name = strAppendUnsigned(name+strlen(name),i, 2);
 415             }
 416             else {
 417               name += zchar2str(name, modelHeaders[i].name, LEN_MODEL_NAME);
 418             }
 419           }
 420           else {
 421             additionalOnes++;
 422           }
 423         }
 424       }
 425     }
 426   }
 427
 428   if (additionalOnes) {
 429     name = strAppend(name, " (+");
 430     name = strAppendUnsigned(name, additionalOnes);
 431     name = strAppend(name, ")");
 432   }
 433
 434   if (reusableBuffer.msgbuf.msg[0] != 0) {
 435     POPUP_WARNING(STR_MODELIDUSED);
 436     SET_WARNING_INFO(reusableBuffer.msgbuf.msg, sizeof(reusableBuffer.msgbuf.msg), 0);
 437   }
 438 }
 439 #endif
 440
 441 void modelDefault(uint8_t id)
 442 {
 443   memset(&g_model, 0, sizeof(g_model));
 444
 445   applyDefaultTemplate();
 446
 447 #if defined(LUA) && defined(PCBTARANIS) //Horus uses menuModelWizard() for wizard
 448   if (isFileAvailable(WIZARD_PATH "/" WIZARD_NAME)) {
 449     f_chdir(WIZARD_PATH);
 450     luaExec(WIZARD_NAME);
 451   }
 452 #endif
 453
 454 #if defined(PCBTARANIS) || defined(PCBHORUS)
 455   g_model.moduleData[INTERNAL_MODULE].type = MODULE_TYPE_XJT;
 456   g_model.moduleData[INTERNAL_MODULE].channelsCount = DEFAULT_CHANNELS(INTERNAL_MODULE);
 457 #elif defined(PCBSKY9X)
 458   g_model.moduleData[EXTERNAL_MODULE].type = MODULE_TYPE_PPM;
 459 #endif
 460
 461 #if defined(CPUARM) && defined(EEPROM)
 462   for (int i=0; i<NUM_MODULES; i++) {
 463     modelHeaders[id].modelId[i] = g_model.header.modelId[i] = id+1;
 464   }
 465   checkModelIdUnique(id, 0);
 466 #endif
 467
 468 #if defined(CPUARM) && defined(FLIGHT_MODES) && defined(GVARS)
 469   for (int p=1; p<MAX_FLIGHT_MODES; p++) {
 470     for (int i=0; i<MAX_GVARS; i++) {
 471       g_model.flightModeData[p].gvars[i] = GVAR_MAX+1;
 472     }
 473   }
 474 #endif
 475
 476 #if defined(FLIGHT_MODES) && defined(ROTARY_ENCODERS)
 477   for (int p=1; p<MAX_FLIGHT_MODES; p++) {
 478     for (int i=0; i<ROTARY_ENCODERS; i++) {
 479       g_model.flightModeData[p].rotaryEncoders[i] = ROTARY_ENCODER_MAX+1;
 480     }
 481   }
 482 #endif
 483
 484 #if defined(TELEMETRY_MAVLINK)
 485   g_model.mavlink.rc_rssi_scale = 15;
 486   g_model.mavlink.pc_rssi_en = 1;
 487 #endif
 488
 489 #if !defined(EEPROM)
 490   strcpy(g_model.header.name, "\015\361\374\373\364");
 491   g_model.header.name[5] = '\033' + id/10;
 492   g_model.header.name[6] = '\033' + id%10;
 493 #endif
 494
 495 #if defined(PCBHORUS)
 496   extern const LayoutFactory * defaultLayout;
 497   delete customScreens[0];
 498   customScreens[0] = defaultLayout->create(&g_model.screenData[0].layoutData);
 499   strcpy(g_model.screenData[0].layoutName, "Layout2P1");
 500   extern const WidgetFactory * defaultWidget;
 501   customScreens[0]->createWidget(0, defaultWidget);
 502   // enable switch warnings
 503   for (int i=0; i<NUM_SWITCHES; i++) {
 504     g_model.switchWarningState |= (1 << (3*i));
 505   }
 506 #endif
 507 }
 508
 509 #if defined(VIRTUAL_INPUTS)
 510 bool isInputRecursive(int index)
 511 {
 512   ExpoData * line = expoAddress(0);
 513   for (int i=0; i<MAX_EXPOS; i++, line++) {
 514     if (line->chn > index)
 515       break;
 516     else if (line->chn < index)
 517       continue;
 518     else if (line->srcRaw >= MIXSRC_FIRST_LOGICAL_SWITCH)
 519       return true;
 520   }
 521   return false;
 522 }
 523 #endif
 524
 525 #if defined(AUTOSOURCE)
 526 int8_t getMovedSource(GET_MOVED_SOURCE_PARAMS)
 527 {
 528   int8_t result = 0;
 529   static tmr10ms_t s_move_last_time = 0;
 530
 531 #if defined(VIRTUAL_INPUTS)
 532   static int16_t inputsStates[MAX_INPUTS];
 533   if (min <= MIXSRC_FIRST_INPUT) {
 534     for (uint8_t i=0; i<MAX_INPUTS; i++) {
 535       if (abs(anas[i] - inputsStates[i]) > 512) {
 536         if (!isInputRecursive(i)) {
 537           result = MIXSRC_FIRST_INPUT+i;
 538           break;
 539         }
 540       }
 541     }
 542   }
 543 #endif
 544
 545   static int16_t sourcesStates[NUM_STICKS+NUM_POTS+NUM_SLIDERS+NUM_MOUSE_ANALOGS];
 546   if (result == 0) {
 547     for (uint8_t i=0; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; i++) {
 548       if (abs(calibratedAnalogs[i] - sourcesStates[i]) > 512) {
 549         result = MIXSRC_Rud+i;
 550         break;
 551       }
 552     }
 553   }
 554
 555   bool recent = ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 10);
 556   if (recent) {
 557     result = 0;
 558   }
 559
 560   if (result || recent) {
 561 #if defined(VIRTUAL_INPUTS)
 562     memcpy(inputsStates, anas, sizeof(inputsStates));
 563 #endif
 564     memcpy(sourcesStates, calibratedAnalogs, sizeof(sourcesStates));
 565   }
 566
 567   s_move_last_time = get_tmr10ms();
 568   return result;
 569 }
 570 #endif
 571
 572 #if defined(FLIGHT_MODES)
 573 uint8_t getFlightMode()
 574 {
 575   for (uint8_t i=1; i<MAX_FLIGHT_MODES; i++) {
 576     FlightModeData *phase = &g_model.flightModeData[i];
 577     if (phase->swtch && getSwitch(phase->swtch)) {
 578       return i;
 579     }
 580   }
 581   return 0;
 582 }
 583 #endif
 584
 585 trim_t getRawTrimValue(uint8_t phase, uint8_t idx)
 586 {
 587   FlightModeData * p = flightModeAddress(phase);
 588 #if defined(PCBSTD)
 589   return (((trim_t)p->trim[idx]) << 2) + ((p->trim_ext >> (2*idx)) & 0x03);
 590 #else
 591   return p->trim[idx];
 592 #endif
 593 }
 594
 595 int getTrimValue(uint8_t phase, uint8_t idx)
 596 {
 597 #if defined(CPUARM)
 598   int result = 0;
 599   for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
 600     trim_t v = getRawTrimValue(phase, idx);
 601     if (v.mode == TRIM_MODE_NONE) {
 602       return result;
 603     }
 604     else {
 605       unsigned int p = v.mode >> 1;
 606       if (p == phase || phase == 0) {
 607         return result + v.value;
 608       }
 609       else {
 610         phase = p;
 611         if (v.mode % 2 != 0) {
 612           result += v.value;
 613         }
 614       }
 615     }
 616   }
 617   return 0;
 618 #else
 619   return getRawTrimValue(getTrimFlightMode(phase, idx), idx);
 620 #endif
 621 }
 622
 623 #if defined(CPUARM)
 624 bool setTrimValue(uint8_t phase, uint8_t idx, int trim)
 625 {
 626   for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
 627     trim_t & v = flightModeAddress(phase)->trim[idx];
 628     if (v.mode == TRIM_MODE_NONE)
 629       return false;
 630     unsigned int p = v.mode >> 1;
 631     if (p == phase || phase == 0) {
 632       v.value = trim;
 633       break;
 634     }
 635     else if (v.mode % 2 == 0) {
 636       phase = p;
 637     }
 638     else {
 639       v.value = limit<int>(TRIM_EXTENDED_MIN, trim - getTrimValue(p, idx), TRIM_EXTENDED_MAX);
 640       break;
 641     }
 642   }
 643   storageDirty(EE_MODEL);
 644   return true;
 645 }
 646 #else
 647 void setTrimValue(uint8_t phase, uint8_t idx, int trim)
 648 {
 649 #if defined(PCBSTD)
 650   FlightModeData *p = flightModeAddress(phase);
 651   p->trim[idx] = (int8_t)(trim >> 2);
 652   idx <<= 1;
 653   p->trim_ext = (p->trim_ext & ~(0x03 << idx)) + (((trim & 0x03) << idx));
 654 #else
 655   FlightModeData *p = flightModeAddress(phase);
 656   p->trim[idx] = trim;
 657 #endif
 658   storageDirty(EE_MODEL);
 659 }
 660 #endif
 661
 662 #if !defined(CPUARM)
 663 uint8_t getTrimFlightMode(uint8_t phase, uint8_t idx)
 664 {
 665   for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
 666     if (phase == 0) return 0;
 667     trim_t trim = getRawTrimValue(phase, idx);
 668     if (trim <= TRIM_EXTENDED_MAX) return phase;
 669     uint8_t result = trim-TRIM_EXTENDED_MAX-1;
 670     if (result >= phase) result++;
 671     phase = result;
 672   }
 673   return 0;
 674 }
 675 #endif
 676
 677 #if defined(ROTARY_ENCODERS)
 678 uint8_t getRotaryEncoderFlightMode(uint8_t idx)
 679 {
 680   uint8_t phase = mixerCurrentFlightMode;
 681   for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
 682     if (phase == 0) return 0;
 683     int16_t value = flightModeAddress(phase)->rotaryEncoders[idx];
 684     if (value <= ROTARY_ENCODER_MAX) return phase;
 685     uint8_t result = value-ROTARY_ENCODER_MAX-1;
 686     if (result >= phase) result++;
 687     phase = result;
 688   }
 689   return 0;
 690 }
 691
 692 int16_t getRotaryEncoder(uint8_t idx)
 693 {
 694   return flightModeAddress(getRotaryEncoderFlightMode(idx))->rotaryEncoders[idx];
 695 }
 696
 697 void incRotaryEncoder(uint8_t idx, int8_t inc)
 698 {
 699   rotencValue[idx] += inc;
 700   int16_t *value = &(flightModeAddress(getRotaryEncoderFlightMode(idx))->rotaryEncoders[idx]);
 701   *value = limit((int16_t)-RESX, (int16_t)(*value + (inc * 8)), (int16_t)+RESX);
 702   storageDirty(EE_MODEL);
 703 }
 704 #endif
 705
 706 #if defined(CPUARM)
 707 getvalue_t convert16bitsTelemValue(source_t channel, ls_telemetry_value_t value)
 708 {
 709   return value;
 710 }
 711
 712 getvalue_t convert8bitsTelemValue(source_t channel, ls_telemetry_value_t value)
 713 {
 714   return value;
 715 }
 716
 717 #if defined(TELEMETRY_FRSKY)
 718 ls_telemetry_value_t minTelemValue(source_t channel)
 719 {
 720   return 0;
 721 }
 722
 723 ls_telemetry_value_t maxTelemValue(source_t channel)
 724 {
 725   return 30000;
 726 }
 727 #endif
 728
 729 ls_telemetry_value_t max8bitsTelemValue(source_t channel)
 730 {
 731   return 30000;
 732 }
 733
 734 #elif defined(TELEMETRY_FRSKY)
 735
 736 /*
 737 ls_telemetry_value_t minTelemValue(uint8_t channel)
 738 {
 739   switch (channel) {
 740     case TELEM_TIMER1:
 741     case TELEM_TIMER2:
 742       return -3600;
 743     case TELEM_ALT:
 744     case TELEM_MIN_ALT:
 745     case TELEM_MAX_ALT:
 746     case TELEM_GPSALT:
 747       return -500;
 748     case TELEM_T1:
 749     case TELEM_MAX_T1:
 750     case TELEM_T2:
 751     case TELEM_MAX_T2:
 752       return -30;
 753     case TELEM_ACCx:
 754     case TELEM_ACCy:
 755     case TELEM_ACCz:
 756       return -1000;
 757     case TELEM_VSPEED:
 758       return -3000;
 759     default:
 760       return 0;
 761   }
 762 }
 763 */
 764 ls_telemetry_value_t maxTelemValue(uint8_t channel)
 765 {
 766   switch (channel) {
 767     case TELEM_FUEL:
 768     case TELEM_RSSI_TX:
 769     case TELEM_RSSI_RX:
 770       return 100;
 771     case TELEM_HDG:
 772       return 180;
 773     default:
 774       return 255;
 775   }
 776 }
 777 #endif
 778
 779 #if !defined(CPUARM)
 780 getvalue_t convert8bitsTelemValue(uint8_t channel, ls_telemetry_value_t value)
 781 {
 782   getvalue_t result;
 783   switch (channel) {
 784     case TELEM_TIMER1:
 785     case TELEM_TIMER2:
 786       result = value * 5;
 787       break;
 788 #if defined(TELEMETRY_FRSKY)
 789     case TELEM_ALT:
 790     case TELEM_GPSALT:
 791     case TELEM_MAX_ALT:
 792     case TELEM_MIN_ALT:
 793       result = value * 8 - 500;
 794       break;
 795     case TELEM_RPM:
 796     case TELEM_MAX_RPM:
 797       result = value * 50;
 798       break;
 799     case TELEM_T1:
 800     case TELEM_T2:
 801     case TELEM_MAX_T1:
 802     case TELEM_MAX_T2:
 803       result = (getvalue_t)value - 30;
 804       break;
 805     case TELEM_CELL:
 806     case TELEM_HDG:
 807     case TELEM_SPEED:
 808     case TELEM_MAX_SPEED:
 809       result = value * 2;
 810       break;
 811     case TELEM_ASPEED:
 812     case TELEM_MAX_ASPEED:
 813       result = value * 20;
 814       break;
 815     case TELEM_DIST:
 816     case TELEM_MAX_DIST:
 817       result = value * 8;
 818       break;
 819     case TELEM_CURRENT:
 820     case TELEM_POWER:
 821     case TELEM_MAX_CURRENT:
 822     case TELEM_MAX_POWER:
 823       result = value * 5;
 824       break;
 825     case TELEM_CONSUMPTION:
 826       result = value * 100;
 827       break;
 828     case TELEM_VSPEED:
 829       result = ((getvalue_t)value - 125) * 10;
 830       break;
 831 #endif
 832     default:
 833       result = value;
 834       break;
 835   }
 836   return result;
 837 }
 838 #endif
 839
 840 #define INAC_STICKS_SHIFT   6
 841 #define INAC_SWITCHES_SHIFT 8
 842 bool inputsMoved()
 843 {
 844   uint8_t sum = 0;
 845   for (uint8_t i=0; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; i++)
 846     sum += anaIn(i) >> INAC_STICKS_SHIFT;
 847   for (uint8_t i=0; i<NUM_SWITCHES; i++)
 848     sum += getValue(MIXSRC_FIRST_SWITCH+i) >> INAC_SWITCHES_SHIFT;
 849
 850   if (abs((int8_t)(sum-inactivity.sum)) > 1) {
 851     inactivity.sum = sum;
 852     return true;
 853   }
 854   else {
 855     return false;
 856   }
 857 }
 858
 859 void checkBacklight()
 860 {
 861   static uint8_t tmr10ms ;
 862
 863 #if defined(PCBSTD) && defined(ROTARY_ENCODER_NAVIGATION)
 864   rotencPoll();
 865 #endif
 866
 867   uint8_t x = g_blinkTmr10ms;
 868   if (tmr10ms != x) {
 869     tmr10ms = x;
 870     if (inputsMoved()) {
 871       inactivity.counter = 0;
 872       if (g_eeGeneral.backlightMode & e_backlight_mode_sticks) {
 873         backlightOn();
 874       }
 875     }
 876
 877     bool backlightOn = (g_eeGeneral.backlightMode == e_backlight_mode_on || lightOffCounter || isFunctionActive(FUNCTION_BACKLIGHT));
 878     if (flashCounter) backlightOn = !backlightOn;
 879     if (backlightOn)
 880       BACKLIGHT_ENABLE();
 881     else
 882       BACKLIGHT_DISABLE();
 883
 884 #if defined(PCBSTD) && defined(VOICE) && !defined(SIMU)
 885     Voice.voice_process() ;
 886 #endif
 887   }
 888 }
 889
 890 void doLoopCommonActions()
 891 {
 892   checkBacklight();
 893 }
 894
 895 void backlightOn()
 896 {
 897   lightOffCounter = ((uint16_t)g_eeGeneral.lightAutoOff*250) << 1;
 898 }
 899
 900 #if MENUS_LOCK == 1
 901 bool readonly = true;
 902 bool readonlyUnlocked()
 903 {
 904   if (readonly) {
 905     POPUP_WARNING(STR_MODS_FORBIDDEN);
 906     return false;
 907   }
 908   else {
 909     return true;
 910   }
 911 }
 912 #endif
 913
 914 #if defined(SPLASH)
 915 void doSplash()
 916 {
 917 #if defined(PWR_BUTTON_PRESS)
 918   bool refresh = false;
 919 #endif
 920
 921   if (SPLASH_NEEDED()) {
 922     drawSplash();
 923
 924 #if !defined(CPUARM)
 925     AUDIO_HELLO();
 926 #endif
 927
 928 #if defined(PCBSTD)
 929     lcdSetContrast();
 930 #elif defined(PCBSKY9X)
 931     tmr10ms_t curTime = get_tmr10ms() + 10;
 932     uint8_t contrast = 10;
 933     lcdSetRefVolt(contrast);
 934 #endif
 935
 936     getADC(); // init ADC array
 937
 938     inputsMoved();
 939
 940     tmr10ms_t tgtime = get_tmr10ms() + SPLASH_TIMEOUT;
 941
 942     while (tgtime > get_tmr10ms()) {
 943 #if defined(SIMU)
 944       SIMU_SLEEP(1);
 945 #elif defined(CPUARM)
 946       CoTickDelay(1);
 947 #endif
 948
 949       getADC();
 950
 951 #if defined(FSPLASH)
 952       // Splash is forced, we can't skip it
 953       if (!(g_eeGeneral.splashMode & 0x04)) {
 954 #endif
 955
 956       if (keyDown() || inputsMoved()) return;
 957
 958 #if defined(FSPLASH)
 959       }
 960 #endif
 961
 962 #if defined(PWR_BUTTON_PRESS)
 963       uint32_t pwr_check = pwrCheck();
 964       if (pwr_check == e_power_off) {
 965         break;
 966       }
 967       else if (pwr_check == e_power_press) {
 968         refresh = true;
 969       }
 970       else if (pwr_check == e_power_on && refresh) {
 971         drawSplash();
 972         refresh = false;
 973       }
 974 #else
 975       if (pwrCheck() == e_power_off) {
 976         return;
 977       }
 978 #endif
 979
 980 #if defined(SPLASH_FRSKY)
 981       static uint8_t secondSplash = false;
 982       if (!secondSplash && get_tmr10ms() >= tgtime-200) {
 983         secondSplash = true;
 984         drawSecondSplash();
 985       }
 986 #endif
 987
 988 #if defined(PCBSKY9X)
 989       if (curTime < get_tmr10ms()) {
 990         curTime += 10;
 991         if (contrast < g_eeGeneral.contrast) {
 992           contrast += 1;
 993           lcdSetRefVolt(contrast);
 994         }
 995       }
 996 #endif
 997
 998       doLoopCommonActions();
 999     }
1000   }
1001 }
1002 #else
1003 #define Splash()
1004 #define doSplash()
1005 #endif
1006
1007 #if defined(SDCARD) && defined(CPUARM)
1008 void checkSDVersion()
1009 {
1010   if (sdMounted()) {
1011     FIL versionFile;
1012     UINT read = 0;
1013     char version[sizeof(REQUIRED_SDCARD_VERSION)-1];
1014     char error[sizeof(TR_WRONG_SDCARDVERSION)+ sizeof(version)];
1015
1016     strAppend(strAppend(error, STR_WRONG_SDCARDVERSION, sizeof(TR_WRONG_SDCARDVERSION)), REQUIRED_SDCARD_VERSION, sizeof(REQUIRED_SDCARD_VERSION));
1017     FRESULT result = f_open(&versionFile, "/opentx.sdcard.version", FA_OPEN_EXISTING | FA_READ);
1018     if (result == FR_OK) {
1019       if (f_read(&versionFile, &version, sizeof(version), &read) != FR_OK ||
1020           read != sizeof(version) ||
1021           strncmp(version, REQUIRED_SDCARD_VERSION, sizeof(version)) != 0) {
1022         TRACE("SD card version mismatch:  %.*s, %s", sizeof(REQUIRED_SDCARD_VERSION)-1, version, REQUIRED_SDCARD_VERSION);
1023         ALERT(STR_SD_CARD, error, AU_ERROR);
1024       }
1025       f_close(&versionFile);
1026     }
1027     else {
1028       ALERT(STR_SD_CARD, error, AU_ERROR);
1029     }
1030   }
1031 }
1032 #endif
1033
1034 #if defined(PCBTARANIS) || defined(PCBHORUS)
1035 void checkFailsafe()
1036 {
1037   for (int i=0; i<NUM_MODULES; i++) {
1038     if (IS_MODULE_PXX(i)) {
1039       ModuleData & moduleData = g_model.moduleData[i];
1040       if (HAS_RF_PROTOCOL_FAILSAFE(moduleData.rfProtocol) && moduleData.failsafeMode == FAILSAFE_NOT_SET) {
1041         ALERT(STR_FAILSAFEWARN, STR_NO_FAILSAFE, AU_ERROR);
1042         break;
1043       }
1044     }
1045   }
1046 }
1047 #else
1048 #define checkFailsafe()
1049 #endif
1050 #if defined(CPUARM)
1051 void checkRSSIAlarmsDisabled()
1052 {
1053   if (g_model.rssiAlarms.disabled) {
1054     ALERT(STR_RSSIALARM_WARN, STR_NO_RSSIALARM, AU_ERROR);
1055   }
1056 }
1057 #endif
1058
1059 #if defined(GUI)
1060 void checkAll()
1061 {
1062 #if defined(EEPROM_RLC)
1063   checkLowEEPROM();
1064 #endif
1065
1066 #if defined(MODULE_ALWAYS_SEND_PULSES)
1067   startupWarningState = STARTUP_WARNING_THROTTLE;
1068 #else
1069   if (g_eeGeneral.chkSum == evalChkSum()) {
1070     checkTHR();
1071   }
1072   checkSwitches();
1073   checkFailsafe();
1074 #endif
1075 #if defined(CPUARM)
1076   checkRSSIAlarmsDisabled();
1077 #endif
1078
1079 #if defined(SDCARD) && defined(CPUARM)
1080   checkSDVersion();
1081 #endif
1082
1083 #if defined(CPUARM)
1084   if (g_model.displayChecklist && modelHasNotes()) {
1085     pushModelNotes();
1086   }
1087 #endif
1088
1089 #if defined(CPUARM)
1090   if (!clearKeyEvents()) {
1091     showMessageBox(STR_KEYSTUCK);
1092     tmr10ms_t tgtime = get_tmr10ms() + 500;
1093     while (tgtime != get_tmr10ms()) {
1094 #if defined(SIMU)
1095       SIMU_SLEEP(1);
1096 #elif defined(CPUARM)
1097       CoTickDelay(1);
1098 #endif
1099       wdt_reset();
1100     }
1101   }
1102 #else    // #if defined(CPUARM)
1103   clearKeyEvents();
1104 #endif   // #if defined(CPUARM)
1105
1106   START_SILENCE_PERIOD();
1107 }
1108 #endif // GUI
1109
1110 #if defined(MODULE_ALWAYS_SEND_PULSES)
1111 void checkStartupWarnings()
1112 {
1113   if (startupWarningState < STARTUP_WARNING_DONE) {
1114     if (startupWarningState == STARTUP_WARNING_THROTTLE)
1115       checkTHR();
1116     else
1117       checkSwitches();
1118   }
1119 }
1120 #endif
1121
1122 #if defined(EEPROM_RLC)
1123 void checkLowEEPROM()
1124 {
1125   if (g_eeGeneral.disableMemoryWarning) return;
1126   if (EeFsGetFree() < 100) {
1127     ALERT(STR_STORAGE_WARNING, STR_EEPROMLOWMEM, AU_ERROR);
1128   }
1129 }
1130 #endif
1131
1132 void checkTHR()
1133 {
1134   uint8_t thrchn = ((g_model.thrTraceSrc==0) || (g_model.thrTraceSrc>NUM_POTS+NUM_SLIDERS)) ? THR_STICK : g_model.thrTraceSrc+NUM_STICKS-1;
1135   // throttle channel is either the stick according stick mode (already handled in evalInputs)
1136   // or P1 to P3;
1137   // in case an output channel is choosen as throttle source (thrTraceSrc>NUM_POTS+NUM_SLIDERS) we assume the throttle stick is the input
1138   // no other information available at the moment, and good enough to my option (otherwise too much exceptions...)
1139
1140 #if defined(MODULE_ALWAYS_SEND_PULSES)
1141   int16_t v = calibratedAnalogs[thrchn];
1142   if (v<=THRCHK_DEADBAND-1024 || g_model.disableThrottleWarning || pwrCheck()==e_power_off || keyDown()) {
1143     startupWarningState = STARTUP_WARNING_THROTTLE+1;
1144   }
1145   else {
1146     calibratedAnalogs[thrchn] = -1024;
1147 #if !defined(VIRTUAL_INPUTS)
1148     if (thrchn < NUM_STICKS) {
1149       rawAnas[thrchn] = anas[thrchn] = calibratedAnalogs[thrchn];
1150     }
1151 #endif
1152     RAISE_ALERT(STR_THROTTLEWARN, STR_THROTTLENOTIDLE, STR_PRESSANYKEYTOSKIP, AU_THROTTLE_ALERT);
1153   }
1154 #else
1155   if (g_model.disableThrottleWarning) {
1156     return;
1157   }
1158
1159   GET_ADC_IF_MIXER_NOT_RUNNING();
1160
1161   evalInputs(e_perout_mode_notrainer); // let do evalInputs do the job
1162
1163   int16_t v = calibratedAnalogs[thrchn];
1164   if (v <= THRCHK_DEADBAND-1024) {
1165     return; // prevent warning if throttle input OK
1166   }
1167
1168   // first - display warning; also deletes inputs if any have been before
1169   LED_ERROR_BEGIN();
1170   RAISE_ALERT(STR_THROTTLEWARN, STR_THROTTLENOTIDLE, STR_PRESSANYKEYTOSKIP, AU_THROTTLE_ALERT);
1171
1172 #if defined(PWR_BUTTON_PRESS)
1173   bool refresh = false;
1174 #endif
1175
1176   while (1) {
1177
1178     GET_ADC_IF_MIXER_NOT_RUNNING();
1179
1180     evalInputs(e_perout_mode_notrainer); // let do evalInputs do the job
1181
1182     v = calibratedAnalogs[thrchn];
1183
1184 #if defined(PWR_BUTTON_PRESS)
1185     uint32_t pwr_check = pwrCheck();
1186     if (pwr_check == e_power_off) {
1187       break;
1188     }
1189     else if (pwr_check == e_power_press) {
1190       refresh = true;
1191     }
1192     else if (pwr_check == e_power_on && refresh) {
1193       RAISE_ALERT(STR_THROTTLEWARN, STR_THROTTLENOTIDLE, STR_PRESSANYKEYTOSKIP, AU_NONE);
1194       refresh = false;
1195     }
1196 #else
1197     if (pwrCheck() == e_power_off) {
1198       break;
1199     }
1200 #endif
1201
1202     if (keyDown() || v <= THRCHK_DEADBAND-1024) {
1203       break;
1204     }
1205
1206     doLoopCommonActions();
1207
1208     wdt_reset();
1209
1210     SIMU_SLEEP(1);
1211 #if defined(CPUARM)
1212     CoTickDelay(10);
1213 #endif
1214
1215   }
1216 #endif
1217
1218   LED_ERROR_END();
1219 }
1220
1221 void checkAlarm() // added by Gohst
1222 {
1223   if (g_eeGeneral.disableAlarmWarning) {
1224     return;
1225   }
1226
1227   if (IS_SOUND_OFF()) {
1228     ALERT(STR_ALARMSWARN, STR_ALARMSDISABLED, AU_ERROR);
1229   }
1230 }
1231
1232 void alert(const pm_char * title, const pm_char * msg ALERT_SOUND_ARG)
1233 {
1234   LED_ERROR_BEGIN();
1235
1236   TRACE("ALERT %s: %s", title, msg);
1237
1238   RAISE_ALERT(title, msg, STR_PRESSANYKEY, sound);
1239
1240 #if defined(PWR_BUTTON_PRESS)
1241   bool refresh = false;
1242 #endif
1243
1244   while(1) {
1245     SIMU_SLEEP(1);
1246 #if defined(CPUARM)
1247     CoTickDelay(10);
1248 #endif
1249
1250     if (keyDown()) break; // wait for key release
1251
1252     doLoopCommonActions();
1253
1254     wdt_reset();
1255
1256 #if defined(PWR_BUTTON_PRESS)
1257     uint32_t pwr_check = pwrCheck();
1258     if (pwr_check == e_power_off) {
1259       boardOff();
1260     }
1261     else if (pwr_check == e_power_press) {
1262       refresh = true;
1263     }
1264     else if (pwr_check == e_power_on && refresh) {
1265       RAISE_ALERT(title, msg, STR_PRESSANYKEY, AU_NONE);
1266       refresh = false;
1267     }
1268 #else
1269     if (pwrCheck() == e_power_off) {
1270       boardOff(); // turn power off now
1271     }
1272 #endif
1273   }
1274
1275   LED_ERROR_END();
1276 }
1277
1278 #if defined(GVARS)
1279 int8_t trimGvar[NUM_STICKS+NUM_AUX_TRIMS] = { -1, -1, -1, -1 };
1280 #endif
1281
1282 #if defined(CPUARM)
1283 void checkTrims()
1284 {
1285   event_t event = getEvent(true);
1286   if (event && !IS_KEY_BREAK(event)) {
1287     int8_t k = EVT_KEY_MASK(event) - TRM_BASE;
1288 #else
1289 uint8_t checkTrim(event_t event)
1290 {
1291   int8_t k = EVT_KEY_MASK(event) - TRM_BASE;
1292   if (k>=0 && k<8 && !IS_KEY_BREAK(event)) {
1293 #endif
1294     // LH_DWN LH_UP LV_DWN LV_UP RV_DWN RV_UP RH_DWN RH_UP
1295     uint8_t idx = CONVERT_MODE((uint8_t)k/2);
1296     uint8_t phase;
1297     int before;
1298     bool thro;
1299
1300 #if defined(CPUARM)
1301     trimsDisplayTimer = 200; // 2 seconds
1302     trimsDisplayMask |= (1<<idx);
1303 #endif
1304
1305 #if defined(GVARS)
1306     if (TRIM_REUSED(idx)) {
1307 #if defined(PCBSTD)
1308       phase = 0;
1309 #else
1310       phase = getGVarFlightMode(mixerCurrentFlightMode, trimGvar[idx]);
1311 #endif
1312       before = GVAR_VALUE(trimGvar[idx], phase);
1313       thro = false;
1314     }
1315     else {
1316       phase = getTrimFlightMode(mixerCurrentFlightMode, idx);
1317 #if defined(CPUARM)
1318       before = getTrimValue(phase, idx);
1319 #else
1320       before = getRawTrimValue(phase, idx);
1321 #endif
1322       thro = (idx==THR_STICK && g_model.thrTrim);
1323     }
1324 #else
1325     phase = getTrimFlightMode(mixerCurrentFlightMode, idx);
1326 #if defined(CPUARM)
1327     before = getTrimValue(phase, idx);
1328 #else
1329     before = getRawTrimValue(phase, idx);
1330 #endif
1331     thro = (idx==THR_STICK && g_model.thrTrim);
1332 #endif
1333     int8_t trimInc = g_model.trimInc + 1;
1334     int8_t v = (trimInc==-1) ? min(32, abs(before)/4+1) : (1 << trimInc); // TODO flash saving if (trimInc < 0)
1335     if (thro) v = 4; // if throttle trim and trim trottle then step=4
1336     int16_t after = (k&1) ? before + v : before - v;   // positive = k&1
1337     bool beepTrim = false;
1338
1339     if (!thro && before!=0 && ((!(after < 0) == (before < 0)) || after==0)) { //forcing a stop at centerered trim when changing sides
1340       after = 0;
1341       beepTrim = true;
1342       AUDIO_TRIM_MIDDLE();
1343       pauseEvents(event);
1344     }
1345     else if (before>TRIM_MIN && after<=TRIM_MIN) {
1346       beepTrim = true;
1347       AUDIO_TRIM_MIN();
1348       killEvents(event);
1349     }
1350     else if (before<TRIM_MAX && after>=TRIM_MAX) {
1351       beepTrim = true;
1352       AUDIO_TRIM_MAX();
1353       killEvents(event);
1354     }
1355
1356     if ((before<after && after>TRIM_MAX) || (before>after && after<TRIM_MIN)) {
1357       if (!g_model.extendedTrims || TRIM_REUSED(idx)) after = before;
1358     }
1359
1360     if (after < TRIM_EXTENDED_MIN) {
1361       after = TRIM_EXTENDED_MIN;
1362     }
1363     if (after > TRIM_EXTENDED_MAX) {
1364       after = TRIM_EXTENDED_MAX;
1365     }
1366
1367 #if defined(GVARS)
1368     if (TRIM_REUSED(idx)) {
1369       SET_GVAR_VALUE(trimGvar[idx], phase, after);
1370     }
1371     else
1372 #endif
1373     {
1374 #if defined(CPUARM)
1375       if (!setTrimValue(phase, idx, after)) {
1376         // we don't play a beep, so we exit now the function
1377         return;
1378       }
1379 #else
1380       setTrimValue(phase, idx, after);
1381 #endif
1382     }
1383
1384     if (!beepTrim) {
1385       AUDIO_TRIM_PRESS(after);
1386     }
1387
1388 #if !defined(CPUARM)
1389     return 0;
1390 #endif
1391   }
1392 #if !defined(CPUARM)
1393   return event;
1394 #endif
1395 }
1396
1397 #if !defined(SIMU)
1398 uint16_t s_anaFilt[NUM_ANALOGS];
1399 #endif
1400
1401 #if defined(SIMU)
1402 uint16_t BandGap = 225;
1403 #elif defined(CPUM2560)
1404 // #define STARTADCONV (ADCSRA  = (1<<ADEN) | (1<<ADPS0) | (1<<ADPS1) | (1<<ADPS2) | (1<<ADSC) | (1 << ADIE))
1405 // G: Note that the above would have set the ADC prescaler to 128, equating to
1406 // 125KHz sample rate. We now sample at 500KHz, with oversampling and other
1407 // filtering options to produce 11-bit results.
1408 uint16_t BandGap = 2040;
1409 #elif defined(PCBSTD)
1410 uint16_t BandGap;
1411 #endif
1412
1413 #if defined(JITTER_MEASURE)
1414 JitterMeter<uint16_t> rawJitter[NUM_ANALOGS];
1415 JitterMeter<uint16_t> avgJitter[NUM_ANALOGS];
1416 tmr10ms_t jitterResetTime = 0;
1417 #endif
1418
1419 #if defined(VIRTUAL_INPUTS)
1420   #define JITTER_FILTER_STRENGTH  4         // tune this value, bigger value - more filtering (range: 1-5) (see explanation below)
1421   #define ANALOG_SCALE            1         // tune this value, bigger value - more filtering (range: 0-1) (see explanation below)
1422
1423   #define JITTER_ALPHA            (1<<JITTER_FILTER_STRENGTH)
1424   #define ANALOG_MULTIPLIER       (1<<ANALOG_SCALE)
1425   #define ANA_FILT(chan)          (s_anaFilt[chan] / (JITTER_ALPHA * ANALOG_MULTIPLIER))
1426   #if (JITTER_ALPHA * ANALOG_MULTIPLIER > 32)
1427     #error "JITTER_FILTER_STRENGTH and ANALOG_SCALE are too big, their summ should be <= 5 !!!"
1428   #endif
1429 #else
1430   #define ANALOG_SCALE            0
1431   #define JITTER_ALPHA            1
1432   #define ANALOG_MULTIPLIER       1
1433   #define ANA_FILT(chan)          (s_anaFilt[chan])
1434 #endif
1435
1436
1437 #if !defined(SIMU)
1438 uint16_t anaIn(uint8_t chan)
1439 {
1440 #if defined(VIRTUAL_INPUTS)
1441   return ANA_FILT(chan);
1442 #else
1443 #if defined(TELEMETRY_MOD_14051) || defined(TELEMETRY_MOD_14051_SWAPPED)
1444   static const pm_char crossAna[] PROGMEM = {3,1,2,0,4,5,6,0/* shouldn't be used */,TX_VOLTAGE};
1445 #else
1446   static const pm_char crossAna[] PROGMEM = {3,1,2,0,4,5,6,7};
1447 #endif
1448 #if defined(FRSKY_STICKS)
1449   volatile uint16_t temp = s_anaFilt[pgm_read_byte(crossAna+chan)];  // volatile saves here 40 bytes; maybe removed for newer AVR when available
1450   if (chan < NUM_STICKS && (g_eeGeneral.stickReverse & (1 << chan))) {
1451     temp = 2048 - temp;
1452   }
1453   return temp;
1454 #else
1455   volatile uint16_t *p = &s_anaFilt[pgm_read_byte(crossAna+chan)];
1456   return *p;
1457 #endif
1458 #endif
1459 }
1460
1461 #if defined(CPUARM)
1462 void getADC()
1463 {
1464 #if defined(JITTER_MEASURE)
1465   if (JITTER_MEASURE_ACTIVE() && jitterResetTime < get_tmr10ms()) {
1466     // reset jitter measurement every second
1467     for (uint32_t x=0; x<NUM_ANALOGS; x++) {
1468       rawJitter[x].reset();
1469       avgJitter[x].reset();
1470     }
1471     jitterResetTime = get_tmr10ms() + 100;  //every second
1472   }
1473 #endif
1474
1475   DEBUG_TIMER_START(debugTimerAdcRead);
1476   adcRead();
1477   DEBUG_TIMER_STOP(debugTimerAdcRead);
1478
1479   for (uint8_t x=0; x<NUM_ANALOGS; x++) {
1480     uint16_t v = getAnalogValue(x) >> (1 - ANALOG_SCALE);
1481
1482 #if defined(VIRTUAL_INPUTS)
1483     // Jitter filter:
1484     //    * pass trough any big change directly
1485     //    * for small change use Modified moving average (MMA) filter
1486     //
1487     // Explanation:
1488     //
1489     // Normal MMA filter has this formula:
1490     //            <out> = ((ALPHA-1)*<out> + <in>)/ALPHA
1491     //
1492     // If calculation is done this way with integer arithmetics, then any small change in
1493     // input signal is lost. One way to combat that, is to rearrange the formula somewhat,
1494     // to store a more precise (larger) number between iterations. The basic idea is to
1495     // store undivided value between iterations. Therefore an new variable <filtered> is
1496     // used. The new formula becomes:
1497     //           <filtered> = <filtered> - <filtered>/ALPHA + <in>
1498     //           <out> = <filtered>/ALPHA  (use only when out is needed)
1499     //
1500     // The above formula with a maximum allowed ALPHA value (we are limited by
1501     // the 16 bit s_anaFilt[]) was tested on the radio. The resulting signal still had
1502     // some jitter (a value of 1 was observed). The jitter might be bigger on other
1503     // radios.
1504     //
1505     // So another idea is to use larger input values for filtering. So instead of using
1506     // input in a range from 0 to 2047, we use twice larger number (temp[x] is divided less)
1507     //
1508     // This also means that ALPHA must be lowered (remember 16 bit limit), but test results
1509     // have proved that this kind of filtering gives better results. So the recommended values
1510     // for filter are:
1511     //     JITTER_FILTER_STRENGTH  4
1512     //     ANALOG_SCALE            1
1513     //
1514     // Variables mapping:
1515     //   * <in> = v
1516     //   * <out> = s_anaFilt[x]
1517     uint16_t previous = s_anaFilt[x] / JITTER_ALPHA;
1518     uint16_t diff = (v > previous) ? (v - previous) : (previous - v);
1519     if (!g_eeGeneral.jitterFilter && diff < (10*ANALOG_MULTIPLIER)) { // g_eeGeneral.jitterFilter is inverted, 0 - active
1520       // apply jitter filter
1521       s_anaFilt[x] = (s_anaFilt[x] - previous) + v;
1522     }
1523     else
1524 #endif  // #if defined(VIRTUAL_INPUTS)
1525     {
1526         //use unfiltered value
1527       s_anaFilt[x] = v * JITTER_ALPHA;
1528     }
1529
1530 #if defined(JITTER_MEASURE)
1531     if (JITTER_MEASURE_ACTIVE()) {
1532       avgJitter[x].measure(ANA_FILT(x));
1533     }
1534 #endif
1535
1536     #define ANAFILT_MAX    (2 * RESX * JITTER_ALPHA * ANALOG_MULTIPLIER - 1)
1537     StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[x];
1538     if (IS_POT_MULTIPOS(x) && IS_MULTIPOS_CALIBRATED(calib)) {
1539       // TODO: consider adding another low pass filter to eliminate multipos switching glitches
1540       uint8_t vShifted = ANA_FILT(x) >> 4;
1541       s_anaFilt[x] = ANAFILT_MAX;
1542       for (uint32_t i=0; i<calib->count; i++) {
1543         if (vShifted < calib->steps[i]) {
1544           s_anaFilt[x] = (i * ANAFILT_MAX) / calib->count;
1545           break;
1546         }
1547       }
1548     }
1549   }
1550 }
1551 #endif  // #if defined(CPUARM)
1552
1553 #endif // SIMU
1554
1555 uint8_t g_vbat100mV = 0;
1556 uint16_t lightOffCounter;
1557 uint8_t flashCounter = 0;
1558
1559 uint16_t sessionTimer;
1560 uint16_t s_timeCumThr;    // THR in 1/16 sec
1561 uint16_t s_timeCum16ThrP; // THR% in 1/16 sec
1562
1563 uint8_t  trimsCheckTimer = 0;
1564
1565 #if defined(CPUARM)
1566 uint8_t trimsDisplayTimer = 0;
1567 uint8_t trimsDisplayMask = 0;
1568 #endif
1569
1570 void flightReset(uint8_t check)
1571 {
1572   // we don't reset the whole audio here (the hello.wav would be cut, if a prompt is queued before FlightReset, it should be played)
1573   // TODO check if the vario / background music are stopped correctly if switching to a model which doesn't have these functions enabled
1574
1575   if (!IS_MANUAL_RESET_TIMER(0)) {
1576     timerReset(0);
1577   }
1578
1579 #if TIMERS > 1
1580   if (!IS_MANUAL_RESET_TIMER(1)) {
1581     timerReset(1);
1582   }
1583 #endif
1584
1585 #if TIMERS > 2
1586   if (!IS_MANUAL_RESET_TIMER(2)) {
1587     timerReset(2);
1588   }
1589 #endif
1590
1591 #if defined(TELEMETRY_FRSKY)
1592   telemetryReset();
1593 #endif
1594
1595   s_mixer_first_run_done = false;
1596
1597   START_SILENCE_PERIOD();
1598
1599   RESET_THR_TRACE();
1600
1601   logicalSwitchesReset();
1602
1603   if (check) {
1604     checkAll();
1605   }
1606 }
1607
1608 #if defined(THRTRACE)
1609 uint8_t  s_traceBuf[MAXTRACE];
1610 uint16_t s_traceWr;
1611 uint8_t  s_cnt_10s;
1612 uint16_t s_cnt_samples_thr_10s;
1613 uint16_t s_sum_samples_thr_10s;
1614 #endif
1615
1616 void evalTrims()
1617 {
1618   uint8_t phase = mixerCurrentFlightMode;
1619   for (uint8_t i=0; i<NUM_STICKS+NUM_AUX_TRIMS; i++) {
1620     // do trim -> throttle trim if applicable
1621     int16_t trim = getTrimValue(phase, i);
1622 #if !defined(CPUARM)
1623     if (i==THR_STICK && g_model.thrTrim) {
1624       int16_t trimMin = g_model.extendedTrims ? TRIM_EXTENDED_MIN : TRIM_MIN;
1625       trim = (((g_model.throttleReversed)?(int32_t)(trim+trimMin):(int32_t)(trim-trimMin)) * (RESX-anas[i])) >> (RESX_SHIFT+1);
1626     }
1627 #endif
1628     if (trimsCheckTimer > 0) {
1629       trim = 0;
1630     }
1631
1632     trims[i] = trim*2;
1633   }
1634 }
1635
1636 #if !defined(PCBSTD)
1637 uint8_t mSwitchDuration[1+NUM_ROTARY_ENCODERS] = { 0 };
1638 #define CFN_PRESSLONG_DURATION   100
1639 #endif
1640
1641
1642 uint8_t s_mixer_first_run_done = false;
1643
1644 void doMixerCalculations()
1645 {
1646   static tmr10ms_t lastTMR = 0;
1647
1648   tmr10ms_t tmr10ms = get_tmr10ms();
1649   uint8_t tick10ms = (tmr10ms >= lastTMR ? tmr10ms - lastTMR : 1);
1650   // handle tick10ms overrun
1651   // correct overflow handling costs a lot of code; happens only each 11 min;
1652   // therefore forget the exact calculation and use only 1 instead; good compromise
1653   lastTMR = tmr10ms;
1654
1655   DEBUG_TIMER_START(debugTimerGetAdc);
1656   getADC();
1657   DEBUG_TIMER_STOP(debugTimerGetAdc);
1658
1659   DEBUG_TIMER_START(debugTimerGetSwitches);
1660   getSwitchesPosition(!s_mixer_first_run_done);
1661   DEBUG_TIMER_STOP(debugTimerGetSwitches);
1662
1663 #if defined(PCBSKY9X) && !defined(REVA) && !defined(SIMU)
1664   Current_analogue = (Current_analogue*31 + s_anaFilt[8] ) >> 5 ;
1665   if (Current_analogue > Current_max)
1666     Current_max = Current_analogue ;
1667 #endif
1668
1669 #if !defined(CPUARM)
1670   adcPrepareBandgap();
1671 #endif
1672
1673   DEBUG_TIMER_START(debugTimerEvalMixes);
1674   evalMixes(tick10ms);
1675   DEBUG_TIMER_STOP(debugTimerEvalMixes);
1676
1677 #if !defined(CPUARM)
1678   // Bandgap has had plenty of time to settle...
1679   getADC_bandgap();
1680 #endif
1681
1682   DEBUG_TIMER_START(debugTimerMixes10ms);
1683   if (tick10ms) {
1684
1685 #if !defined(CPUM64) && !defined(ACCURAT_THROTTLE_TIMER)
1686     //  code cost is about 16 bytes for higher throttle accuracy for timer
1687     //  would not be noticable anyway, because all version up to this change had only 16 steps;
1688     //  now it has already 32  steps; this define would increase to 128 steps
1689     #define ACCURAT_THROTTLE_TIMER
1690 #endif
1691
1692     /* Throttle trace */
1693     int16_t val;
1694
1695     if (g_model.thrTraceSrc > NUM_POTS+NUM_SLIDERS) {
1696       uint8_t ch = g_model.thrTraceSrc-NUM_POTS-NUM_SLIDERS-1;
1697       val = channelOutputs[ch];
1698
1699       LimitData *lim = limitAddress(ch);
1700       int16_t gModelMax = LIMIT_MAX_RESX(lim);
1701       int16_t gModelMin = LIMIT_MIN_RESX(lim);
1702
1703       if (lim->revert)
1704         val = -val + gModelMax;
1705       else
1706         val = val - gModelMin;
1707
1708 #if defined(PPM_LIMITS_SYMETRICAL)
1709       if (lim->symetrical) {
1710         val -= calc1000toRESX(lim->offset);
1711       }
1712 #endif
1713
1714       gModelMax -= gModelMin; // we compare difference between Max and Mix for recaling needed; Max and Min are shifted to 0 by default
1715       // usually max is 1024 min is -1024 --> max-min = 2048 full range
1716
1717 #ifdef ACCURAT_THROTTLE_TIMER
1718       if (gModelMax!=0 && gModelMax!=2048) val = (int32_t) (val << 11) / (gModelMax); // rescaling only needed if Min, Max differs
1719 #else
1720       // @@@ open.20.fsguruh  optimized calculation; now *8 /8 instead of 10 base; (*16/16 already cause a overrun; unsigned calculation also not possible, because v may be negative)
1721       gModelMax+=255; // force rounding up --> gModelMax is bigger --> val is smaller
1722       gModelMax >>= (10-2);
1723
1724       if (gModelMax!=0 && gModelMax!=8) {
1725         val = (val << 3) / gModelMax; // rescaling only needed if Min, Max differs
1726       }
1727 #endif
1728
1729       if (val<0) val=0;  // prevent val be negative, which would corrupt throttle trace and timers; could occur if safetyswitch is smaller than limits
1730     }
1731     else {
1732 #if defined(VIRTUAL_INPUTS)
1733       val = RESX + calibratedAnalogs[g_model.thrTraceSrc == 0 ? THR_STICK : g_model.thrTraceSrc+NUM_STICKS-1];
1734 #else
1735       val = RESX + (g_model.thrTraceSrc == 0 ? rawAnas[THR_STICK] : calibratedAnalogs[g_model.thrTraceSrc+NUM_STICKS-1]);
1736 #endif
1737     }
1738
1739 #if defined(ACCURAT_THROTTLE_TIMER)
1740     val >>= (RESX_SHIFT-6); // calibrate it (resolution increased by factor 4)
1741 #else
1742     val >>= (RESX_SHIFT-4); // calibrate it
1743 #endif
1744
1745     evalTimers(val, tick10ms);
1746
1747     static uint8_t  s_cnt_100ms;
1748     static uint8_t  s_cnt_1s;
1749     static uint8_t  s_cnt_samples_thr_1s;
1750     static uint16_t s_sum_samples_thr_1s;
1751
1752     s_cnt_samples_thr_1s++;
1753     s_sum_samples_thr_1s+=val;
1754
1755     if ((s_cnt_100ms += tick10ms) >= 10) { // 0.1sec
1756       s_cnt_100ms -= 10;
1757       s_cnt_1s += 1;
1758
1759       logicalSwitchesTimerTick();
1760       checkTrainerSignalWarning();
1761
1762       if (s_cnt_1s >= 10) { // 1sec
1763         s_cnt_1s -= 10;
1764         sessionTimer += 1;
1765
1766         struct t_inactivity *ptrInactivity = &inactivity;
1767         FORCE_INDIRECT(ptrInactivity) ;
1768         ptrInactivity->counter++;
1769         if ((((uint8_t)ptrInactivity->counter)&0x07)==0x01 && g_eeGeneral.inactivityTimer && g_vbat100mV>50 && ptrInactivity->counter > ((uint16_t)g_eeGeneral.inactivityTimer*60))
1770           AUDIO_INACTIVITY();
1771
1772 #if defined(AUDIO)
1773         if (mixWarning & 1) if ((sessionTimer&0x03)==0) AUDIO_MIX_WARNING(1);
1774         if (mixWarning & 2) if ((sessionTimer&0x03)==1) AUDIO_MIX_WARNING(2);
1775         if (mixWarning & 4) if ((sessionTimer&0x03)==2) AUDIO_MIX_WARNING(3);
1776 #endif
1777
1778 #if defined(ACCURAT_THROTTLE_TIMER)
1779         val = s_sum_samples_thr_1s / s_cnt_samples_thr_1s;
1780         s_timeCum16ThrP += (val>>3);  // s_timeCum16ThrP would overrun if we would store throttle value with higher accuracy; therefore stay with 16 steps
1781         if (val) s_timeCumThr += 1;
1782         s_sum_samples_thr_1s>>=2;  // correct better accuracy now, because trace graph can show this information; in case thrtrace is not active, the compile should remove this
1783 #else
1784         val = s_sum_samples_thr_1s / s_cnt_samples_thr_1s;
1785         s_timeCum16ThrP += (val>>1);
1786         if (val) s_timeCumThr += 1;
1787 #endif
1788
1789 #if defined(THRTRACE)
1790         // throttle trace is done every 10 seconds; Tracebuffer is adjusted to screen size.
1791         // in case buffer runs out, it wraps around
1792         // resolution for y axis is only 32, therefore no higher value makes sense
1793         s_cnt_samples_thr_10s += s_cnt_samples_thr_1s;
1794         s_sum_samples_thr_10s += s_sum_samples_thr_1s;
1795
1796         if (++s_cnt_10s >= 10) { // 10s
1797           s_cnt_10s -= 10;
1798           val = s_sum_samples_thr_10s / s_cnt_samples_thr_10s;
1799           s_sum_samples_thr_10s = 0;
1800           s_cnt_samples_thr_10s = 0;
1801           s_traceBuf[s_traceWr % MAXTRACE] = val;
1802           s_traceWr++;
1803         }
1804 #endif
1805
1806         s_cnt_samples_thr_1s = 0;
1807         s_sum_samples_thr_1s = 0;
1808       }
1809     }
1810
1811 #if defined(PXX) || defined(DSM2)
1812     static uint8_t countRangecheck = 0;
1813     for (uint8_t i=0; i<NUM_MODULES; ++i) {
1814 #if defined(MULTIMODULE)
1815       if (moduleFlag[i] != MODULE_NORMAL_MODE || (i == EXTERNAL_MODULE && multiModuleStatus.isBinding())) {
1816 #else
1817       if (moduleFlag[i] != MODULE_NORMAL_MODE) {
1818 #endif
1819         if (++countRangecheck >= 250) {
1820           countRangecheck = 0;
1821           AUDIO_PLAY(AU_SPECIAL_SOUND_CHEEP);
1822         }
1823       }
1824     }
1825 #endif
1826
1827 #if defined(CPUARM)
1828     checkTrims();
1829 #endif
1830   }
1831   DEBUG_TIMER_STOP(debugTimerMixes10ms);
1832
1833   s_mixer_first_run_done = true;
1834 }
1835
1836 #if defined(NAVIGATION_STICKS)
1837 uint8_t StickScrollAllowed;
1838 uint8_t StickScrollTimer;
1839 static const pm_uint8_t rate[] PROGMEM = { 0, 0, 100, 40, 16, 7, 3, 1 } ;
1840
1841 uint8_t calcStickScroll( uint8_t index )
1842 {
1843   uint8_t direction;
1844   int8_t value;
1845
1846   if ( ( g_eeGeneral.stickMode & 1 ) == 0 )
1847     index ^= 3;
1848
1849   value = calibratedAnalogs[index] / 128;
1850   direction = value > 0 ? 0x80 : 0;
1851   if (value < 0)
1852     value = -value;                        // (abs)
1853   if (value > 7)
1854     value = 7;
1855   value = pgm_read_byte(rate+(uint8_t)value);
1856   if (value)
1857     StickScrollTimer = STICK_SCROLL_TIMEOUT;               // Seconds
1858   return value | direction;
1859 }
1860 #endif
1861
1862 #if defined(CPUARM)
1863   #define OPENTX_START_ARGS            uint8_t splash=true
1864   #define OPENTX_START_SPLASH_NEEDED() (splash)
1865 #else
1866   #define OPENTX_START_ARGS
1867   #define OPENTX_START_SPLASH_NEEDED() true
1868 #endif
1869
1870 void opentxStart(OPENTX_START_ARGS)
1871 {
1872   TRACE("opentxStart");
1873
1874 #if defined(SIMU)
1875   if (main_thread_running == 2) {
1876     return;
1877   }
1878 #endif
1879
1880   uint8_t calibration_needed = (g_eeGeneral.chkSum != evalChkSum());
1881
1882 #if defined(GUI)
1883   if (!calibration_needed && OPENTX_START_SPLASH_NEEDED()) {
1884     doSplash();
1885   }
1886 #endif
1887
1888 #if defined(DEBUG_TRACE_BUFFER)
1889   trace_event(trace_start, 0x12345678);
1890 #endif
1891
1892 #if defined(PCBSKY9X) && defined(SDCARD) && !defined(SIMU)
1893   for (int i=0; i<500 && !Card_initialized; i++) {
1894     CoTickDelay(1);  // 2ms
1895   }
1896 #endif
1897
1898 #if defined(NIGHTLY_BUILD_WARNING)
1899   ALERT(STR_NIGHTLY_WARNING, TR_NIGHTLY_NOTSAFE, AU_ERROR);
1900 #endif
1901
1902 #if defined(GUI)
1903   if (calibration_needed) {
1904     chainMenu(menuFirstCalib);
1905   }
1906   else {
1907     checkAlarm();
1908     checkAll();
1909     PLAY_MODEL_NAME();
1910   }
1911 #endif
1912 }
1913
1914 #if defined(CPUARM) || defined(CPUM2560)
1915 void opentxClose(uint8_t shutdown)
1916 {
1917   TRACE("opentxClose");
1918
1919   if (shutdown) {
1920 #if defined(CPUARM)
1921     watchdogSuspend(2000/*20s*/);
1922 #endif
1923     pausePulses();   // stop mixer task to disable trims processing while in shutdown
1924     AUDIO_BYE();
1925 #if defined(TELEMETRY_FRSKY)
1926     // TODO needed? telemetryEnd();
1927 #endif
1928 #if defined(LUA)
1929     luaClose(&lsScripts);
1930 #if defined(PCBHORUS)
1931     luaClose(&lsWidgets);
1932 #endif
1933 #endif
1934 #if defined(HAPTIC)
1935     hapticOff();
1936 #endif
1937   }
1938
1939 #if defined(SDCARD)
1940   logsClose();
1941 #endif
1942
1943   storageFlushCurrentModel();
1944
1945 #if defined(CPUARM) && !defined(REVA)
1946   if (sessionTimer > 0) {
1947     g_eeGeneral.globalTimer += sessionTimer;
1948     sessionTimer = 0;
1949   }
1950 #endif
1951
1952 #if defined(PCBSKY9X)
1953   uint32_t mAhUsed = g_eeGeneral.mAhUsed + Current_used * (488 + g_eeGeneral.txCurrentCalibration) / 8192 / 36;
1954   if (g_eeGeneral.mAhUsed != mAhUsed) {
1955     g_eeGeneral.mAhUsed = mAhUsed;
1956   }
1957 #endif
1958
1959   g_eeGeneral.unexpectedShutdown = 0;
1960   storageDirty(EE_GENERAL);
1961   storageCheck(true);
1962
1963 #if defined(CPUARM)
1964   while (IS_PLAYING(ID_PLAY_PROMPT_BASE + AU_BYE)) {
1965     CoTickDelay(10);
1966   }
1967   CoTickDelay(50);
1968 #endif
1969
1970 #if defined(SDCARD)
1971   sdDone();
1972 #endif
1973 }
1974 #endif
1975
1976 #if defined(STM32)
1977 void opentxResume()
1978 {
1979   TRACE("opentxResume");
1980
1981   menuHandlers[0] = menuMainView;
1982
1983   sdMount();
1984   storageReadAll();
1985 #if defined(PCBHORUS)
1986   loadTheme();
1987   loadFontCache();
1988 #endif
1989
1990   opentxStart(false);
1991
1992 #if defined(CPUARM)
1993   referenceSystemAudioFiles();
1994 #endif
1995
1996 #if defined(CPUARM) || defined(CPUM2560)
1997   if (!g_eeGeneral.unexpectedShutdown) {
1998     g_eeGeneral.unexpectedShutdown = 1;
1999     storageDirty(EE_GENERAL);
2000   }
2001 #endif
2002 }
2003 #endif
2004
2005 #if defined(NAVIGATION_STICKS)
2006 uint8_t getSticksNavigationEvent()
2007 {
2008   uint8_t evt = 0;
2009   if (StickScrollAllowed) {
2010     if ( StickScrollTimer ) {
2011       static uint8_t repeater;
2012       uint8_t direction;
2013       uint8_t value;
2014
2015       if ( repeater < 128 )
2016       {
2017         repeater += 1;
2018       }
2019       value = calcStickScroll( 2 );
2020       direction = value & 0x80;
2021       value &= 0x7F;
2022       if ( value )
2023       {
2024         if ( repeater > value )
2025         {
2026           repeater = 0;
2027           if ( evt == 0 )
2028           {
2029             if ( direction )
2030             {
2031               evt = EVT_KEY_FIRST(KEY_UP);
2032             }
2033             else
2034             {
2035               evt = EVT_KEY_FIRST(KEY_DOWN);
2036             }
2037           }
2038         }
2039       }
2040       else
2041       {
2042         value = calcStickScroll( 3 );
2043         direction = value & 0x80;
2044         value &= 0x7F;
2045         if ( value )
2046         {
2047           if ( repeater > value )
2048           {
2049             repeater = 0;
2050             if ( evt == 0 )
2051             {
2052               if ( direction )
2053               {
2054                 evt = EVT_KEY_FIRST(KEY_RIGHT);
2055               }
2056               else
2057               {
2058                 evt = EVT_KEY_FIRST(KEY_LEFT);
2059               }
2060             }
2061           }
2062         }
2063       }
2064     }
2065   }
2066   else {
2067     StickScrollTimer = 0;          // Seconds
2068   }
2069   StickScrollAllowed = 1 ;
2070   return evt;
2071 }
2072 #endif
2073
2074 #if !defined(CPUARM)
2075 void checkBattery()
2076 {
2077   static uint8_t counter = 0;
2078 #if defined(GUI) && !defined(COLORLCD)
2079   // TODO not the right menu I think ...
2080   if (menuHandlers[menuLevel] == menuRadioDiagAnalogs) {
2081     g_vbat100mV = 0;
2082     counter = 0;
2083   }
2084 #endif
2085   if (counter-- == 0) {
2086     counter = 10;
2087     int32_t instant_vbat = anaIn(TX_VOLTAGE);
2088 #if defined(CPUM2560)
2089     instant_vbat = (instant_vbat*1112 + instant_vbat*g_eeGeneral.txVoltageCalibration + (BandGap<<2)) / (BandGap<<3);
2090 #else
2091     instant_vbat = (instant_vbat*16 + instant_vbat*g_eeGeneral.txVoltageCalibration/8) / BandGap;
2092 #endif
2093
2094     static uint8_t  s_batCheck;
2095     static uint16_t s_batSum;
2096
2097 #if defined(VOICE)
2098     s_batCheck += 8;
2099 #else
2100     s_batCheck += 32;
2101 #endif
2102
2103     s_batSum += instant_vbat;
2104
2105     if (g_vbat100mV == 0) {
2106       g_vbat100mV = instant_vbat;
2107       s_batSum = 0;
2108       s_batCheck = 0;
2109     }
2110 #if defined(VOICE)
2111     else if (!(s_batCheck & 0x3f)) {
2112 #else
2113     else if (s_batCheck == 0) {
2114 #endif
2115       g_vbat100mV = s_batSum / 8;
2116       s_batSum = 0;
2117 #if defined(VOICE)
2118       if (s_batCheck != 0) {
2119         // no alarms
2120       }
2121       else
2122 #endif
2123       if (IS_TXBATT_WARNING() && g_vbat100mV>50) {
2124         AUDIO_TX_BATTERY_LOW();
2125       }
2126     }
2127   }
2128 }
2129 #endif // #if !defined(CPUARM)
2130
2131
2132 #if !defined(SIMU) && !defined(CPUARM)
2133
2134 volatile uint8_t g_tmr16KHz; //continuous timer 16ms (16MHz/1024/256) -- 8-bit counter overflow
2135 ISR(TIMER_16KHZ_VECT, ISR_NOBLOCK)
2136 {
2137   g_tmr16KHz++; // gruvin: Not 16KHz. Overflows occur at 61.035Hz (1/256th of 15.625KHz)
2138                 // to give *16.384ms* intervals. Kind of matters for accuracy elsewhere. ;)
2139                 // g_tmr16KHz is used to software-construct a 16-bit timer
2140                 // from TIMER-0 (8-bit). See getTmr16KHz, below.
2141 }
2142
2143 uint16_t getTmr16KHz()
2144 {
2145   while(1){
2146     uint8_t hb  = g_tmr16KHz;
2147     uint8_t lb  = COUNTER_16KHZ;
2148     if(hb-g_tmr16KHz==0) return (hb<<8)|lb;
2149   }
2150 }
2151
2152 #if defined(PCBSTD) && (defined(AUDIO) || defined(VOICE))
2153 // Clocks every 128 uS
2154 ISR(TIMER_AUDIO_VECT, ISR_NOBLOCK)
2155 {
2156   cli();
2157   PAUSE_AUDIO_INTERRUPT(); // stop reentrance
2158   sei();
2159
2160 #if defined(AUDIO)
2161   AUDIO_DRIVER();
2162 #endif
2163
2164 #if defined(VOICE)
2165   VOICE_DRIVER();
2166 #endif
2167
2168   cli();
2169   RESUME_AUDIO_INTERRUPT();
2170   sei();
2171 }
2172 #endif
2173
2174 // Clocks every 10ms
2175 ISR(TIMER_10MS_VECT, ISR_NOBLOCK)
2176 {
2177   // without correction we are 0,16% too fast; that mean in one hour we are 5,76Sek too fast; we do not like that
2178   static uint8_t accuracyWarble; // because 16M / 1024 / 100 = 156.25. we need to correct the fault; no start value needed
2179
2180 #if defined(AUDIO)
2181   AUDIO_HEARTBEAT();
2182 #endif
2183
2184 #if defined(BUZZER)
2185   BUZZER_HEARTBEAT();
2186 #endif
2187
2188 #if defined(HAPTIC)
2189   HAPTIC_HEARTBEAT();
2190 #endif
2191
2192   per10ms();
2193
2194   uint8_t bump = (!(++accuracyWarble & 0x03)) ? 157 : 156;
2195   TIMER_10MS_COMPVAL += bump;
2196 }
2197
2198 // Timer3 used for PPM_IN pulse width capture. Counter running at 16MHz / 8 = 2MHz
2199 // equating to one count every half millisecond. (2 counts = 1ms). Control channel
2200 // count delta values thus can range from about 1600 to 4400 counts (800us to 2200us),
2201 // corresponding to a PPM signal in the range 0.8ms to 2.2ms (1.5ms at center).
2202 // (The timer is free-running and is thus not reset to zero at each capture interval.)
2203 ISR(TIMER3_CAPT_vect) // G: High frequency noise can cause stack overflo with ISR_NOBLOCK
2204 {
2205   uint16_t capture=ICR3;
2206
2207   // Prevent rentrance for this IRQ only
2208   PAUSE_PPMIN_INTERRUPT();
2209   sei(); // enable other interrupts
2210
2211   captureTrainerPulses(capture);
2212
2213   cli(); // disable other interrupts for stack pops before this function's RETI
2214   RESUME_PPMIN_INTERRUPT();
2215 }
2216 #endif
2217
2218 #if defined(DSM2_SERIAL) && !defined(CPUARM)
2219 FORCEINLINE void DSM2_USART_vect()
2220 {
2221   UDR0 = *((uint16_t*)pulses2MHzRPtr); // transmit next byte
2222
2223   pulses2MHzRPtr += sizeof(uint16_t);
2224
2225   if (pulses2MHzRPtr == pulses2MHzWPtr) { // if reached end of DSM2 data buffer ...
2226     UCSRB_N(TLM_USART) &= ~(1 << UDRIE_N(TLM_USART)); // disable UDRE interrupt
2227   }
2228 }
2229 #endif
2230
2231 #if !defined(SIMU) && !defined(CPUARM)
2232
2233 #if defined(TELEMETRY_FRSKY)
2234
2235 FORCEINLINE void FRSKY_USART_vect()
2236 {
2237   if (frskyTxBufferCount > 0) {
2238     UDR_N(TLM_USART) = frskyTxBuffer[--frskyTxBufferCount];
2239   }
2240   else {
2241     UCSRB_N(TLM_USART) &= ~(1 << UDRIE_N(TLM_USART)); // disable UDRE interrupt
2242   }
2243 }
2244
2245 // USART0/1 Transmit Data Register Emtpy ISR
2246 ISR(USART_UDRE_vect_N(TLM_USART))
2247 {
2248 #if defined(TELEMETRY_FRSKY) && defined(DSM2_SERIAL)
2249   if (IS_DSM2_PROTOCOL(g_model.protocol)) { // TODO not s_current_protocol?
2250     DSM2_USART_vect();
2251   }
2252   else {
2253     FRSKY_USART_vect();
2254   }
2255 #elif defined(TELEMETRY_FRSKY)
2256   FRSKY_USART_vect();
2257 #else
2258   DSM2_USART_vect();
2259 #endif
2260 }
2261 #endif
2262 #endif
2263
2264 #if defined(CPUARM)
2265   #define INSTANT_TRIM_MARGIN 10 /* around 1% */
2266 #else
2267   #define INSTANT_TRIM_MARGIN 15 /* around 1.5% */
2268 #endif
2269
2270 void instantTrim()
2271 {
2272 #if defined(VIRTUAL_INPUTS)
2273   int16_t  anas_0[NUM_INPUTS];
2274   evalInputs(e_perout_mode_notrainer | e_perout_mode_nosticks);
2275   memcpy(anas_0, anas, sizeof(anas_0));
2276 #endif
2277
2278   evalInputs(e_perout_mode_notrainer);
2279
2280   for (uint8_t stick=0; stick<NUM_STICKS; stick++) {
2281     if (stick!=THR_STICK) {
2282       // don't instant trim the throttle stick
2283       uint8_t trim_phase = getTrimFlightMode(mixerCurrentFlightMode, stick);
2284 #if defined(VIRTUAL_INPUTS)
2285       int16_t delta = 0;
2286       for (int e=0; e<MAX_EXPOS; e++) {
2287         ExpoData * ed = expoAddress(e);
2288         if (!EXPO_VALID(ed)) break; // end of list
2289         if (ed->srcRaw-MIXSRC_Rud == stick) {
2290           delta = anas[ed->chn] - anas_0[ed->chn];
2291           break;
2292         }
2293       }
2294 #else
2295       int16_t delta = anas[stick];
2296 #endif
2297       if (abs(delta) >= INSTANT_TRIM_MARGIN) {
2298         int16_t trim = limit<int16_t>(TRIM_EXTENDED_MIN, (delta + trims[stick]) / 2, TRIM_EXTENDED_MAX);
2299         setTrimValue(trim_phase, stick, trim);
2300       }
2301     }
2302   }
2303
2304   storageDirty(EE_MODEL);
2305   AUDIO_WARNING2();
2306 }
2307
2308 void copySticksToOffset(uint8_t ch)
2309 {
2310   pauseMixerCalculations();
2311   int32_t zero = (int32_t)channelOutputs[ch];
2312
2313   evalFlightModeMixes(e_perout_mode_nosticks+e_perout_mode_notrainer, 0);
2314   int32_t val = chans[ch];
2315   LimitData *ld = limitAddress(ch);
2316   limit_min_max_t lim = LIMIT_MIN(ld);
2317   if (val < 0) {
2318     val = -val;
2319     lim = LIMIT_MIN(ld);
2320   }
2321 #if defined(CPUARM)
2322   zero = (zero*256000 - val*lim) / (1024*256-val);
2323 #else
2324   zero = (zero*25600 - val*lim) / (26214-val);
2325 #endif
2326   ld->offset = (ld->revert ? -zero : zero);
2327   resumeMixerCalculations();
2328   storageDirty(EE_MODEL);
2329 }
2330
2331 void copyTrimsToOffset(uint8_t ch)
2332 {
2333   int16_t zero;
2334
2335   pauseMixerCalculations();
2336
2337   evalFlightModeMixes(e_perout_mode_noinput, 0); // do output loop - zero input sticks and trims
2338   zero = applyLimits(ch, chans[ch]);
2339
2340   evalFlightModeMixes(e_perout_mode_noinput-e_perout_mode_notrims, 0); // do output loop - only trims
2341
2342   int16_t output = applyLimits(ch, chans[ch]) - zero;
2343   int16_t v = g_model.limitData[ch].offset;
2344   if (g_model.limitData[ch].revert) output = -output;
2345 #if defined(CPUARM)
2346   v += (output * 125) / 128;
2347 #else
2348   v += output;
2349 #endif
2350   g_model.limitData[ch].offset = limit((int16_t)-1000, (int16_t)v, (int16_t)1000); // make sure the offset doesn't go haywire
2351
2352   resumeMixerCalculations();
2353   storageDirty(EE_MODEL);
2354 }
2355
2356 void moveTrimsToOffsets() // copy state of 3 primary to subtrim
2357 {
2358   int16_t zeros[MAX_OUTPUT_CHANNELS];
2359
2360   pauseMixerCalculations();
2361
2362   evalFlightModeMixes(e_perout_mode_noinput, 0); // do output loop - zero input sticks and trims
2363   for (uint8_t i=0; i<MAX_OUTPUT_CHANNELS; i++) {
2364     zeros[i] = applyLimits(i, chans[i]);
2365   }
2366
2367   evalFlightModeMixes(e_perout_mode_noinput-e_perout_mode_notrims, 0); // do output loop - only trims
2368
2369   for (uint8_t i=0; i<MAX_OUTPUT_CHANNELS; i++) {
2370     int16_t output = applyLimits(i, chans[i]) - zeros[i];
2371     int16_t v = g_model.limitData[i].offset;
2372     if (g_model.limitData[i].revert) output = -output;
2373 #if defined(CPUARM)
2374     v += (output * 125) / 128;
2375 #else
2376     v += output;
2377 #endif
2378     g_model.limitData[i].offset = limit((int16_t)-1000, (int16_t)v, (int16_t)1000); // make sure the offset doesn't go haywire
2379   }
2380
2381   // reset all trims, except throttle (if throttle trim)
2382   for (uint8_t i=0; i<NUM_STICKS+NUM_AUX_TRIMS; i++) {
2383     if (i!=THR_STICK || !g_model.thrTrim) {
2384       int16_t original_trim = getTrimValue(mixerCurrentFlightMode, i);
2385       for (uint8_t phase=0; phase<MAX_FLIGHT_MODES; phase++) {
2386 #if defined(CPUARM)
2387         trim_t trim = getRawTrimValue(phase, i);
2388         if (trim.mode / 2 == phase)
2389           setTrimValue(phase, i, trim.value - original_trim);
2390 #else
2391         trim_t trim = getRawTrimValue(phase, i);
2392         if (trim <= TRIM_EXTENDED_MAX)
2393           setTrimValue(phase, i, trim - original_trim);
2394 #endif
2395       }
2396     }
2397   }
2398
2399   resumeMixerCalculations();
2400
2401   storageDirty(EE_MODEL);
2402   AUDIO_WARNING2();
2403 }
2404
2405 #if defined(ROTARY_ENCODERS)
2406   volatile rotenc_t rotencValue[ROTARY_ENCODERS] = {0};
2407 #elif defined(ROTARY_ENCODER_NAVIGATION)
2408   volatile rotenc_t rotencValue[1] = {0};
2409 #endif
2410
2411 #if defined(CPUARM) && defined(ROTARY_ENCODER_NAVIGATION)
2412 uint8_t rotencSpeed;
2413 #endif
2414
2415 #if !defined(CPUARM) && !defined(SIMU)
2416 extern unsigned char __bss_end ;
2417 #define STACKPTR     _SFR_IO16(0x3D)
2418 void stackPaint()
2419 {
2420   // Init Stack while interrupts are disabled
2421   unsigned char *p ;
2422   unsigned char *q ;
2423
2424   p = (unsigned char *) STACKPTR ;
2425   q = &__bss_end ;
2426   p -= 2 ;
2427   while ( p > q ) {
2428     *p-- = 0x55 ;
2429   }
2430 }
2431
2432 uint16_t stackAvailable()
2433 {
2434   unsigned char *p ;
2435
2436   p = &__bss_end + 1 ;
2437   while ( *p++ == 0x55 );
2438   return p - &__bss_end ;
2439 }
2440 #endif
2441
2442 #if defined(CPUM2560)
2443   #define OPENTX_INIT_ARGS const uint8_t mcusr
2444 #elif defined(PCBSTD)
2445   #define OPENTX_INIT_ARGS const uint8_t mcusr
2446 #else
2447   #define OPENTX_INIT_ARGS
2448 #endif
2449
2450 void opentxInit(OPENTX_INIT_ARGS)
2451 {
2452   TRACE("opentxInit");
2453
2454 #if defined(GUI)
2455   menuHandlers[0] = menuMainView;
2456   #if MENUS_LOCK != 2/*no menus*/
2457     menuHandlers[1] = menuModelSelect;
2458   #endif
2459 #endif
2460
2461 #if defined(RTCLOCK) && !defined(COPROCESSOR)
2462   rtcInit(); // RTC must be initialized before rambackupRestore() is called
2463 #endif
2464
2465 #if defined(EEPROM)
2466   storageReadRadioSettings();
2467 #endif
2468
2469   // Radios handle UNEXPECTED_SHUTDOWN() differently:
2470   //  * radios with WDT and EEPROM and CPU controlled power use Reset status register
2471   //    and eeGeneral.unexpectedShutdown
2472   //  * radios with SDCARD model storage use Reset status register and special
2473   //    variables in RAM. They can not use eeGeneral.unexpectedShutdown
2474   //  * radios without CPU controlled power can only use Reset status register (if available)
2475   if (UNEXPECTED_SHUTDOWN()) {
2476     TRACE("Unexpected Shutdown detected");
2477     unexpectedShutdown = 1;
2478   }
2479
2480 #if defined(SDCARD) && !defined(PCBMEGA2560)
2481   // SDCARD related stuff, only done if not unexpectedShutdown
2482   if (!unexpectedShutdown) {
2483     sdInit();
2484     logsInit();
2485   }
2486 #endif
2487
2488 #if defined(EEPROM)
2489   storageReadCurrentModel();
2490 #endif
2491
2492 #if defined(PCBHORUS)
2493   if (!unexpectedShutdown) {
2494     // g_model.topbarData is still zero here (because it was not yet read from SDCARD),
2495     // but we only remember the pointer to in in constructor.
2496     // The storageReadAll() needs topbar object, so it must be created here
2497 #if __clang__
2498 // clang does not like this at all, turn into a warning so that -Werror does not stop here
2499 // taking address of packed member 'topbarData' of class or structure 'ModelData' may result in an unaligned pointer value [-Werror,-Waddress-of-packed-member]
2500 #pragma clang diagnostic push
2501 #pragma clang diagnostic warning "-Waddress-of-packed-member"
2502 #endif
2503     topbar = new Topbar(&g_model.topbarData);
2504 #if __clang__
2505 // Restore warnings
2506 #pragma clang diagnostic pop
2507 #endif
2508
2509     // lua widget state must also be prepared before the call to storageReadAll()
2510     LUA_INIT_THEMES_AND_WIDGETS();
2511   }
2512 #endif
2513
2514   // handling of storage for radios that have no EEPROM
2515 #if !defined(EEPROM)
2516 #if defined(RAMBACKUP)
2517   if (unexpectedShutdown) {
2518     // SDCARD not available, try to restore last model from RAM
2519     TRACE("rambackupRestore");
2520     rambackupRestore();
2521   }
2522   else {
2523     storageReadAll();
2524   }
2525 #else
2526   storageReadAll();
2527 #endif
2528 #endif  // #if !defined(EEPROM)
2529
2530 #if defined(SERIAL2)
2531   serial2Init(g_eeGeneral.serial2Mode, modelTelemetryProtocol());
2532 #endif
2533
2534 #if defined(PCBTARANIS)
2535   BACKLIGHT_ENABLE();
2536 #endif
2537
2538 #if MENUS_LOCK == 1
2539   getMovedSwitch();
2540   if (TRIMS_PRESSED() && g_eeGeneral.switchUnlockStates==switches_states) {
2541     readonly = false;
2542   }
2543 #endif
2544
2545 #if defined(VOICE) && defined(CPUARM)
2546   currentSpeakerVolume = requiredSpeakerVolume = g_eeGeneral.speakerVolume + VOLUME_LEVEL_DEF;
2547   #if !defined(SOFTWARE_VOLUME)
2548     setScaledVolume(currentSpeakerVolume);
2549   #endif
2550 #endif
2551
2552 #if defined(CPUARM)
2553   referenceSystemAudioFiles();
2554   audioQueue.start();
2555   BACKLIGHT_ENABLE();
2556 #endif
2557
2558 #if defined(PCBSKY9X)
2559   // Set ADC gains here
2560   setSticksGain(g_eeGeneral.sticksGain);
2561 #endif
2562
2563 #if defined(PCBSKY9X) && defined(BLUETOOTH)
2564   btInit();
2565 #endif
2566
2567 #if defined(PCBHORUS)
2568   loadTheme();
2569   loadFontCache();
2570 #endif
2571
2572   if (g_eeGeneral.backlightMode != e_backlight_mode_off) {
2573     // on Tx start turn the light on
2574     backlightOn();
2575   }
2576
2577 #if NUM_PWMANALOGS > 0
2578   pwmCheck();
2579 #endif
2580
2581   if (!unexpectedShutdown) {
2582     opentxStart();
2583   }
2584
2585 #if defined(CPUARM) || defined(CPUM2560)
2586         // TODO Horus does not need this
2587   if (!g_eeGeneral.unexpectedShutdown) {
2588     g_eeGeneral.unexpectedShutdown = 1;
2589     storageDirty(EE_GENERAL);
2590   }
2591 #endif
2592
2593 #if defined(GUI)
2594   lcdSetContrast();
2595 #endif
2596   backlightOn();
2597
2598 #if defined(PCBSKY9X) && !defined(SIMU)
2599   init_trainer_capture();
2600 #endif
2601
2602 #if !defined(CPUARM)
2603   doMixerCalculations();
2604 #endif
2605
2606   startPulses();
2607
2608   wdt_enable(WDTO_500MS);
2609 }
2610
2611 #if defined(SIMU)
2612 void * simuMain(void *)
2613 #else
2614 int main()
2615 #endif
2616 {
2617   // G: The WDT remains active after a WDT reset -- at maximum clock speed. So it's
2618   // important to disable it before commencing with system initialisation (or
2619   // we could put a bunch more wdt_reset()s in. But I don't like that approach
2620   // during boot up.)
2621 #if defined(CPUM2560) || defined(CPUM2561)
2622   uint8_t mcusr = MCUSR; // save the WDT (etc) flags
2623   MCUSR = 0; // must be zeroed before disabling the WDT
2624   MCUCR = 0x80 ;   // Disable JTAG port that can interfere with POT3
2625   MCUCR = 0x80 ;   // Must be done twice
2626 #elif defined(PCBSTD)
2627   uint8_t mcusr = MCUCSR;
2628   MCUCSR = 0x80 ;   // Disable JTAG port that can interfere with POT3
2629   MCUCSR = 0x80 ;   // Must be done twice
2630 #endif
2631 #if defined(PCBTARANIS)
2632   g_eeGeneral.contrast = LCD_CONTRAST_DEFAULT;
2633 #endif
2634   wdt_disable();
2635
2636   boardInit();
2637
2638 #if defined(PCBX7)
2639   bluetoothInit(BLUETOOTH_DEFAULT_BAUDRATE);   //BT is turn on for a brief period to differentiate X7 and X7S
2640 #endif
2641
2642 #if defined(GUI) && !defined(PCBTARANIS) && !defined(PCBHORUS)
2643   // TODO remove this
2644   lcdInit();
2645 #endif
2646
2647 #if !defined(SIMU)
2648   stackPaint();
2649 #endif
2650
2651 #if defined(GUI) && !defined(PCBTARANIS)
2652   // lcdSetRefVolt(25);
2653 #endif
2654
2655 #if defined(SPLASH) && (defined(PCBTARANIS) || defined(PCBHORUS))
2656   drawSplash();
2657 #endif
2658
2659   sei(); // interrupts needed now
2660
2661 #if !defined(CPUARM) && defined(TELEMETRY_FRSKY) && !defined(DSM2_SERIAL)
2662   telemetryInit();
2663 #endif
2664
2665 #if defined(DSM2_SERIAL) && !defined(TELEMETRY_FRSKY)
2666   DSM2_Init();
2667 #endif
2668
2669 #if defined(TELEMETRY_JETI)
2670   JETI_Init();
2671 #endif
2672
2673 #if defined(TELEMETRY_ARDUPILOT)
2674   ARDUPILOT_Init();
2675 #endif
2676
2677 #if defined(TELEMETRY_NMEA)
2678   NMEA_Init();
2679 #endif
2680
2681 #if defined(TELEMETRY_MAVLINK)
2682   MAVLINK_Init();
2683 #endif
2684
2685 #if defined(MENU_ROTARY_SW)
2686   init_rotary_sw();
2687 #endif
2688
2689 #if defined(PCBHORUS)
2690   if (!IS_FIRMWARE_COMPATIBLE_WITH_BOARD()) {
2691     runFatalErrorScreen(STR_WRONG_PCBREV);
2692   }
2693 #endif
2694
2695 #if !defined(EEPROM)
2696   if (!SD_CARD_PRESENT() && !UNEXPECTED_SHUTDOWN()) {
2697     runFatalErrorScreen(STR_NO_SDCARD);
2698   }
2699 #endif
2700
2701 #if defined(CPUARM)
2702   tasksStart();
2703 #else
2704   opentxInit(mcusr);
2705 #if defined(CPUM2560)
2706   uint8_t shutdown_state = 0;
2707 #endif
2708
2709   while (1) {
2710 #if defined(CPUM2560)
2711     if ((shutdown_state=pwrCheck()) > e_power_trainer)
2712       break;
2713 #endif
2714 #if defined(SIMU)
2715     sleep(5/*ms*/);
2716     if (main_thread_running == 0)
2717       return 0;
2718 #endif
2719
2720     perMain();
2721
2722     if (heartbeat == HEART_WDT_CHECK) {
2723       wdt_reset();
2724       heartbeat = 0;
2725     }
2726   }
2727 #endif
2728
2729 #if defined(CPUM2560)
2730   // Time to switch off
2731   drawSleepBitmap();
2732   opentxClose();
2733   boardOff(); // Only turn power off if necessary
2734   wdt_disable();  // this function is provided by AVR Libc
2735   while(1); // never return from main() - there is no code to return back, if any delays occurs in physical power it does dead loop.
2736 #endif
2737
2738 #if defined(SIMU)
2739   return NULL;
2740 #endif
2741 }
2742
2743 #if defined(PWR_BUTTON_PRESS)
2744 uint32_t pwr_press_time = 0;
2745
2746 uint32_t pwrPressedDuration()
2747 {
2748   if (pwr_press_time == 0) {
2749     return 0;
2750   }
2751   else {
2752     return get_tmr10ms() - pwr_press_time;
2753   }
2754 }
2755
2756 uint32_t pwrCheck()
2757 {
2758   const char * message = NULL;
2759
2760   enum PwrCheckState {
2761     PWR_CHECK_ON,
2762     PWR_CHECK_OFF,
2763     PWR_CHECK_PAUSED,
2764   };
2765
2766   static uint8_t pwr_check_state = PWR_CHECK_ON;
2767
2768   if (pwr_check_state == PWR_CHECK_OFF) {
2769     return e_power_off;
2770   }
2771   else if (pwrPressed()) {
2772     if (TELEMETRY_STREAMING()) {
2773       message = STR_MODEL_STILL_POWERED;
2774     }
2775     if (pwr_check_state == PWR_CHECK_PAUSED) {
2776       // nothing
2777     }
2778     else if (pwr_press_time == 0) {
2779       pwr_press_time = get_tmr10ms();
2780       if (message && !g_eeGeneral.disableRssiPoweroffAlarm) {
2781         audioEvent(AU_MODEL_STILL_POWERED);
2782       }
2783     }
2784     else {
2785       inactivity.counter = 0;
2786       if (g_eeGeneral.backlightMode != e_backlight_mode_off) {
2787         BACKLIGHT_ENABLE();
2788       }
2789       if (get_tmr10ms() - pwr_press_time > PWR_PRESS_SHUTDOWN_DELAY) {
2790 #if defined(SHUTDOWN_CONFIRMATION)
2791         while (1) {
2792 #else
2793         while ((TELEMETRY_STREAMING() && !g_eeGeneral.disableRssiPoweroffAlarm)) {
2794 #endif
2795           lcdRefreshWait();
2796           lcdClear();
2797           POPUP_CONFIRMATION("Confirm Shutdown");
2798           event_t evt = getEvent(false);
2799           DISPLAY_WARNING(evt);
2800           lcdRefresh();
2801           if (warningResult) {
2802             pwr_check_state = PWR_CHECK_OFF;
2803             return e_power_off;
2804           }
2805           else if (!warningText) {
2806             // shutdown has been cancelled
2807             pwr_check_state = PWR_CHECK_PAUSED;
2808             return e_power_on;
2809           }
2810         }
2811         haptic.play(15, 3, PLAY_NOW);
2812         pwr_check_state = PWR_CHECK_OFF;
2813         return e_power_off;
2814       }
2815       else {
2816         drawShutdownAnimation(pwrPressedDuration(), message);
2817         return e_power_press;
2818       }
2819     }
2820   }
2821   else {
2822     pwr_check_state = PWR_CHECK_ON;
2823     pwr_press_time = 0;
2824   }
2825
2826   return e_power_on;
2827 }
2828 #elif defined(CPUARM)
2829 uint32_t pwrCheck()
2830 {
2831 #if defined(SOFT_PWR_CTRL)
2832   if (pwrPressed()) {
2833     return e_power_on;
2834   }
2835 #endif
2836
2837   if (usbPlugged()) {
2838     return e_power_usb;
2839   }
2840
2841 #if defined(TRAINER_PWR)
2842   if (TRAINER_CONNECTED()) {
2843     return e_power_trainer;
2844   }
2845 #endif
2846
2847   if (!g_eeGeneral.disableRssiPoweroffAlarm) {
2848     if (TELEMETRY_STREAMING()) {
2849       RAISE_ALERT(STR_MODEL, STR_MODEL_STILL_POWERED, STR_PRESS_ENTER_TO_CONFIRM, AU_MODEL_STILL_POWERED);
2850       while (TELEMETRY_STREAMING()) {
2851         resetForcePowerOffRequest();
2852         CoTickDelay(10);
2853         if (pwrPressed()) {
2854           return e_power_on;
2855         }
2856         else if (readKeys() == (1 << KEY_ENTER)) {
2857           return e_power_off;
2858         }
2859       }
2860     }
2861   }
2862
2863   return e_power_off;
2864 }
2865 #endif