radio/src/mixer.cpp

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