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