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