radio/src/switches.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 #define CS_LAST_VALUE_INIT -32768
  24
  25 #if defined(PCBHORUS)
  26   #define SWITCH_WARNING_LIST_X        WARNING_LINE_X
  27   #define SWITCH_WARNING_LIST_Y        WARNING_LINE_Y+3*FH
  28 #elif LCD_W >= 212
  29   #define SWITCH_WARNING_LIST_X        60
  30   #define SWITCH_WARNING_LIST_Y        4*FH+4
  31 #else
  32   #define SWITCH_WARNING_LIST_X        4
  33   #define SWITCH_WARNING_LIST_Y        4*FH+4
  34 #endif
  35
  36 enum LogicalSwitchContextState {
  37   SWITCH_START,
  38   SWITCH_DELAY,
  39   SWITCH_ENABLE
  40 };
  41
  42 PACK(struct LogicalSwitchContext {
  43   uint8_t state:1;
  44   uint8_t timerState:2;
  45   uint8_t spare:5;
  46   uint8_t timer;
  47   int16_t lastValue;
  48 });
  49
  50 PACK(struct LogicalSwitchesFlightModeContext {
  51   LogicalSwitchContext lsw[MAX_LOGICAL_SWITCHES];
  52 });
  53 LogicalSwitchesFlightModeContext lswFm[MAX_FLIGHT_MODES];
  54
  55 #define LS_LAST_VALUE(fm, idx) lswFm[fm].lsw[idx].lastValue
  56
  57 #if defined(PCBTARANIS) || defined(PCBHORUS)
  58 #if defined(PCBX9E)
  59 tmr10ms_t switchesMidposStart[16];
  60 #else
  61 tmr10ms_t switchesMidposStart[6]; // TODO constant
  62 #endif
  63 uint64_t  switchesPos = 0;
  64 tmr10ms_t potsLastposStart[NUM_XPOTS];
  65 uint8_t   potsPos[NUM_XPOTS];
  66
  67 #define SWITCH_POSITION(sw)  (switchesPos & ((MASK_CFN_TYPE)1<<(sw)))
  68 #define POT_POSITION(sw)     ((potsPos[(sw)/XPOTS_MULTIPOS_COUNT] & 0x0f) == ((sw) % XPOTS_MULTIPOS_COUNT))
  69
  70 div_t switchInfo(int switchPosition)
  71 {
  72   return div(switchPosition-SWSRC_FIRST_SWITCH, 3);
  73 }
  74
  75 uint64_t check2PosSwitchPosition(uint8_t sw)
  76 {
  77   uint32_t index = (switchState(sw) ? sw : sw + 2);
  78   uint64_t result = ((uint64_t)1 << index);
  79
  80   if (!(switchesPos & result)) {
  81     PLAY_SWITCH_MOVED(index);
  82   }
  83
  84   return result;
  85 }
  86
  87 uint64_t check3PosSwitchPosition(uint8_t idx, uint8_t sw, bool startup)
  88 {
  89   uint64_t result;
  90   uint32_t index;
  91
  92   if (switchState(sw)) {
  93     index = sw;
  94     result = ((MASK_CFN_TYPE)1 << index);
  95     switchesMidposStart[idx] = 0;
  96   }
  97   else if (switchState(sw+2)) {
  98     index = sw + 2;
  99     result = ((MASK_CFN_TYPE)1 << index);
 100     switchesMidposStart[idx] = 0;
 101   }
 102   else {
 103     index = sw + 1;
 104     if (startup || SWITCH_POSITION(index) || g_eeGeneral.switchesDelay==SWITCHES_DELAY_NONE || (switchesMidposStart[idx] && (tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) > SWITCHES_DELAY())) {
 105       result = ((MASK_CFN_TYPE)1 << index);
 106       switchesMidposStart[idx] = 0;
 107     }
 108     else {
 109       result = (switchesPos & ((MASK_CFN_TYPE)0x7 << sw));
 110       if (!switchesMidposStart[idx]) {
 111         switchesMidposStart[idx] = get_tmr10ms();
 112       }
 113     }
 114   }
 115
 116   if (!(switchesPos & result)) {
 117     PLAY_SWITCH_MOVED(index);
 118   }
 119
 120   return result;
 121 }
 122
 123 #define CHECK_2POS(sw)       newPos |= check2PosSwitchPosition(sw ## 0)
 124 #define CHECK_3POS(idx, sw)  newPos |= check3PosSwitchPosition(idx, sw ## 0, startup)
 125
 126 void getSwitchesPosition(bool startup)
 127 {
 128   uint64_t newPos = 0;
 129   CHECK_3POS(0, SW_SA);
 130   CHECK_3POS(1, SW_SB);
 131   CHECK_3POS(2, SW_SC);
 132
 133 #if defined(PCBX9LITES)
 134   CHECK_2POS(SW_SD);
 135   CHECK_2POS(SW_SE);
 136   CHECK_2POS(SW_SF);
 137   CHECK_2POS(SW_SG);
 138 #elif defined(PCBX9LITE)
 139   CHECK_2POS(SW_SD);
 140   CHECK_2POS(SW_SE);
 141 #elif defined(PCBXLITES)
 142   CHECK_3POS(3, SW_SD);
 143   CHECK_2POS(SW_SE);
 144   CHECK_2POS(SW_SF);
 145   // no SWG and SWH on XLITES
 146 #elif defined(PCBXLITE)
 147   CHECK_3POS(3, SW_SD);
 148   // no SWE, SWF, SWG and SWH on XLITE
 149 #elif defined(PCBX7)
 150   CHECK_3POS(3, SW_SD);
 151   CHECK_2POS(SW_SF);
 152   CHECK_2POS(SW_SH);
 153 #else
 154   CHECK_3POS(3, SW_SD);
 155   CHECK_3POS(4, SW_SE);
 156   CHECK_2POS(SW_SF);
 157   CHECK_3POS(5, SW_SG);
 158   CHECK_2POS(SW_SH);
 159 #endif
 160
 161 #if defined(PCBX9DP) && PCBREV >= 2019
 162   CHECK_2POS(SW_SI);
 163 #endif
 164
 165 #if defined(PCBHORUS) || defined(PCBX7)
 166   CHECK_2POS(SW_SI);
 167   CHECK_2POS(SW_SJ);
 168 #endif
 169
 170 #if defined(PCBX9E)
 171   CHECK_3POS(6, SW_SI);
 172   CHECK_3POS(7, SW_SJ);
 173   CHECK_3POS(8, SW_SK);
 174   CHECK_3POS(9, SW_SL);
 175   CHECK_3POS(10, SW_SM);
 176   CHECK_3POS(11, SW_SN);
 177   CHECK_3POS(12, SW_SO);
 178   CHECK_3POS(13, SW_SP);
 179   CHECK_3POS(14, SW_SQ);
 180   CHECK_3POS(15, SW_SR);
 181 #endif
 182
 183   switchesPos = newPos;
 184
 185   for (int i=0; i<NUM_XPOTS; i++) {
 186     if (IS_POT_MULTIPOS(POT1+i)) {
 187       StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[POT1+i];
 188       if (IS_MULTIPOS_CALIBRATED(calib)) {
 189         uint8_t pos = anaIn(POT1+i) / (2*RESX/calib->count);
 190         uint8_t previousPos = potsPos[i] >> 4;
 191         uint8_t previousStoredPos = potsPos[i] & 0x0F;
 192         if (startup) {
 193           potsPos[i] = (pos << 4) | pos;
 194         }
 195         else if (pos != previousPos) {
 196           potsLastposStart[i] = get_tmr10ms();
 197           potsPos[i] = (pos << 4) | previousStoredPos;
 198         }
 199         else if (g_eeGeneral.switchesDelay==SWITCHES_DELAY_NONE || (tmr10ms_t)(get_tmr10ms() - potsLastposStart[i]) > SWITCHES_DELAY()) {
 200           potsLastposStart[i] = 0;
 201           potsPos[i] = (pos << 4) | pos;
 202           if (previousStoredPos != pos) {
 203             PLAY_SWITCH_MOVED(SWSRC_LAST_SWITCH+i*XPOTS_MULTIPOS_COUNT+pos);
 204           }
 205         }
 206       }
 207     }
 208   }
 209 }
 210
 211
 212 getvalue_t getValueForLogicalSwitch(mixsrc_t i)
 213 {
 214   getvalue_t result = getValue(i);
 215   if (i>=MIXSRC_FIRST_INPUT && i<=MIXSRC_LAST_INPUT) {
 216     int8_t trimIdx = virtualInputsTrims[i-MIXSRC_FIRST_INPUT];
 217     if (trimIdx >= 0) {
 218       int16_t trim = trims[trimIdx];
 219       if (trimIdx == THR_STICK && g_model.throttleReversed)
 220         result -= trim;
 221       else
 222         result += trim;
 223     }
 224   }
 225   return result;
 226 }
 227 #else
 228   #define getValueForLogicalSwitch(i) getValue(i)
 229 #endif
 230
 231 PACK(typedef struct {
 232   uint8_t state;
 233   uint8_t last;
 234 }) ls_sticky_struct;
 235
 236 PACK(typedef struct {
 237   uint16_t state:1;
 238   uint16_t duration:15;
 239 }) ls_stay_struct;
 240
 241 bool getLogicalSwitch(uint8_t idx)
 242 {
 243   LogicalSwitchData * ls = lswAddress(idx);
 244   bool result;
 245
 246   swsrc_t s = ls->andsw;
 247
 248   if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) {
 249     if (ls->func != LS_FUNC_STICKY && ls->func != LS_FUNC_EDGE ) {
 250       // AND switch must not affect STICKY and EDGE processing
 251       LS_LAST_VALUE(mixerCurrentFlightMode, idx) = CS_LAST_VALUE_INIT;
 252     }
 253     result = false;
 254   }
 255   else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) {
 256     bool res1 = getSwitch(ls->v1);
 257     bool res2 = getSwitch(ls->v2);
 258     switch (ls->func) {
 259       case LS_FUNC_AND:
 260         result = (res1 && res2);
 261         break;
 262       case LS_FUNC_OR:
 263         result = (res1 || res2);
 264         break;
 265       // case LS_FUNC_XOR:
 266       default:
 267         result = (res1 ^ res2);
 268         break;
 269     }
 270   }
 271   else if (s == LS_FAMILY_TIMER) {
 272     result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) <= 0);
 273   }
 274   else if (s == LS_FAMILY_STICKY) {
 275     result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) & (1<<0));
 276   }
 277   else if (s == LS_FAMILY_EDGE) {
 278     result = (LS_LAST_VALUE(mixerCurrentFlightMode, idx) & (1<<0));
 279   }
 280   else {
 281     getvalue_t x = getValueForLogicalSwitch(ls->v1);
 282     getvalue_t y;
 283     if (s == LS_FAMILY_COMP) {
 284       y = getValueForLogicalSwitch(ls->v2);
 285
 286       switch (ls->func) {
 287         case LS_FUNC_EQUAL:
 288           result = (x==y);
 289           break;
 290         case LS_FUNC_GREATER:
 291           result = (x>y);
 292           break;
 293         default:
 294           result = (x<y);
 295           break;
 296       }
 297     }
 298     else {
 299       mixsrc_t v1 = ls->v1;
 300       // Telemetry
 301       if (v1 >= MIXSRC_FIRST_TELEM) {
 302         if (!TELEMETRY_STREAMING() || IS_FAI_FORBIDDEN(v1-1)) {
 303           result = false;
 304           goto DurationAndDelayProcessing;
 305         }
 306
 307         y = convertLswTelemValue(ls);
 308
 309
 310       }
 311       else if (v1 >= MIXSRC_GVAR1) {
 312         y = ls->v2;
 313       }
 314       else {
 315         y = calc100toRESX(ls->v2);
 316       }
 317
 318       switch (ls->func) {
 319         case LS_FUNC_VEQUAL:
 320           result = (x==y);
 321           break;
 322         case LS_FUNC_VALMOSTEQUAL:
 323 #if defined(GVARS)
 324           if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR)
 325             result = (x==y);
 326           else
 327 #endif
 328           result = (abs(x-y) < (1024 / STICK_TOLERANCE));
 329           break;
 330         case LS_FUNC_VPOS:
 331           result = (x>y);
 332           break;
 333         case LS_FUNC_VNEG:
 334           result = (x<y);
 335           break;
 336         case LS_FUNC_APOS:
 337           result = (abs(x)>y);
 338           break;
 339         case LS_FUNC_ANEG:
 340           result = (abs(x)<y);
 341           break;
 342         default:
 343         {
 344           if (LS_LAST_VALUE(mixerCurrentFlightMode, idx) == CS_LAST_VALUE_INIT) {
 345             LS_LAST_VALUE(mixerCurrentFlightMode, idx) = x;
 346           }
 347           int16_t diff = x - LS_LAST_VALUE(mixerCurrentFlightMode, idx);
 348           bool update = false;
 349           if (ls->func == LS_FUNC_DIFFEGREATER) {
 350             if (y >= 0) {
 351               result = (diff >= y);
 352               if (diff < 0)
 353                 update = true;
 354             }
 355             else {
 356               result = (diff <= y);
 357               if (diff > 0)
 358                 update = true;
 359             }
 360           }
 361           else {
 362             result = (abs(diff) >= y);
 363           }
 364           if (result || update) {
 365             LS_LAST_VALUE(mixerCurrentFlightMode, idx) = x;
 366           }
 367           break;
 368         }
 369       }
 370     }
 371   }
 372
 373 DurationAndDelayProcessing:
 374
 375     if (ls->delay || ls->duration) {
 376       LogicalSwitchContext &context = lswFm[mixerCurrentFlightMode].lsw[idx];
 377       if (result) {
 378         if (context.timerState == SWITCH_START) {
 379           // set delay timer
 380           context.timerState = SWITCH_DELAY;
 381           context.timer = (ls->func == LS_FUNC_EDGE ? 0 : ls->delay);
 382         }
 383
 384         if (context.timerState == SWITCH_DELAY) {
 385           if (context.timer) {
 386             result = false;   // return false while delay timer running
 387           }
 388           else {
 389             // set duration timer
 390             context.timerState = SWITCH_ENABLE;
 391             context.timer = ls->duration;
 392           }
 393         }
 394
 395         if (context.timerState == SWITCH_ENABLE) {
 396           result = (ls->duration==0 || context.timer>0); // return false after duration timer runs out
 397           if (!result && ls->func == LS_FUNC_STICKY) {
 398             ls_sticky_struct & lastValue = (ls_sticky_struct &)context.lastValue;
 399             lastValue.state = 0;
 400           }
 401         }
 402       }
 403       else if (context.timerState == SWITCH_ENABLE && ls->duration > 0 && context.timer > 0) {
 404         result = true;
 405       }
 406       else {
 407         context.timerState = SWITCH_START;
 408         context.timer = 0;
 409       }
 410     }
 411
 412   return result;
 413 }
 414
 415 bool getSwitch(swsrc_t swtch, uint8_t flags)
 416 {
 417   bool result;
 418
 419   if (swtch == SWSRC_NONE)
 420     return true;
 421
 422   uint8_t cs_idx = abs(swtch);
 423
 424   if (cs_idx == SWSRC_ONE) {
 425     result = !s_mixer_first_run_done;
 426   }
 427   else if (cs_idx == SWSRC_ON) {
 428     result = true;
 429   }
 430 #if defined(DEBUG_LATENCY)
 431   else if (cs_idx == SWSRC_LATENCY_TOGGLE) {
 432     result = latencyToggleSwitch;
 433   }
 434 #endif
 435   else if (cs_idx <= SWSRC_LAST_SWITCH) {
 436 #if defined(PCBTARANIS) || defined(PCBHORUS)
 437     if (flags & GETSWITCH_MIDPOS_DELAY)
 438       result = SWITCH_POSITION(cs_idx-SWSRC_FIRST_SWITCH);
 439     else
 440       result = switchState(cs_idx-SWSRC_FIRST_SWITCH);
 441 #else
 442     result = switchState(cs_idx-SWSRC_FIRST_SWITCH);
 443 #endif
 444
 445   }
 446 #if NUM_XPOTS > 0
 447   else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) {
 448     result = POT_POSITION(cs_idx-SWSRC_FIRST_MULTIPOS_SWITCH);
 449   }
 450 #endif
 451   else if (cs_idx <= SWSRC_LAST_TRIM) {
 452     uint8_t idx = cs_idx - SWSRC_FIRST_TRIM;
 453     idx = (CONVERT_MODE_TRIMS(idx/2) << 1) + (idx & 1);
 454     result = trimDown(idx);
 455   }
 456   else if (cs_idx == SWSRC_RADIO_ACTIVITY) {
 457     result = (inactivity.counter < 2);
 458   }
 459   else if (cs_idx >= SWSRC_FIRST_SENSOR) {
 460     result = !telemetryItems[cs_idx-SWSRC_FIRST_SENSOR].isOld();
 461   }
 462   else if (cs_idx == SWSRC_TELEMETRY_STREAMING) {
 463     result = TELEMETRY_STREAMING();
 464   }
 465   else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) {
 466 #if defined(FLIGHT_MODES)
 467     uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE;
 468     if (flags & GETSWITCH_MIDPOS_DELAY)
 469       result = (idx == flightModeTransitionLast);
 470     else
 471       result = (idx == mixerCurrentFlightMode);
 472 #else
 473     result = false;
 474 #endif
 475    }
 476   else {
 477     cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH;
 478     result = lswFm[mixerCurrentFlightMode].lsw[cs_idx].state;
 479   }
 480
 481   return swtch > 0 ? result : !result;
 482 }
 483
 484 /**
 485   @brief Calculates new state of logical switches for mixerCurrentFlightMode
 486 */
 487 void evalLogicalSwitches(bool isCurrentFlightmode)
 488 {
 489   for (unsigned int idx=0; idx<MAX_LOGICAL_SWITCHES; idx++) {
 490     LogicalSwitchContext & context = lswFm[mixerCurrentFlightMode].lsw[idx];
 491     bool result = getLogicalSwitch(idx);
 492     if (isCurrentFlightmode) {
 493       if (result) {
 494         if (!context.state) PLAY_LOGICAL_SWITCH_ON(idx);
 495       }
 496       else {
 497         if (context.state) PLAY_LOGICAL_SWITCH_OFF(idx);
 498       }
 499     }
 500     context.state = result;
 501   }
 502 }
 503
 504 swarnstate_t switches_states = 0;
 505 swsrc_t getMovedSwitch()
 506 {
 507   static tmr10ms_t s_move_last_time = 0;
 508   swsrc_t result = 0;
 509
 510 #if defined(PCBTARANIS) || defined(PCBHORUS)
 511   for (int i=0; i<NUM_SWITCHES; i++) {
 512     if (SWITCH_EXISTS(i)) {
 513       swarnstate_t mask = ((swarnstate_t)0x03 << (i*2));
 514       uint8_t prev = (switches_states & mask) >> (i*2);
 515       uint8_t next = (1024+getValue(MIXSRC_SA+i)) / 1024;
 516       if (prev != next) {
 517         switches_states = (switches_states & (~mask)) | ((swarnstate_t)next << (i*2));
 518         result = 1+(3*i)+next;
 519       }
 520     }
 521   }
 522 #else
 523   // return delivers 1 to 3 for ID1 to ID3
 524   // 4..8 for all other switches if changed to true
 525   // -4..-8 for all other switches if changed to false
 526   // 9 for Trainer switch if changed to true; Change to false is ignored
 527   swarnstate_t mask = 0x80;
 528   for (uint8_t i=NUM_SWITCHES_POSITIONS; i>1; i--) {
 529     bool prev;
 530     prev = (switches_states & mask);
 531     // don't use getSwitch here to always get the proper value, even getSwitch manipulates
 532     bool next = switchState(i-1);
 533     if (prev != next) {
 534       if (((i<NUM_SWITCHES_POSITIONS) && (i>3)) || next==true)
 535         result = next ? i : -i;
 536       if (i<=3 && result==0) result = 1;
 537       switches_states ^= mask;
 538     }
 539     mask >>= 1;
 540   }
 541 #endif
 542
 543   if ((tmr10ms_t)(get_tmr10ms() - s_move_last_time) > 10)
 544     result = 0;
 545
 546   s_move_last_time = get_tmr10ms();
 547   return result;
 548 }
 549
 550 #if defined(GUI)
 551 void checkSwitches()
 552 {
 553   swarnstate_t last_bad_switches = 0xff;
 554   swarnstate_t states = g_model.switchWarningState;
 555
 556 #if defined(PCBTARANIS) || defined(PCBHORUS)
 557   uint8_t bad_pots = 0, last_bad_pots = 0xff;
 558 #endif
 559
 560 #if defined(PWR_BUTTON_PRESS)
 561   bool refresh = false;
 562 #endif
 563
 564   while (true) {
 565 #if defined(PCBTARANIS) || defined(PCBHORUS)
 566   #define GETADC_COUNT 1
 567 #endif
 568
 569 #ifdef GETADC_COUNT
 570     for (uint8_t i=0; i<GETADC_COUNT; i++) {
 571       GET_ADC_IF_MIXER_NOT_RUNNING();
 572     }
 573 #undef GETADC_COUNT
 574 #endif
 575
 576     getMovedSwitch();
 577
 578     bool warn = false;
 579 #if defined(COLORLCD)
 580     for (int i=0; i<NUM_SWITCHES; i++) {
 581       if (SWITCH_WARNING_ALLOWED(i)) {
 582         unsigned int state = ((states >> (3*i)) & 0x07);
 583         if (state && state-1 != ((switches_states >> (i*2)) & 0x03)) {
 584           warn = true;
 585         }
 586       }
 587     }
 588     if (g_model.potsWarnMode) {
 589       evalFlightModeMixes(e_perout_mode_normal, 0);
 590       bad_pots = 0;
 591       for (int i=0; i<NUM_POTS+NUM_SLIDERS; i++) {
 592         if (!IS_POT_SLIDER_AVAILABLE(POT1+i)) {
 593           continue;
 594         }
 595         if (!(g_model.potsWarnEnabled & (1 << i)) && (abs(g_model.potsWarnPosition[i] - GET_LOWRES_POT_POSITION(i)) > 1)) {
 596           warn = true;
 597           bad_pots |= (1<<i);
 598         }
 599       }
 600     }
 601 #elif defined(PCBTARANIS)
 602     for (int i=0; i<NUM_SWITCHES; i++) {
 603       if (SWITCH_WARNING_ALLOWED(i) && !(g_model.switchWarningEnable & (1<<i))) {
 604         swarnstate_t mask = ((swarnstate_t)0x03 << (i*2));
 605         if (!((states & mask) == (switches_states & mask))) {
 606           warn = true;
 607         }
 608       }
 609     }
 610     if (g_model.potsWarnMode) {
 611       evalFlightModeMixes(e_perout_mode_normal, 0);
 612       bad_pots = 0;
 613       for (int i=0; i<NUM_POTS+NUM_SLIDERS; i++) {
 614         if (!IS_POT_SLIDER_AVAILABLE(POT1+i)) {
 615           continue;
 616         }
 617         if (!(g_model.potsWarnEnabled & (1 << i)) && (abs(g_model.potsWarnPosition[i] - GET_LOWRES_POT_POSITION(i)) > 1)) {
 618           warn = true;
 619           bad_pots |= (1<<i);
 620         }
 621       }
 622     }
 623 #else
 624     for (int i=0; i<NUM_SWITCHES-1; i++) {
 625       if (!(g_model.switchWarningEnable & (1<<i))) {
 626         if (i == 0) {
 627           if ((states & 0x03) != (switches_states & 0x03)) {
 628             warn = true;
 629           }
 630         }
 631         else if ((states & (1<<(i+1))) != (switches_states & (1<<(i+1)))) {
 632           warn = true;
 633         }
 634       }
 635     }
 636 #endif
 637
 638     if (!warn) {
 639       break;
 640     }
 641
 642     LED_ERROR_BEGIN();
 643     backlightOn();
 644
 645     // first - display warning
 646 #if defined(PCBTARANIS) || defined(PCBHORUS)
 647     if (last_bad_switches != switches_states || last_bad_pots != bad_pots) {
 648       drawAlertBox(STR_SWITCHWARN, nullptr, STR_PRESSANYKEYTOSKIP);
 649       if (last_bad_switches == 0xff || last_bad_pots == 0xff) {
 650         AUDIO_ERROR_MESSAGE(AU_SWITCH_ALERT);
 651       }
 652       int x = SWITCH_WARNING_LIST_X;
 653       int y = SWITCH_WARNING_LIST_Y;
 654       int numWarnings = 0;
 655       for (int i=0; i<NUM_SWITCHES; ++i) {
 656 #if defined(COLORLCD)
 657         if (SWITCH_WARNING_ALLOWED(i)) {
 658           unsigned int state = ((g_model.switchWarningState >> (3*i)) & 0x07);
 659           if (state && state-1 != ((switches_states >> (i*2)) & 0x03)) {
 660             if (++numWarnings < 6) {
 661               // LcdFlags attr = ((states & mask) == (switches_states & mask)) ? TEXT_COLOR : ALARM_COLOR;
 662               LcdFlags attr = ALARM_COLOR;
 663               drawSwitch(x, y, SWSRC_FIRST_SWITCH+i*3+state-1, attr);
 664               x += 35;
 665             }
 666           }
 667         }
 668 #else
 669         if (SWITCH_WARNING_ALLOWED(i) && !(g_model.switchWarningEnable & (1<<i))) {
 670           swarnstate_t mask = ((swarnstate_t)0x03 << (i*2));
 671           LcdFlags attr = ((states & mask) == (switches_states & mask)) ? 0 : INVERS;
 672           if (attr) {
 673             if (++numWarnings < 6) {
 674               char c = "\300-\301"[(states & mask) >> (i*2)];
 675               drawSource(x, y, MIXSRC_FIRST_SWITCH+i, attr);
 676               lcdDrawChar(lcdNextPos, y, c, attr);
 677               x = lcdNextPos + 3;
 678             }
 679           }
 680         }
 681 #endif
 682       }
 683
 684       if (g_model.potsWarnMode) {
 685         for (int i=0; i<NUM_POTS+NUM_SLIDERS; i++) {
 686           if (!IS_POT_SLIDER_AVAILABLE(POT1+i)) {
 687             continue;
 688           }
 689           if (!(g_model.potsWarnEnabled & (1 << i))) {
 690             if (abs(g_model.potsWarnPosition[i] - GET_LOWRES_POT_POSITION(i)) > 1) {
 691               if (++numWarnings < 6) {
 692 #if defined(COLORLCD)
 693                 char s[8];
 694                 // TODO add an helper
 695                 strncpy(s, &STR_VSRCRAW[1+(NUM_STICKS+1+i)*STR_VSRCRAW[0]], STR_VSRCRAW[0]);
 696                 s[int(STR_VSRCRAW[0])] = '\0';
 697                 lcdDrawText(x, y, s, ALARM_COLOR);
 698                 x += 40;
 699 #else
 700                 lcdDrawTextAtIndex(x, y, STR_VSRCRAW, NUM_STICKS + 1 + i, INVERS);
 701                 if (IS_POT(POT1 + i))
 702                   lcdDrawChar(lcdNextPos, y, g_model.potsWarnPosition[i] > GET_LOWRES_POT_POSITION(i) ? 126 : 127, INVERS);
 703                 else
 704                   lcdDrawChar(lcdNextPos, y, g_model.potsWarnPosition[i] > GET_LOWRES_POT_POSITION(i) ? '\300' : '\301', INVERS);
 705                 x = lcdNextPos + 3;
 706 #endif
 707               }
 708             }
 709           }
 710         }
 711       }
 712
 713       if (numWarnings >= 6) {
 714 #if defined(COLORLCD)
 715         lcdDrawText(x, y, "...", ALARM_COLOR);
 716 #else
 717         lcdDrawText(x, y, "...", 0);
 718 #endif
 719       }
 720
 721       last_bad_pots = bad_pots;
 722 #else
 723     if (last_bad_switches != switches_states) {
 724       RAISE_ALERT(STR_SWITCHWARN, NULL, STR_PRESSANYKEYTOSKIP, last_bad_switches == 0xff ? AU_SWITCH_ALERT : AU_NONE);
 725       uint8_t x = 2;
 726       for (uint8_t i=0; i<NUM_SWITCHES-1; i++) {
 727         uint8_t attr;
 728         if (i == 0)
 729           attr = ((states & 0x03) != (switches_states & 0x03)) ? INVERS : 0;
 730         else
 731           attr = (states & (1 << (i+1))) == (switches_states & (1 << (i+1))) ? 0 : INVERS;
 732         if (!(g_model.switchWarningEnable & (1<<i)))
 733           drawSwitch(x, 5*FH, (i>0?(i+3):(states&0x3)+1), attr);
 734         x += 3*FW+FW/2;
 735       }
 736 #endif
 737
 738       lcdRefresh();
 739       lcdSetContrast();
 740       waitKeysReleased();
 741
 742       last_bad_switches = switches_states;
 743     }
 744
 745     if (keyDown())
 746       break;
 747
 748 #if defined(PWR_BUTTON_PRESS)
 749     uint32_t power = pwrCheck();
 750     if (power == e_power_off) {
 751       drawSleepBitmap();
 752       boardOff();
 753       break;
 754     }
 755     else if (power == e_power_press) {
 756       refresh = true;
 757     }
 758     else if (power == e_power_on && refresh) {
 759       last_bad_switches = 0xff;
 760       last_bad_pots = 0xff;
 761       refresh = false;
 762     }
 763 #else
 764     if (pwrCheck() == e_power_off) {
 765       break;
 766     }
 767 #endif
 768
 769     doLoopCommonActions();
 770
 771     WDG_RESET();
 772
 773     RTOS_WAIT_MS(10);
 774   }
 775
 776   LED_ERROR_END();
 777 }
 778 #endif // GUI
 779
 780 void logicalSwitchesTimerTick()
 781 {
 782   for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
 783     for (uint8_t i=0; i<MAX_LOGICAL_SWITCHES; i++) {
 784       LogicalSwitchData * ls = lswAddress(i);
 785       if (ls->func == LS_FUNC_TIMER) {
 786         int16_t * lastValue = &LS_LAST_VALUE(fm, i);
 787         if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) {
 788           *lastValue = -lswTimerValue(ls->v1);
 789         }
 790
 791         else if (*lastValue < 0) {
 792           if (++(*lastValue) == 0)
 793             *lastValue = lswTimerValue(ls->v2);
 794         }
 795         else { // if (*lastValue > 0)
 796           *lastValue -= 1;
 797         }
 798       }
 799       else if (ls->func == LS_FUNC_STICKY) {
 800         ls_sticky_struct & lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i);
 801         bool before = lastValue.last & 0x01;
 802         if (lastValue.state) {
 803           bool now = getSwitch(ls->v2);
 804           if (now != before) {
 805             lastValue.last ^= 1;
 806             if (!before) {
 807               lastValue.state = 0;
 808             }
 809           }
 810         }
 811         else {
 812           bool now = getSwitch(ls->v1);
 813           if (before != now) {
 814             lastValue.last ^= 1;
 815             if (!before) {
 816               lastValue.state = 1;
 817             }
 818           }
 819         }
 820       }
 821       else if (ls->func == LS_FUNC_EDGE) {
 822         ls_stay_struct & lastValue = (ls_stay_struct &)LS_LAST_VALUE(fm, i);
 823         // if this ls was reset by the logicalSwitchesReset() the lastValue will be set to CS_LAST_VALUE_INIT(0x8000)
 824         // when it is unpacked into ls_stay_struct the lastValue.duration will have a value of 0x4000
 825         // this will produce an instant true for edge logical switch if the second parameter is big enough.
 826         // So we reset it here.
 827         if (LS_LAST_VALUE(fm, i) == CS_LAST_VALUE_INIT) {
 828           lastValue.duration = 0;
 829         }
 830         lastValue.state = false;
 831         bool state = getSwitch(ls->v1);
 832         if (state) {
 833           if (ls->v3 == -1 && lastValue.duration == lswTimerValue(ls->v2))
 834             lastValue.state = true;
 835           if (lastValue.duration < 1000)
 836             lastValue.duration++;
 837         }
 838         else {
 839           if (lastValue.duration > lswTimerValue(ls->v2) && (ls->v3 == 0 || lastValue.duration <= lswTimerValue(ls->v2+ls->v3)))
 840             lastValue.state = true;
 841           lastValue.duration = 0;
 842         }
 843       }
 844
 845       // decrement delay/duration timer
 846       LogicalSwitchContext &context = lswFm[fm].lsw[i];
 847       if (context.timer) {
 848         context.timer--;
 849       }
 850     }
 851   }
 852 }
 853
 854 LogicalSwitchData * lswAddress(uint8_t idx)
 855 {
 856   return &g_model.logicalSw[idx];
 857 }
 858
 859 uint8_t lswFamily(uint8_t func)
 860 {
 861   if (func <= LS_FUNC_ANEG)
 862     return LS_FAMILY_OFS;
 863   else if (func <= LS_FUNC_XOR)
 864     return LS_FAMILY_BOOL;
 865   else if (func == LS_FUNC_EDGE)
 866     return LS_FAMILY_EDGE;
 867   else if (func <= LS_FUNC_LESS)
 868     return LS_FAMILY_COMP;
 869   else if (func <= LS_FUNC_ADIFFEGREATER)
 870     return LS_FAMILY_DIFF;
 871   else
 872     return LS_FAMILY_TIMER+func-LS_FUNC_TIMER;
 873 }
 874
 875 int16_t lswTimerValue(delayval_t val)
 876 {
 877   return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10));
 878 }
 879
 880 void logicalSwitchesReset()
 881 {
 882   memset(lswFm, 0, sizeof(lswFm));
 883
 884   for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
 885     for (uint8_t i=0; i<MAX_LOGICAL_SWITCHES; i++) {
 886       LS_LAST_VALUE(fm, i) = CS_LAST_VALUE_INIT;
 887     }
 888   }
 889 }
 890
 891 getvalue_t convertLswTelemValue(LogicalSwitchData * ls)
 892 {
 893   getvalue_t val;
 894   val = convert16bitsTelemValue(ls->v1 - MIXSRC_FIRST_TELEM + 1, ls->v2);
 895   return val;
 896 }
 897
 898 void logicalSwitchesCopyState(uint8_t src, uint8_t dst)
 899 {
 900   lswFm[dst] = lswFm[src];
 901 }