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