radio/src/mixer.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 #include "timers.h"
  23
  24 #if defined(VIRTUAL_INPUTS)
  25   int8_t  virtualInputsTrims[NUM_INPUTS];
  26 #else
  27   int16_t rawAnas[NUM_INPUTS] = {0};
  28 #endif
  29
  30 int16_t  anas [NUM_INPUTS] = {0};
  31 int16_t  trims[NUM_STICKS+NUM_AUX_TRIMS] = {0};
  32 int32_t  chans[MAX_OUTPUT_CHANNELS] = {0};
  33 BeepANACenter bpanaCenter = 0;
  34
  35 int24_t act   [MAX_MIXERS] = {0};
  36 SwOn    swOn  [MAX_MIXERS]; // TODO better name later...
  37
  38 uint8_t mixWarning;
  39
  40 #if defined(MODULE_ALWAYS_SEND_PULSES)
  41   uint8_t startupWarningState;
  42 #endif
  43
  44 int16_t calibratedAnalogs[NUM_CALIBRATED_ANALOGS];
  45 int16_t channelOutputs[MAX_OUTPUT_CHANNELS] = {0};
  46 int16_t ex_chans[MAX_OUTPUT_CHANNELS] = {0}; // Outputs (before LIMITS) of the last perMain;
  47
  48 #if defined(HELI)
  49   int16_t cyc_anas[3] = {0};
  50 #endif
  51
  52 void applyExpos(int16_t * anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS)
  53 {
  54 #if !defined(VIRTUAL_INPUTS)
  55   int16_t anas2[NUM_INPUTS]; // values before expo, to ensure same expo base when multiple expo lines are used
  56   memcpy(anas2, anas, sizeof(anas2));
  57 #endif
  58
  59   int8_t cur_chn = -1;
  60
  61   for (uint8_t i=0; i<MAX_EXPOS; i++) {
  62 #if defined(BOLD_FONT)
  63     if (mode==e_perout_mode_normal) swOn[i].activeExpo = false;
  64 #endif
  65     ExpoData * ed = expoAddress(i);
  66     if (!EXPO_VALID(ed)) break; // end of list
  67     if (ed->chn == cur_chn)
  68       continue;
  69     if (ed->flightModes & (1<<mixerCurrentFlightMode))
  70       continue;
  71     if (getSwitch(ed->swtch)) {
  72 #if defined(VIRTUAL_INPUTS)
  73       int32_t v;
  74       if (ed->srcRaw == ovwrIdx) {
  75         v = ovwrValue;
  76       }
  77       else {
  78         v = getValue(ed->srcRaw);
  79         if (ed->srcRaw >= MIXSRC_FIRST_TELEM && ed->scale > 0) {
  80           v = (v * 1024) / convertTelemValue(ed->srcRaw-MIXSRC_FIRST_TELEM+1, ed->scale);
  81         }
  82         v = limit<int32_t>(-1024, v, 1024);
  83       }
  84 #else
  85       int16_t v = anas2[ed->chn];
  86 #endif
  87       if (EXPO_MODE_ENABLE(ed, v)) {
  88 #if defined(BOLD_FONT)
  89         if (mode==e_perout_mode_normal) swOn[i].activeExpo = true;
  90 #endif
  91         cur_chn = ed->chn;
  92
  93         //========== CURVE=================
  94 #if defined(CPUARM)
  95         if (ed->curve.value) {
  96           v = applyCurve(v, ed->curve);
  97         }
  98 #else
  99         int8_t curveParam = ed->curveParam;
 100         if (curveParam) {
 101           if (ed->curveMode == MODE_CURVE)
 102             v = applyCurve(v, curveParam);
 103           else
 104             v = expo(v, GET_GVAR(curveParam, -100, 100, mixerCurrentFlightMode));
 105         }
 106 #endif
 107
 108         //========== WEIGHT ===============
 109 #if defined(CPUARM)
 110         int32_t weight = GET_GVAR_PREC1(ed->weight, MIN_EXPO_WEIGHT, 100, mixerCurrentFlightMode);
 111         v = div_and_round((int32_t)v * weight, 1000);
 112 #else
 113         int16_t weight = GET_GVAR(ed->weight, MIN_EXPO_WEIGHT, 100, mixerCurrentFlightMode);
 114         weight = calc100to256(weight);
 115         v = ((int32_t)v * weight) >> 8;
 116 #endif
 117
 118 #if defined(VIRTUAL_INPUTS)
 119         //========== OFFSET ===============
 120         int32_t offset = GET_GVAR_PREC1(ed->offset, -100, 100, mixerCurrentFlightMode);
 121         if (offset) v += div_and_round(calc100toRESX(offset), 10);
 122
 123         //========== TRIMS ================
 124         if (ed->carryTrim < TRIM_ON)
 125           virtualInputsTrims[cur_chn] = -ed->carryTrim - 1;
 126         else if (ed->carryTrim == TRIM_ON && ed->srcRaw >= MIXSRC_Rud && ed->srcRaw <= MIXSRC_Ail)
 127           virtualInputsTrims[cur_chn] = ed->srcRaw - MIXSRC_Rud;
 128         else
 129           virtualInputsTrims[cur_chn] = -1;
 130 #endif
 131
 132         anas[cur_chn] = v;
 133       }
 134     }
 135   }
 136 }
 137
 138 // #define PREVENT_ARITHMETIC_OVERFLOW
 139 // because of optimizations the reserves before overruns occurs is only the half
 140 // this defines enables some checks the greatly improves this situation
 141 // It should nearly prevent all overruns (is still a chance for it, but quite low)
 142 // negative side is code cost 96 bytes flash
 143
 144 // we do it now half way, only in applyLimits, which costs currently 50bytes
 145 // according opinion poll this topic is currently not very important
 146 // the change below improves already the situation
 147 // the check inside mixer would slow down mix a little bit and costs additionally flash
 148 // also the check inside mixer still is not bulletproof, there may be still situations a overflow could occur
 149 // a bulletproof implementation would take about additional 100bytes flash
 150 // therefore with go with this compromize, interested people could activate this define
 151
 152 // @@@2 open.20.fsguruh ;
 153 // channel = channelnumber -1;
 154 // value = outputvalue with 100 mulitplied usual range -102400 to 102400; output -1024 to 1024
 155 // changed rescaling from *100 to *256 to optimize performance
 156 // rescaled from -262144 to 262144
 157 int16_t applyLimits(uint8_t channel, int32_t value)
 158 {
 159   LimitData * lim = limitAddress(channel);
 160
 161 #if defined(CPUARM) && defined(CURVES)
 162   if (lim->curve) {
 163     // TODO we loose precision here, applyCustomCurve could work with int32_t on ARM boards...
 164     if (lim->curve > 0)
 165       value = 256 * applyCustomCurve(value/256, lim->curve-1);
 166     else
 167       value = 256 * applyCustomCurve(-value/256, -lim->curve-1);
 168   }
 169 #endif
 170
 171
 172   int16_t ofs   = LIMIT_OFS_RESX(lim);
 173   int16_t lim_p = LIMIT_MAX_RESX(lim);
 174   int16_t lim_n = LIMIT_MIN_RESX(lim);
 175
 176   if (ofs > lim_p) ofs = lim_p;
 177   if (ofs < lim_n) ofs = lim_n;
 178
 179   // because the rescaling optimization would reduce the calculation reserve we activate this for all builds
 180   // it increases the calculation reserve from factor 20,25x to 32x, which it slightly better as original
 181   // without it we would only have 16x which is slightly worse as original, we should not do this
 182
 183   // thanks to gbirkus, he motivated this change, which greatly reduces overruns
 184   // unfortunately the constants and 32bit compares generates about 50 bytes codes; didn't find a way to get it down.
 185   value = limit(int32_t(-RESXl*256), value, int32_t(RESXl*256));  // saves 2 bytes compared to other solutions up to now
 186
 187 #if defined(PPM_LIMITS_SYMETRICAL)
 188   if (value) {
 189     int16_t tmp;
 190     if (lim->symetrical)
 191       tmp = (value > 0) ? (lim_p) : (-lim_n);
 192     else
 193       tmp = (value > 0) ? (lim_p - ofs) : (-lim_n + ofs);
 194     value = (int32_t) value * tmp;   //  div by 1024*256 -> output = -1024..1024
 195 #else
 196   if (value) {
 197     int16_t tmp = (value > 0) ? (lim_p - ofs) : (-lim_n + ofs);
 198     value = (int32_t) value * tmp;   //  div by 1024*256 -> output = -1024..1024
 199 #endif
 200
 201 #ifdef CORRECT_NEGATIVE_SHIFTS
 202     int8_t sign = (value<0?1:0);
 203     value -= sign;
 204     tmp = value>>16;   // that's quite tricky: the shiftright 16 operation is assmbled just with addressmove; just forget the two least significant bytes;
 205     tmp >>= 2;   // now one simple shift right for two bytes does the rest
 206     tmp += sign;
 207 #else
 208     tmp = value>>16;   // that's quite tricky: the shiftright 16 operation is assmbled just with addressmove; just forget the two least significant bytes;
 209     tmp >>= 2;   // now one simple shift right for two bytes does the rest
 210 #endif
 211
 212     ofs += tmp;  // ofs can to added directly because already recalculated,
 213   }
 214
 215   if (ofs > lim_p) ofs = lim_p;
 216   if (ofs < lim_n) ofs = lim_n;
 217
 218   if (lim->revert) ofs = -ofs; // finally do the reverse.
 219
 220 #if defined(OVERRIDE_CHANNEL_FUNCTION)
 221   if (safetyCh[channel] != OVERRIDE_CHANNEL_UNDEFINED) {
 222     // safety channel available for channel check
 223     ofs = calc100toRESX(safetyCh[channel]);
 224   }
 225 #endif
 226
 227   return ofs;
 228 }
 229
 230 // TODO same naming convention than the drawSource
 231
 232 getvalue_t getValue(mixsrc_t i)
 233 {
 234   if (i == MIXSRC_NONE) {
 235     return 0;
 236   }
 237
 238 #if defined(VIRTUAL_INPUTS)
 239   else if (i <= MIXSRC_LAST_INPUT) {
 240     return anas[i-MIXSRC_FIRST_INPUT];
 241   }
 242 #endif
 243
 244 #if defined(LUA_INPUTS)
 245   else if (i < MIXSRC_LAST_LUA) {
 246 #if defined(LUA_MODEL_SCRIPTS)
 247     div_t qr = div(i-MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS);
 248     return scriptInputsOutputs[qr.quot].outputs[qr.rem].value;
 249 #else
 250     return 0;
 251 #endif
 252   }
 253 #endif
 254
 255 #if defined(LUA_INPUTS)
 256   else if (i <= MIXSRC_LAST_POT+NUM_MOUSE_ANALOGS) {
 257     return calibratedAnalogs[i-MIXSRC_Rud];
 258   }
 259 #else
 260   else if (i>=MIXSRC_FIRST_STICK && i<=MIXSRC_LAST_POT+NUM_MOUSE_ANALOGS) {
 261     return calibratedAnalogs[i-MIXSRC_Rud];
 262   }
 263 #endif
 264
 265 #if defined(PCBGRUVIN9X) || defined(PCBMEGA2560) || defined(ROTARY_ENCODERS)
 266   else if (i <= MIXSRC_LAST_ROTARY_ENCODER) {
 267     return getRotaryEncoder(i-MIXSRC_REa);
 268   }
 269 #endif
 270
 271   else if (i == MIXSRC_MAX) {
 272     return 1024;
 273   }
 274
 275   else if (i <= MIXSRC_CYC3) {
 276 #if defined(HELI)
 277     return cyc_anas[i - MIXSRC_CYC1];
 278 #else
 279     return 0;
 280 #endif
 281   }
 282
 283   else if (i <= MIXSRC_LAST_TRIM) {
 284     return calc1000toRESX((int16_t)8 * getTrimValue(mixerCurrentFlightMode, i-MIXSRC_FIRST_TRIM));
 285   }
 286
 287 #if defined(PCBFLAMENCO)
 288   else if (i==MIXSRC_SA) return (switchState(SW_SA0) ? -1024 : (switchState(SW_SA1) ? 0 : 1024));
 289   else if (i==MIXSRC_SB) return (switchState(SW_SB0) ? -1024 : 1024);
 290   else if (i==MIXSRC_SC) return (switchState(SW_SC0) ? -1024 : (switchState(SW_SC1) ? 0 : 1024));
 291   else if (i==MIXSRC_SE) return (switchState(SW_SE0) ? -1024 : 1024);
 292   else if (i==MIXSRC_SF) return (switchState(SW_SF0) ? -1024 : (switchState(SW_SF1) ? 0 : 1024));
 293 #elif defined(PCBTARANIS) || defined(PCBHORUS)
 294   else if ((i >= MIXSRC_FIRST_SWITCH) && (i <= MIXSRC_LAST_SWITCH)) {
 295     mixsrc_t sw = i-MIXSRC_FIRST_SWITCH;
 296     if (SWITCH_EXISTS(sw)) {
 297       return (switchState(3*sw) ? -1024 : (switchState(3*sw+1) ? 0 : 1024));
 298     }
 299     else {
 300       return 0;
 301     }
 302   }
 303 #else
 304   else if (i == MIXSRC_3POS) {
 305     return (getSwitch(SW_ID0+1) ? -1024 : (getSwitch(SW_ID1+1) ? 0 : 1024));
 306   }
 307   // don't use switchState directly to give getSwitch possibility to hack values if needed for switch warning
 308   else if (i < MIXSRC_SW1) {
 309     return getSwitch(SWSRC_THR+i-MIXSRC_THR) ? 1024 : -1024;
 310   }
 311 #endif
 312
 313   else if (i <= MIXSRC_LAST_LOGICAL_SWITCH) {
 314     return getSwitch(SWSRC_FIRST_LOGICAL_SWITCH+i-MIXSRC_FIRST_LOGICAL_SWITCH) ? 1024 : -1024;
 315   }
 316   else if (i <= MIXSRC_LAST_TRAINER) {
 317     int16_t x = ppmInput[i-MIXSRC_FIRST_TRAINER];
 318     if (i<MIXSRC_FIRST_TRAINER+NUM_CAL_PPM) {
 319       x -= g_eeGeneral.trainer.calib[i-MIXSRC_FIRST_TRAINER];
 320     }
 321     return x*2;
 322   }
 323   else if (i <= MIXSRC_LAST_CH) {
 324     return ex_chans[i-MIXSRC_CH1];
 325   }
 326
 327 #if defined(GVARS)
 328   else if (i <= MIXSRC_LAST_GVAR) {
 329     return GVAR_VALUE(i-MIXSRC_GVAR1, getGVarFlightMode(mixerCurrentFlightMode, i - MIXSRC_GVAR1));
 330   }
 331 #endif
 332
 333 #if defined(CPUARM)
 334   else if (i == MIXSRC_TX_VOLTAGE) {
 335     return g_vbat100mV;
 336   }
 337   else if (i < MIXSRC_FIRST_TIMER) {
 338     // TX_TIME + SPARES
 339 #if defined(RTCLOCK)
 340     return (g_rtcTime % SECS_PER_DAY) / 60; // number of minutes from midnight
 341 #else
 342     return 0;
 343 #endif
 344   }
 345   else if (i <= MIXSRC_LAST_TIMER) {
 346     return timersStates[i-MIXSRC_FIRST_TIMER].val;
 347   }
 348 #else
 349   else if (i == MIXSRC_FIRST_TELEM-1+TELEM_TX_VOLTAGE) {
 350     return g_vbat100mV;
 351   }
 352   else if (i <= MIXSRC_FIRST_TELEM-1+TELEM_TIMER2) {
 353     return timersStates[i-MIXSRC_FIRST_TELEM+1-TELEM_TIMER1].val;
 354   }
 355 #endif
 356
 357 #if defined(CPUARM)
 358   else if (i <= MIXSRC_LAST_TELEM) {
 359     i -= MIXSRC_FIRST_TELEM;
 360     div_t qr = div(i, 3);
 361     TelemetryItem & telemetryItem = telemetryItems[qr.quot];
 362     switch (qr.rem) {
 363       case 1:
 364         return telemetryItem.valueMin;
 365       case 2:
 366         return telemetryItem.valueMax;
 367       default:
 368         return telemetryItem.value;
 369     }
 370   }
 371 #elif defined(TELEMETRY_FRSKY)
 372   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_TX) return telemetryData.rssi[1].value;
 373   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_RX) return telemetryData.rssi[0].value;
 374   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A1) return telemetryData.analog[TELEM_ANA_A1].value;
 375   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A2) return telemetryData.analog[TELEM_ANA_A2].value;
 376 #if defined(TELEMETRY_FRSKY_SPORT)
 377   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return telemetryData.hub.baroAltitude;
 378 #elif defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
 379   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return TELEMETRY_RELATIVE_BARO_ALT_BP;
 380 #endif
 381 #if defined(FRSKY_HUB)
 382   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return telemetryData.hub.rpm;
 383   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_FUEL) return telemetryData.hub.fuelLevel;
 384   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T1) return telemetryData.hub.temperature1;
 385   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T2) return telemetryData.hub.temperature2;
 386   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_SPEED) return TELEMETRY_GPS_SPEED_BP;
 387   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DIST) return telemetryData.hub.gpsDistance;
 388   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_GPSALT) return TELEMETRY_RELATIVE_GPS_ALT_BP;
 389   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELL) return (int16_t)TELEMETRY_MIN_CELL_VOLTAGE;
 390   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELLS_SUM) return (int16_t)telemetryData.hub.cellsSum;
 391   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)telemetryData.hub.vfas;
 392   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CURRENT) return (int16_t)telemetryData.hub.current;
 393   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CONSUMPTION) return telemetryData.hub.currentConsumption;
 394   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_POWER) return telemetryData.hub.power;
 395   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCx) return telemetryData.hub.accelX;
 396   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCy) return telemetryData.hub.accelY;
 397   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCz) return telemetryData.hub.accelZ;
 398   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_HDG) return telemetryData.hub.gpsCourse_bp;
 399   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VSPEED) return telemetryData.hub.varioSpeed;
 400   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ASPEED) return telemetryData.hub.airSpeed;
 401   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DTE) return telemetryData.hub.dTE;
 402   else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_MIN_A1) return telemetryData.analog[TELEM_ANA_A1].min;
 403   else if (i==MIXSRC_FIRST_TELEM-1+TELEM_MIN_A2) return telemetryData.analog[TELEM_ANA_A2].min;
 404   else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_CSW_MAX) return *(((int16_t*)(&telemetryData.hub.minAltitude))+i-(MIXSRC_FIRST_TELEM-1+TELEM_MIN_ALT));
 405 #endif
 406 #endif
 407   else return 0;
 408 }
 409
 410 void evalInputs(uint8_t mode)
 411 {
 412   BeepANACenter anaCenter = 0;
 413
 414 #if defined(HELI) && !defined(VIRTUAL_INPUTS)
 415   uint16_t d = 0;
 416   if (g_model.swashR.value) {
 417     uint32_t v = (int32_t(calibratedAnalogs[ELE_STICK])*calibratedAnalogs[ELE_STICK] + int32_t(calibratedAnalogs[AIL_STICK])*calibratedAnalogs[AIL_STICK]);
 418     uint32_t q = calc100toRESX(g_model.swashR.value);
 419     q *= q;
 420     if (v > q) {
 421       d = isqrt32(v);
 422     }
 423   }
 424 #endif
 425
 426   for (uint8_t i=0; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; i++) {
 427     // normalization [0..2048] -> [-1024..1024]
 428     uint8_t ch = (i < NUM_STICKS ? CONVERT_MODE(i) : i);
 429     int16_t v = anaIn(i);
 430
 431     if (IS_POT_MULTIPOS(i)) {
 432       v -= RESX;
 433     }
 434 #if !defined(SIMU)
 435     else {
 436       CalibData * calib = &g_eeGeneral.calib[i];
 437       v -= calib->mid;
 438       v = v * (int32_t) RESX / (max((int16_t) 100, (v > 0 ? calib->spanPos : calib->spanNeg)));
 439     }
 440 #endif
 441
 442     if (v < -RESX) v = -RESX;
 443     if (v >  RESX) v =  RESX;
 444
 445 #if defined(PCBTARANIS) && !defined(PCBX7) && !defined(SIMU)
 446     // TODO why not in the driver?
 447     if (i==POT1 || i==SLIDER1) {
 448       v = -v;
 449     }
 450 #endif
 451
 452     if (g_model.throttleReversed && ch==THR_STICK) {
 453       v = -v;
 454     }
 455
 456     BeepANACenter mask = (BeepANACenter)1 << ch;
 457
 458     calibratedAnalogs[ch] = v; // for show in expo
 459
 460     // filtering for center beep
 461     uint8_t tmp = (uint16_t)abs(v) / 16;
 462 #if defined(CPUARM)
 463     if (mode == e_perout_mode_normal) {
 464       if (tmp==0 || (tmp==1 && (bpanaCenter & mask))) {
 465         anaCenter |= mask;
 466         if ((g_model.beepANACenter & mask) && !(bpanaCenter & mask) && !menuCalibrationState) {
 467           if (!IS_POT(i) || IS_POT_SLIDER_AVAILABLE(i)) {
 468             AUDIO_POT_MIDDLE(i);
 469           }
 470         }
 471       }
 472     }
 473 #else
 474     if (tmp <= 1) anaCenter |= (tmp==0 ? mask : (bpanaCenter & mask));
 475 #endif
 476
 477     if (ch < NUM_STICKS) { // only do this for sticks
 478 #if defined(VIRTUAL_INPUTS)
 479       if (mode & e_perout_mode_nosticks) {
 480         v = 0;
 481       }
 482 #endif
 483
 484       if (mode <= e_perout_mode_inactive_flight_mode && isFunctionActive(FUNCTION_TRAINER+ch) && IS_TRAINER_INPUT_VALID()) {
 485         // trainer mode
 486         TrainerMix* td = &g_eeGeneral.trainer.mix[ch];
 487         if (td->mode) {
 488           uint8_t chStud = td->srcChn;
 489           int32_t vStud  = (ppmInput[chStud]- g_eeGeneral.trainer.calib[chStud]);
 490           vStud *= td->studWeight;
 491           vStud /= 50;
 492           switch (td->mode) {
 493             case 1:
 494               // add-mode
 495               v = limit<int16_t>(-RESX, v+vStud, RESX);
 496               break;
 497             case 2:
 498               // subst-mode
 499               v = vStud;
 500               break;
 501           }
 502         }
 503       }
 504
 505 #if defined(VIRTUAL_INPUTS)
 506       calibratedAnalogs[ch] = v;
 507 #else
 508 #if defined(HELI)
 509       if (d && (ch==ELE_STICK || ch==AIL_STICK)) {
 510         v = (int32_t(v) * calc100toRESX(g_model.swashR.value)) / int32_t(d);
 511       }
 512 #endif
 513       rawAnas[ch] = v;
 514       anas[ch] = v; // set values for mixer
 515 #endif
 516     }
 517   }
 518
 519 #if defined(ROTARY_ENCODERS)
 520   for (uint8_t i=0; i<NUM_ROTARY_ENCODERS; i++) {
 521     if (getRotaryEncoder(i) == 0) {
 522       anaCenter |= ((BeepANACenter)1 << (NUM_STICKS+NUM_POTS+NUM_SLIDERS+NUM_MOUSE_ANALOGS+i));
 523     }
 524   }
 525 #endif
 526
 527 #if NUM_MOUSE_ANALOGS > 0
 528   for (uint8_t i=0; i<NUM_MOUSE_ANALOGS; i++) {
 529     uint8_t ch = NUM_STICKS+NUM_POTS+NUM_SLIDERS+i;
 530     int16_t v = anaIn(MOUSE1+i);
 531     CalibData * calib = &g_eeGeneral.calib[ch];
 532     v -= calib->mid;
 533     v = v * (int32_t) RESX / (max((int16_t) 100, (v > 0 ? calib->spanPos : calib->spanNeg)));
 534     if (v < -RESX) v = -RESX;
 535     if (v >  RESX) v =  RESX;
 536     calibratedAnalogs[ch] = v;
 537   }
 538 #endif
 539
 540   /* EXPOs */
 541   applyExpos(anas, mode);
 542
 543   /* TRIMs */
 544   evalTrims(); // when no virtual inputs, the trims need the anas array calculated above (when throttle trim enabled)
 545
 546   if (mode == e_perout_mode_normal) {
 547 #if !defined(CPUARM)
 548     anaCenter &= g_model.beepANACenter;
 549     if (((bpanaCenter ^ anaCenter) & anaCenter)) AUDIO_POT_MIDDLE();
 550 #endif
 551     bpanaCenter = anaCenter;
 552   }
 553 }
 554
 555 #if defined(VIRTUAL_INPUTS)
 556 int getStickTrimValue(int stick, int stickValue)
 557 {
 558   if (stick < 0)
 559     return 0;
 560
 561   int trim = trims[stick];
 562   if (stick == THR_STICK) {
 563     if (g_model.thrTrim) {
 564       int trimMin = g_model.extendedTrims ? 2*TRIM_EXTENDED_MIN : 2*TRIM_MIN;
 565       trim = ((g_model.throttleReversed ? (trim+trimMin) : (trim-trimMin)) * (RESX-stickValue)) >> (RESX_SHIFT+1);
 566     }
 567     if (g_model.throttleReversed) {
 568       trim = -trim;
 569     }
 570   }
 571   return trim;
 572 }
 573
 574 int getSourceTrimValue(int source, int stickValue=0)
 575 {
 576   if (source >= MIXSRC_Rud && source <= MIXSRC_Ail)
 577     return getStickTrimValue(source - MIXSRC_Rud, stickValue);
 578   else if (source >= MIXSRC_FIRST_INPUT && source <= MIXSRC_LAST_INPUT)
 579     return getStickTrimValue(virtualInputsTrims[source - MIXSRC_FIRST_INPUT], stickValue);
 580   else
 581     return 0;
 582 }
 583 #endif
 584
 585 uint8_t mixerCurrentFlightMode;
 586 void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
 587 {
 588   evalInputs(mode);
 589
 590   if (tick10ms) evalLogicalSwitches(mode==e_perout_mode_normal);
 591
 592 #if defined(MODULE_ALWAYS_SEND_PULSES)
 593   checkStartupWarnings();
 594 #endif
 595
 596 #if defined(HELI)
 597 #if defined(VIRTUAL_INPUTS)
 598   int heliEleValue = getValue(g_model.swashR.elevatorSource);
 599   int heliAilValue = getValue(g_model.swashR.aileronSource);
 600 #else
 601   int16_t heliEleValue = anas[ELE_STICK];
 602   int16_t heliAilValue = anas[AIL_STICK];
 603 #endif
 604   if (g_model.swashR.value) {
 605     uint32_t v = ((int32_t)heliEleValue*heliEleValue + (int32_t)heliAilValue*heliAilValue);
 606     uint32_t q = calc100toRESX(g_model.swashR.value);
 607     q *= q;
 608     if (v>q) {
 609       uint16_t d = isqrt32(v);
 610       int16_t tmp = calc100toRESX(g_model.swashR.value);
 611       heliEleValue = (int32_t) heliEleValue*tmp/d;
 612       heliAilValue = (int32_t) heliAilValue*tmp/d;
 613     }
 614   }
 615
 616 #define REZ_SWASH_X(x)  ((x) - (x)/8 - (x)/128 - (x)/512)   //  1024*sin(60) ~= 886
 617 #define REZ_SWASH_Y(x)  ((x))   //  1024 => 1024
 618
 619   if (g_model.swashR.type) {
 620 #if defined(VIRTUAL_INPUTS)
 621     getvalue_t vp = heliEleValue + getSourceTrimValue(g_model.swashR.elevatorSource);
 622     getvalue_t vr = heliAilValue + getSourceTrimValue(g_model.swashR.aileronSource);
 623 #else
 624     getvalue_t vp = heliEleValue + trims[ELE_STICK];
 625     getvalue_t vr = heliAilValue + trims[AIL_STICK];
 626 #endif
 627     getvalue_t vc = 0;
 628     if (g_model.swashR.collectiveSource)
 629       vc = getValue(g_model.swashR.collectiveSource);
 630
 631 #if defined(VIRTUAL_INPUTS)
 632     vp = (vp * g_model.swashR.elevatorWeight) / 100;
 633     vr = (vr * g_model.swashR.aileronWeight) / 100;
 634     vc = (vc * g_model.swashR.collectiveWeight) / 100;
 635 #else
 636     if (g_model.swashR.invertELE) vp = -vp;
 637     if (g_model.swashR.invertAIL) vr = -vr;
 638     if (g_model.swashR.invertCOL) vc = -vc;
 639 #endif
 640
 641     switch (g_model.swashR.type) {
 642       case SWASH_TYPE_120:
 643         vp = REZ_SWASH_Y(vp);
 644         vr = REZ_SWASH_X(vr);
 645         cyc_anas[0] = vc - vp;
 646         cyc_anas[1] = vc + vp/2 + vr;
 647         cyc_anas[2] = vc + vp/2 - vr;
 648         break;
 649       case SWASH_TYPE_120X:
 650         vp = REZ_SWASH_X(vp);
 651         vr = REZ_SWASH_Y(vr);
 652         cyc_anas[0] = vc - vr;
 653         cyc_anas[1] = vc + vr/2 + vp;
 654         cyc_anas[2] = vc + vr/2 - vp;
 655         break;
 656       case SWASH_TYPE_140:
 657         vp = REZ_SWASH_Y(vp);
 658         vr = REZ_SWASH_Y(vr);
 659         cyc_anas[0] = vc - vp;
 660         cyc_anas[1] = vc + vp + vr;
 661         cyc_anas[2] = vc + vp - vr;
 662         break;
 663       case SWASH_TYPE_90:
 664         vp = REZ_SWASH_Y(vp);
 665         vr = REZ_SWASH_Y(vr);
 666         cyc_anas[0] = vc - vp;
 667         cyc_anas[1] = vc + vr;
 668         cyc_anas[2] = vc - vr;
 669         break;
 670       default:
 671         break;
 672     }
 673   }
 674 #endif
 675
 676   memclear(chans, sizeof(chans));        // All outputs to 0
 677
 678   //========== MIXER LOOP ===============
 679   uint8_t lv_mixWarning = 0;
 680
 681   uint8_t pass = 0;
 682
 683   bitfield_channels_t dirtyChannels = (bitfield_channels_t)-1; // all dirty when mixer starts
 684
 685   do {
 686
 687     bitfield_channels_t passDirtyChannels = 0;
 688
 689     for (uint8_t i=0; i<MAX_MIXERS; i++) {
 690
 691 #if defined(BOLD_FONT)
 692       if (mode==e_perout_mode_normal && pass==0) swOn[i].activeMix = 0;
 693 #endif
 694
 695       MixData *md = mixAddress(i);
 696
 697       if (md->srcRaw == 0) break;
 698
 699       mixsrc_t stickIndex = md->srcRaw - MIXSRC_Rud;
 700
 701       if (!(dirtyChannels & ((bitfield_channels_t)1 << md->destCh))) continue;
 702
 703       // if this is the first calculation for the destination channel, initialize it with 0 (otherwise would be random)
 704       if (i == 0 || md->destCh != (md-1)->destCh) {
 705         chans[md->destCh] = 0;
 706       }
 707
 708       //========== PHASE && SWITCH =====
 709       bool mixCondition = (md->flightModes != 0 || md->swtch);
 710       delayval_t mixEnabled = (!(md->flightModes & (1 << mixerCurrentFlightMode)) && getSwitch(md->swtch)) ? DELAY_POS_MARGIN+1 : 0;
 711
 712 #define MIXER_LINE_DISABLE()   (mixCondition = true, mixEnabled = 0)
 713
 714       if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !IS_TRAINER_INPUT_VALID()) {
 715         MIXER_LINE_DISABLE();
 716       }
 717
 718 #if defined(LUA_MODEL_SCRIPTS)
 719       // disable mixer if Lua script is used as source and script was killed
 720       if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_LUA && md->srcRaw <= MIXSRC_LAST_LUA) {
 721         div_t qr = div(md->srcRaw-MIXSRC_FIRST_LUA, MAX_SCRIPT_OUTPUTS);
 722         if (scriptInternalData[qr.quot].state != SCRIPT_OK) {
 723           MIXER_LINE_DISABLE();
 724         }
 725       }
 726 #endif
 727
 728       //========== VALUE ===============
 729       getvalue_t v = 0;
 730       if (mode > e_perout_mode_inactive_flight_mode) {
 731 #if defined(VIRTUAL_INPUTS)
 732         if (!mixEnabled) {
 733           continue;
 734         }
 735         else {
 736           v = getValue(md->srcRaw);
 737         }
 738 #else
 739         if (!mixEnabled || stickIndex >= NUM_STICKS || (stickIndex == THR_STICK && g_model.thrTrim)) {
 740           continue;
 741         }
 742         else {
 743           if (!(mode & e_perout_mode_nosticks)) v = anas[stickIndex];
 744         }
 745 #endif
 746       }
 747       else {
 748 #if !defined(VIRTUAL_INPUTS)
 749         if (stickIndex < NUM_STICKS) {
 750           v = md->noExpo ? rawAnas[stickIndex] : anas[stickIndex];
 751         }
 752         else
 753 #endif
 754         {
 755           mixsrc_t srcRaw = MIXSRC_Rud + stickIndex;
 756           v = getValue(srcRaw);
 757           srcRaw -= MIXSRC_CH1;
 758           if (srcRaw<=MIXSRC_LAST_CH-MIXSRC_CH1 && md->destCh != srcRaw) {
 759             if (dirtyChannels & ((bitfield_channels_t)1 << srcRaw) & (passDirtyChannels|~(((bitfield_channels_t) 1 << md->destCh)-1)))
 760               passDirtyChannels |= (bitfield_channels_t) 1 << md->destCh;
 761             if (srcRaw < md->destCh || pass > 0)
 762               v = chans[srcRaw] >> 8;
 763           }
 764         }
 765         if (!mixCondition) {
 766           mixEnabled = v >> DELAY_POS_SHIFT;
 767         }
 768       }
 769
 770       bool apply_offset_and_curve = true;
 771
 772       //========== DELAYS ===============
 773       delayval_t _swOn = swOn[i].now;
 774       delayval_t _swPrev = swOn[i].prev;
 775       bool swTog = (mixEnabled > _swOn+DELAY_POS_MARGIN || mixEnabled < _swOn-DELAY_POS_MARGIN);
 776       if (mode==e_perout_mode_normal && swTog) {
 777         if (!swOn[i].delay) _swPrev = _swOn;
 778         swOn[i].delay = (mixEnabled > _swOn ? md->delayUp : md->delayDown) * (100/DELAY_STEP);
 779         swOn[i].now = mixEnabled;
 780         swOn[i].prev = _swPrev;
 781       }
 782       if (mode==e_perout_mode_normal && swOn[i].delay > 0) {
 783         swOn[i].delay = max<int16_t>(0, (int16_t)swOn[i].delay - tick10ms);
 784         if (!mixCondition)
 785           v = _swPrev << DELAY_POS_SHIFT;
 786         else if (mixEnabled)
 787           continue;
 788       }
 789       else {
 790         if (mode==e_perout_mode_normal) {
 791           swOn[i].now = swOn[i].prev = mixEnabled;
 792         }
 793         if (!mixEnabled) {
 794           if ((md->speedDown || md->speedUp) && md->mltpx!=MLTPX_REP) {
 795             if (mixCondition) {
 796               v = (md->mltpx == MLTPX_ADD ? 0 : RESX);
 797               apply_offset_and_curve = false;
 798             }
 799           }
 800           else if (mixCondition) {
 801             continue;
 802           }
 803         }
 804       }
 805
 806       if (mode==e_perout_mode_normal && (!mixCondition || mixEnabled || swOn[i].delay)) {
 807         if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1);
 808 #if defined(BOLD_FONT)
 809         swOn[i].activeMix = true;
 810 #endif
 811       }
 812
 813       if (apply_offset_and_curve) {
 814
 815         //========== TRIMS ================
 816         if (!(mode & e_perout_mode_notrims)) {
 817 #if defined(VIRTUAL_INPUTS)
 818           if (md->carryTrim == 0) {
 819             v += getSourceTrimValue(md->srcRaw, v);
 820           }
 821 #else
 822           int8_t mix_trim = md->carryTrim;
 823           if (mix_trim < TRIM_ON)
 824             mix_trim = -mix_trim - 1;
 825           else if (mix_trim == TRIM_ON && stickIndex < NUM_STICKS)
 826             mix_trim = stickIndex;
 827           else
 828             mix_trim = -1;
 829           if (mix_trim >= 0) {
 830             int16_t trim = trims[mix_trim];
 831             if (mix_trim == THR_STICK && g_model.throttleReversed)
 832               v -= trim;
 833             else
 834               v += trim;
 835           }
 836 #endif
 837         }
 838       }
 839
 840 #if defined(CPUARM)
 841       int32_t weight = GET_GVAR_PREC1(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);
 842       weight = calc100to256_16Bits(weight);
 843 #else
 844       // saves 12 bytes code if done here and not together with weight; unknown reason
 845       int16_t weight = GET_GVAR(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);
 846       weight = calc100to256_16Bits(weight);
 847 #endif
 848       //========== SPEED ===============
 849       // now its on input side, but without weight compensation. More like other remote controls
 850       // lower weight causes slower movement
 851
 852       if (mode <= e_perout_mode_inactive_flight_mode && (md->speedUp || md->speedDown)) { // there are delay values
 853 #define DEL_MULT_SHIFT 8
 854         // we recale to a mult 256 higher value for calculation
 855         int32_t tact = act[i];
 856         int16_t diff = v - (tact>>DEL_MULT_SHIFT);
 857         if (diff) {
 858           // open.20.fsguruh: speed is defined in % movement per second; In menu we specify the full movement (-100% to 100%) = 200% in total
 859           // the unit of the stored value is the value from md->speedUp or md->speedDown divide SLOW_STEP seconds; e.g. value 4 means 4/SLOW_STEP = 2 seconds for CPU64
 860           // because we get a tick each 10msec, we need 100 ticks for one second
 861           // the value in md->speedXXX gives the time it should take to do a full movement from -100 to 100 therefore 200%. This equals 2048 in recalculated internal range
 862           if (tick10ms || !s_mixer_first_run_done) {
 863             // only if already time is passed add or substract a value according the speed configured
 864             int32_t rate = (int32_t) tick10ms << (DEL_MULT_SHIFT+11);  // = DEL_MULT*2048*tick10ms
 865             // rate equals a full range for one second; if less time is passed rate is accordingly smaller
 866             // if one second passed, rate would be 2048 (full motion)*256(recalculated weight)*100(100 ticks needed for one second)
 867             int32_t currentValue = ((int32_t) v<<DEL_MULT_SHIFT);
 868             if (diff > 0) {
 869               if (s_mixer_first_run_done && md->speedUp > 0) {
 870                 // if a speed upwards is defined recalculate the new value according configured speed; the higher the speed the smaller the add value is
 871                 int32_t newValue = tact+rate/((int16_t)(100/SLOW_STEP)*md->speedUp);
 872                 if (newValue<currentValue) currentValue = newValue; // Endposition; prevent toggling around the destination
 873               }
 874             }
 875             else {  // if is <0 because ==0 is not possible
 876               if (s_mixer_first_run_done && md->speedDown > 0) {
 877                 // see explanation in speedUp
 878                 int32_t newValue = tact-rate/((int16_t)(100/SLOW_STEP)*md->speedDown);
 879                 if (newValue>currentValue) currentValue = newValue; // Endposition; prevent toggling around the destination
 880               }
 881             }
 882             act[i] = tact = currentValue;
 883             // open.20.fsguruh: this implementation would save about 50 bytes code
 884           } // endif tick10ms ; in case no time passed assign the old value, not the current value from source
 885           v = (tact >> DEL_MULT_SHIFT);
 886         }
 887       }
 888
 889       //========== CURVES ===============
 890 #if defined(CPUARM)
 891       if (apply_offset_and_curve && md->curve.type != CURVE_REF_DIFF && md->curve.value) {
 892         v = applyCurve(v, md->curve);
 893       }
 894 #else
 895       if (apply_offset_and_curve && md->curveParam && md->curveMode == MODE_CURVE) {
 896         v = applyCurve(v, md->curveParam);
 897       }
 898 #endif
 899
 900       //========== WEIGHT ===============
 901       int32_t dv = (int32_t)v * weight;
 902 #if defined(CPUARM)
 903       dv = div_and_round(dv, 10);
 904 #endif
 905
 906       //========== OFFSET / AFTER ===============
 907       if (apply_offset_and_curve) {
 908 #if defined(CPUARM)
 909         int32_t offset = GET_GVAR_PREC1(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);
 910         if (offset) dv += div_and_round(calc100toRESX_16Bits(offset), 10) << 8;
 911 #else
 912         int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);
 913         if (offset) dv += int32_t(calc100toRESX_16Bits(offset)) << 8;
 914 #endif
 915       }
 916
 917       //========== DIFFERENTIAL =========
 918 #if defined(CPUARM)
 919       if (md->curve.type == CURVE_REF_DIFF && md->curve.value) {
 920         dv = applyCurve(dv, md->curve);
 921       }
 922 #else
 923       if (md->curveMode == MODE_DIFFERENTIAL) {
 924         // @@@2 also recalculate curveParam to a 256 basis which ease the calculation later a lot
 925         int16_t curveParam = calc100to256(GET_GVAR(md->curveParam, -100, 100, mixerCurrentFlightMode));
 926         if (curveParam > 0 && dv < 0)
 927           dv = (dv * (256 - curveParam)) >> 8;
 928         else if (curveParam < 0 && dv > 0)
 929           dv = (dv * (256 + curveParam)) >> 8;
 930       }
 931 #endif
 932
 933       int32_t * ptr = &chans[md->destCh]; // Save calculating address several times
 934
 935       switch (md->mltpx) {
 936         case MLTPX_REP:
 937           *ptr = dv;
 938 #if defined(BOLD_FONT)
 939           if (mode==e_perout_mode_normal) {
 940             for (uint8_t m=i-1; m<MAX_MIXERS && mixAddress(m)->destCh==md->destCh; m--)
 941               swOn[m].activeMix = false;
 942           }
 943 #endif
 944           break;
 945         case MLTPX_MUL:
 946           // @@@2 we have to remove the weight factor of 256 in case of 100%; now we use the new base of 256
 947           dv >>= 8;
 948           dv *= *ptr;
 949           dv >>= RESX_SHIFT;   // same as dv /= RESXl;
 950           *ptr = dv;
 951           break;
 952         default: // MLTPX_ADD
 953           *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100);
 954           break;
 955       } //endswitch md->mltpx
 956 #ifdef PREVENT_ARITHMETIC_OVERFLOW
 957 /*
 958       // a lot of assumptions must be true, for this kind of check; not really worth for only 4 bytes flash savings
 959       // this solution would save again 4 bytes flash
 960       int8_t testVar=(*ptr<<1)>>24;
 961       if ( (testVar!=-1) && (testVar!=0 ) ) {
 962         // this devices by 64 which should give a good balance between still over 100% but lower then 32x100%; should be OK
 963         *ptr >>= 6;  // this is quite tricky, reduces the value a lot but should be still over 100% and reduces flash need
 964       } */
 965
 966
 967       PACK( union u_int16int32_t {
 968         struct {
 969           int16_t lo;
 970           int16_t hi;
 971         } words_t;
 972         int32_t dword;
 973       });
 974
 975       u_int16int32_t tmp;
 976       tmp.dword=*ptr;
 977
 978       if (tmp.dword<0) {
 979         if ((tmp.words_t.hi&0xFF80)!=0xFF80) tmp.words_t.hi=0xFF86; // set to min nearly
 980       }
 981       else {
 982         if ((tmp.words_t.hi|0x007F)!=0x007F) tmp.words_t.hi=0x0079; // set to max nearly
 983       }
 984       *ptr = tmp.dword;
 985       // this implementation saves 18bytes flash
 986
 987 /*      dv=*ptr>>8;
 988       if (dv>(32767-RESXl)) {
 989         *ptr=(32767-RESXl)<<8;
 990       } else if (dv<(-32767+RESXl)) {
 991         *ptr=(-32767+RESXl)<<8;
 992       }*/
 993       // *ptr=limit( int32_t(int32_t(-1)<<23), *ptr, int32_t(int32_t(1)<<23));  // limit code cost 72 bytes
 994       // *ptr=limit( int32_t((-32767+RESXl)<<8), *ptr, int32_t((32767-RESXl)<<8));  // limit code cost 80 bytes
 995 #endif
 996
 997     } //endfor mixers
 998
 999     tick10ms = 0;
1000     dirtyChannels &= passDirtyChannels;
1001
1002   } while (++pass < 5 && dirtyChannels);
1003
1004   mixWarning = lv_mixWarning;
1005 }
1006
1007 int32_t sum_chans512[MAX_OUTPUT_CHANNELS] = {0};
1008
1009
1010 #define MAX_ACT 0xffff
1011 uint8_t lastFlightMode = 255; // TODO reinit everything here when the model changes, no???
1012
1013 #if defined(CPUARM)
1014 tmr10ms_t flightModeTransitionTime;
1015 uint8_t   flightModeTransitionLast = 255;
1016 #endif
1017
1018 void evalMixes(uint8_t tick10ms)
1019 {
1020 #if defined(PCBMEGA2560) && defined(DEBUG) && !defined(VOICE)
1021   PORTH |= 0x40; // PORTH:6 LOW->HIGH signals start of mixer interrupt
1022 #endif
1023
1024   static uint16_t fp_act[MAX_FLIGHT_MODES] = {0};
1025   static uint16_t delta = 0;
1026   static ACTIVE_PHASES_TYPE flightModesFade = 0;
1027
1028   LS_RECURSIVE_EVALUATION_RESET();
1029
1030   uint8_t fm = getFlightMode();
1031
1032   if (lastFlightMode != fm) {
1033 #if defined(CPUARM)
1034     flightModeTransitionTime = get_tmr10ms();
1035 #endif
1036
1037     if (lastFlightMode == 255) {
1038       fp_act[fm] = MAX_ACT;
1039     }
1040     else {
1041       uint8_t fadeTime = max(g_model.flightModeData[lastFlightMode].fadeOut, g_model.flightModeData[fm].fadeIn);
1042       ACTIVE_PHASES_TYPE transitionMask = ((ACTIVE_PHASES_TYPE)1 << lastFlightMode) + ((ACTIVE_PHASES_TYPE)1 << fm);
1043       if (fadeTime) {
1044         flightModesFade |= transitionMask;
1045         delta = (MAX_ACT / (100/SLOW_STEP)) / fadeTime;
1046       }
1047       else {
1048         flightModesFade &= ~transitionMask;
1049         fp_act[lastFlightMode] = 0;
1050         fp_act[fm] = MAX_ACT;
1051       }
1052 #if defined(CPUARM)
1053       logicalSwitchesCopyState(lastFlightMode, fm); // push last logical switches state from old to new flight mode
1054 #endif
1055     }
1056     lastFlightMode = fm;
1057   }
1058
1059 #if defined(CPUARM)
1060   if (flightModeTransitionTime && get_tmr10ms() > flightModeTransitionTime+SWITCHES_DELAY()) {
1061     flightModeTransitionTime = 0;
1062     if (fm != flightModeTransitionLast) {
1063       if (flightModeTransitionLast != 255) PLAY_PHASE_OFF(flightModeTransitionLast);
1064       PLAY_PHASE_ON(fm);
1065       flightModeTransitionLast = fm;
1066     }
1067   }
1068 #endif
1069
1070   int32_t weight = 0;
1071   if (flightModesFade) {
1072     memclear(sum_chans512, sizeof(sum_chans512));
1073     for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
1074       LS_RECURSIVE_EVALUATION_RESET();
1075       if (flightModesFade & ((ACTIVE_PHASES_TYPE)1 << p)) {
1076         mixerCurrentFlightMode = p;
1077         evalFlightModeMixes(p==fm ? e_perout_mode_normal : e_perout_mode_inactive_flight_mode, p==fm ? tick10ms : 0);
1078         for (uint8_t i=0; i<MAX_OUTPUT_CHANNELS; i++)
1079           sum_chans512[i] += (chans[i] >> 4) * fp_act[p];
1080         weight += fp_act[p];
1081       }
1082       LS_RECURSIVE_EVALUATION_RESET();
1083     }
1084     assert(weight);
1085     mixerCurrentFlightMode = fm;
1086   }
1087   else {
1088     mixerCurrentFlightMode = fm;
1089     evalFlightModeMixes(e_perout_mode_normal, tick10ms);
1090   }
1091
1092   //========== FUNCTIONS ===============
1093   // must be done after mixing because some functions use the inputs/channels values
1094   // must be done before limits because of the applyLimit function: it checks for safety switches which would be not initialized otherwise
1095   if (tick10ms) {
1096 #if defined(MASTER_VOLUME)
1097     requiredSpeakerVolume = g_eeGeneral.speakerVolume + VOLUME_LEVEL_DEF;
1098 #endif
1099
1100 #if defined(CPUARM)
1101     if (!g_model.noGlobalFunctions) {
1102       evalFunctions(g_eeGeneral.customFn, globalFunctionsContext);
1103     }
1104     evalFunctions(g_model.customFn, modelFunctionsContext);
1105 #else
1106     evalFunctions();
1107 #endif
1108   }
1109
1110   //========== LIMITS ===============
1111   for (uint8_t i=0; i<MAX_OUTPUT_CHANNELS; i++) {
1112     // chans[i] holds data from mixer.   chans[i] = v*weight => 1024*256
1113     // later we multiply by the limit (up to 100) and then we need to normalize
1114     // at the end chans[i] = chans[i]/256 =>  -1024..1024
1115     // interpolate value with min/max so we get smooth motion from center to stop
1116     // this limits based on v original values and min=-1024, max=1024  RESX=1024
1117     int32_t q = (flightModesFade ? (sum_chans512[i] / weight) << 4 : chans[i]);
1118
1119 #if defined(PCBSTD)
1120     ex_chans[i] = q >> 8;
1121 #else
1122     ex_chans[i] = q / 256;
1123 #endif
1124
1125     int16_t value = applyLimits(i, q);  // applyLimits will remove the 256 100% basis
1126
1127     cli();
1128     channelOutputs[i] = value;  // copy consistent word to int-level
1129     sei();
1130   }
1131
1132   if (tick10ms && flightModesFade) {
1133     uint16_t tick_delta = delta * tick10ms;
1134     for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
1135       ACTIVE_PHASES_TYPE flightModeMask = ((ACTIVE_PHASES_TYPE)1 << p);
1136       if (flightModesFade & flightModeMask) {
1137         if (p == fm) {
1138           if (MAX_ACT - fp_act[p] > tick_delta)
1139             fp_act[p] += tick_delta;
1140           else {
1141             fp_act[p] = MAX_ACT;
1142             flightModesFade -= flightModeMask;
1143           }
1144         }
1145         else {
1146           if (fp_act[p] > tick_delta)
1147             fp_act[p] -= tick_delta;
1148           else {
1149             fp_act[p] = 0;
1150             flightModesFade -= flightModeMask;
1151           }
1152         }
1153       }
1154     }
1155   }
1156
1157 #if defined(CPUM2560) && defined(DEBUG) && !defined(VOICE)
1158   PORTH &= ~0x40; // PORTH:6 HIGH->LOW signals end of mixer interrupt
1159 #endif
1160 }