diff --git a/companion/src/eeprominterface.h b/companion/src/eeprominterface.h index 0666dfd50..c68a7a4e9 100644 --- a/companion/src/eeprominterface.h +++ b/companion/src/eeprominterface.h @@ -69,7 +69,7 @@ const uint8_t modn12x3[4][4]= { {4, 3, 2, 1} }; #define C9X_MAX_MODELS 60 -#define C9X_MAX_FLIGHT_MODES 9 +#define C9X_MAX_FLIGHT_MODES 9 #define C9X_MAX_MIXERS 64 #define C9X_MAX_INPUTS 32 #define C9X_MAX_EXPOS 64 diff --git a/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp b/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp index a7e1f0db1..0d677ccec 100644 --- a/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp +++ b/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp @@ -61,8 +61,10 @@ namespace Open9xGruvin9x { #include "radio/src/eeprom_common.cpp" #include "radio/src/eeprom_rlc.cpp" #include "radio/src/opentx.cpp" +#include "radio/src/strhelpers.cpp" #include "radio/src/switches.cpp" #include "radio/src/curves.cpp" +#include "radio/src/mixer.cpp" #include "radio/src/protocols/pulses_avr.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" diff --git a/companion/src/firmwares/opentx/opentxM128simulator.cpp b/companion/src/firmwares/opentx/opentxM128simulator.cpp index 18a58dd42..6500cd2db 100644 --- a/companion/src/firmwares/opentx/opentxM128simulator.cpp +++ b/companion/src/firmwares/opentx/opentxM128simulator.cpp @@ -60,8 +60,10 @@ namespace Open9xM128 { #include "radio/src/eeprom_common.cpp" #include "radio/src/eeprom_rlc.cpp" #include "radio/src/opentx.cpp" +#include "radio/src/strhelpers.cpp" #include "radio/src/switches.cpp" #include "radio/src/curves.cpp" +#include "radio/src/mixer.cpp" #include "radio/src/protocols/pulses_avr.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" diff --git a/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp b/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp index 9befd70e6..02e94691d 100644 --- a/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp +++ b/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp @@ -75,7 +75,9 @@ namespace Open9xSky9x { #include "radio/src/eeprom_raw.cpp" #include "radio/src/eeprom_conversions.cpp" #include "radio/src/opentx.cpp" +#include "radio/src/strhelpers.cpp" #include "radio/src/switches.cpp" +#include "radio/src/mixer.cpp" #include "radio/src/curves.cpp" #include "radio/src/targets/sky9x/pulses_driver.cpp" #include "radio/src/protocols/pulses_arm.cpp" diff --git a/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp b/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp index 87bd1062d..292eea558 100644 --- a/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp +++ b/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp @@ -78,8 +78,10 @@ inline int geteepromsize() { #include "radio/src/eeprom_conversions.cpp" #include "radio/src/eeprom_rlc.cpp" #include "radio/src/opentx.cpp" +#include "radio/src/strhelpers.cpp" #include "radio/src/switches.cpp" #include "radio/src/curves.cpp" +#include "radio/src/mixer.cpp" #include "radio/src/targets/taranis/pulses_driver.cpp" #include "radio/src/targets/taranis/rtc_driver.cpp" #include "radio/src/targets/taranis/trainer_driver.cpp" @@ -97,7 +99,6 @@ inline int geteepromsize() { #include "radio/src/gui/view_text.cpp" #include "radio/src/gui/view_about.cpp" #include "radio/src/lcd.cpp" -#include "radio/src/strhelpers.cpp" #include "radio/src/logs.cpp" #include "radio/src/rtc.cpp" #include "radio/src/targets/taranis/keys_driver.cpp" diff --git a/companion/src/firmwares/opentx/opentxsimulator.cpp b/companion/src/firmwares/opentx/opentxsimulator.cpp index 2dab2960c..d05056a7a 100644 --- a/companion/src/firmwares/opentx/opentxsimulator.cpp +++ b/companion/src/firmwares/opentx/opentxsimulator.cpp @@ -66,8 +66,10 @@ namespace Open9x { #include "radio/src/eeprom_common.cpp" #include "radio/src/eeprom_rlc.cpp" #include "radio/src/opentx.cpp" +#include "radio/src/strhelpers.cpp" #include "radio/src/switches.cpp" #include "radio/src/curves.cpp" +#include "radio/src/mixer.cpp" #include "radio/src/protocols/pulses_avr.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" diff --git a/radio/src/Makefile b/radio/src/Makefile index b0347c3a1..6b528963b 100644 --- a/radio/src/Makefile +++ b/radio/src/Makefile @@ -759,7 +759,7 @@ else TTS_SRC = $(shell sh -c "if test -f $(STD_TTS_SRC); then echo $(STD_TTS_SRC); else echo translations/tts_en.cpp; fi") endif -CPPSRC += opentx.cpp $(PULSESSRC) switches.cpp curves.cpp stamp.cpp gui/menus.cpp gui/menu_model.cpp gui/menu_general.cpp gui/view_main.cpp gui/view_statistics.cpp $(EEPROMSRC) lcd.cpp keys.cpp maths.cpp translations.cpp fonts.cpp $(TTS_SRC) +CPPSRC += opentx.cpp strhelpers.cpp $(PULSESSRC) switches.cpp curves.cpp mixer.cpp stamp.cpp gui/menus.cpp gui/menu_model.cpp gui/menu_general.cpp gui/view_main.cpp gui/view_statistics.cpp $(EEPROMSRC) lcd.cpp keys.cpp maths.cpp translations.cpp fonts.cpp $(TTS_SRC) # Debugging format. # Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs. @@ -968,7 +968,7 @@ endif ifeq ($(SDCARD), YES) CPPDEFS += -DSDCARD - CPPSRC += logs.cpp strhelpers.cpp + CPPSRC += logs.cpp endif RUN_FROM_FLASH = 1 diff --git a/radio/src/maths.cpp b/radio/src/maths.cpp index b45ab3655..2d07eb27c 100644 --- a/radio/src/maths.cpp +++ b/radio/src/maths.cpp @@ -36,6 +36,102 @@ #include "opentx.h" +#if !defined(CPUARM) +// #define CORRECT_NEGATIVE_SHIFTS +// open.20.fsguruh; shift right operations do the rounding different for negative values compared to positive values +// so all negative divisions round always further down, which give absolute values bigger compared to a usual division +// this is noticable on the display, because instead of -100.0 -99.9 is shown; While in praxis I doublt somebody will notice a +// difference this is more a mental thing. Maybe people are distracted, because the easy calculations are obviously wrong +// this define would correct this, but costs 34 bytes code for stock version + +// currently we set this to active always, because it might cause a fault about 1% compared positive and negative values +// is done now in makefile + +int16_t calc100to256_16Bits(int16_t x) // return x*2.56 +{ + // y = 2*x + x/2 +x/16-x/512-x/2048 + // 512 and 2048 are out of scope from int8 input --> forget it +#ifdef CORRECT_NEGATIVE_SHIFTS + int16_t res=(int16_t)x<<1; + //int8_t sign=(uint8_t) x>>7; + int8_t sign=(x<0?1:0); + + x-=sign; + res+=(x>>1); + res+=sign; + res+=(x>>4); + res+=sign; + return res; +#else + return ((int16_t)x<<1)+(x>>1)+(x>>4); +#endif +} + +int16_t calc100to256(int8_t x) // return x*2.56 +{ + return calc100to256_16Bits(x); +} + +int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24 +{ +#ifdef CORRECT_NEGATIVE_SHIFTS + int16_t res= ((int16_t)x*41)>>2; + int8_t sign=(x<0?1:0); + //int8_t sign=(uint8_t) x>>7; + x-=sign; + res-=(x>>6); + res-=sign; + return res; +#else + // return (int16_t)x*10 + x/4 - x/64; + return ((x*41)>>2) - (x>>6); +#endif +} + +int16_t calc100toRESX(int8_t x) // return x*10.24 +{ + return calc100toRESX_16Bits(x); +} + +// return x*1.024 +int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie +{ + // return x + x/32 - x/128 + x/512; + int16_t y = x>>5; + x+=y; + y=y>>2; + x-=y; + return x+(y>>2); +} + +int16_t calcRESXto1000(int16_t x) // return x/1.024 +{ +// *1000/1024 = x - x/32 + x/128 + return (x - (x>>5) + (x>>7)); +} + +int8_t calcRESXto100(int16_t x) +{ + return (x*25) >> 8; +} +#endif + +#if defined(HELI) || defined(FRSKY_HUB) +uint16_t isqrt32(uint32_t n) +{ + uint16_t c = 0x8000; + uint16_t g = 0x8000; + + for (;;) { + if ((uint32_t)g*g > n) + g ^= c; + c >>= 1; + if(c == 0) + return g; + g |= c; + } +} +#endif /* Division by 10 and rounding or fixed point arithmetic values diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp new file mode 100755 index 000000000..d7a3c9ff6 --- /dev/null +++ b/radio/src/mixer.cpp @@ -0,0 +1,1389 @@ +/* + * Authors (alphabetical order) + * - Andre Bernet + * - Andreas Weitl + * - Bertrand Songis + * - Bryan J. Rentoul (Gruvin) + * - Cameron Weeks + * - Erez Raviv + * - Gabriel Birkus + * - Jean-Pierre Parisy + * - Karl Szmutny + * - Michael Blandford + * - Michal Hlavinka + * - Pat Mackenzie + * - Philip Moss + * - Rob Thomson + * - Romolo Manfredini + * - Thomas Husterer + * + * opentx is based on code named + * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, + * er9x by Erez Raviv: http://code.google.com/p/er9x/, + * and the original (and ongoing) project by + * Thomas Husterer, th9x: http://code.google.com/p/th9x/ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include "opentx.h" + +#if defined(PCBTARANIS) + int8_t virtualInputsTrims[NUM_INPUTS]; +#else + int16_t rawAnas[NUM_INPUTS] = {0}; +#endif + +int16_t anas [NUM_INPUTS] = {0}; +int16_t trims[NUM_STICKS] = {0}; +int32_t chans[NUM_CHNOUT] = {0}; +BeepANACenter bpanaCenter = 0; + +int24_t act [MAX_MIXERS] = {0}; +SwOn swOn [MAX_MIXERS]; // TODO better name later... + +uint8_t mixWarning; + +#if defined(MODULE_ALWAYS_SEND_PULSES) + uint8_t startupWarningState; +#endif + +int16_t calibratedStick[NUM_STICKS+NUM_POTS]; +int16_t channelOutputs[NUM_CHNOUT] = {0}; +int16_t ex_chans[NUM_CHNOUT] = {0}; // Outputs (before LIMITS) of the last perMain; + +#if defined(HELI) + int16_t cyc_anas[3] = {0}; + #if defined(PCBTARANIS) + int16_t heliAnas[4] = {0}; + #endif +#endif + +bool s_mixer_first_run_done = false; + +void applyExpos(int16_t *anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS) +{ +#if defined(PCBTARANIS) +#if defined(HELI) + int16_t heliAnasCopy[4]; + memcpy(heliAnasCopy, heliAnas, sizeof(heliAnasCopy)); +#endif +#else + int16_t anas2[NUM_INPUTS]; // values before expo, to ensure same expo base when multiple expo lines are used + memcpy(anas2, anas, sizeof(anas2)); +#endif + + int8_t cur_chn = -1; + + for (uint8_t i=0; ichn == cur_chn) + continue; + if (ed->phases & (1<swtch)) { +#if defined(PCBTARANIS) + int v; + if (ed->srcRaw == ovwrIdx) + v = ovwrValue; +#if defined(HELI) + else if (ed->srcRaw == MIXSRC_Ele) + v = heliAnasCopy[ELE_STICK]; + else if (ed->srcRaw == MIXSRC_Ail) + v = heliAnasCopy[AIL_STICK]; +#endif + else { + v = getValue(ed->srcRaw); + if (ed->srcRaw >= MIXSRC_FIRST_TELEM && ed->scale > 0) { + v = limit(-1024, int((v * 1024) / convertTelemValue(ed->srcRaw-MIXSRC_FIRST_TELEM+1, ed->scale)), 1024); + } + } +#else + int16_t v = anas2[ed->chn]; +#endif + if (EXPO_MODE_ENABLE(ed, v)) { +#if defined(BOLD_FONT) + if (mode==e_perout_mode_normal) swOn[i].activeExpo = true; +#endif + cur_chn = ed->chn; + + //========== CURVE================= +#if defined(PCBTARANIS) + if (ed->curve.value) { + v = applyCurve(v, ed->curve); + } +#else + int8_t curveParam = ed->curveParam; + if (curveParam) { + if (ed->curveMode == MODE_CURVE) + v = applyCurve(v, curveParam); + else + v = expo(v, GET_GVAR(curveParam, -100, 100, s_perout_flight_mode)); + } +#endif + + //========== WEIGHT =============== + int16_t weight = GET_GVAR(ed->weight, MIN_EXPO_WEIGHT, 100, s_perout_flight_mode); + weight = calc100to256(weight); + v = ((int32_t)v * weight) >> 8; + +#if defined(PCBTARANIS) + //========== OFFSET =============== + int16_t offset = GET_GVAR(ed->offset, -100, 100, s_perout_flight_mode); + if (offset) v += calc100toRESX(offset); + + //========== TRIMS ================ + if (ed->carryTrim < TRIM_ON) + virtualInputsTrims[cur_chn] = -ed->carryTrim - 1; + else if (ed->carryTrim == TRIM_ON && ed->srcRaw >= MIXSRC_Rud && ed->srcRaw <= MIXSRC_Ail) + virtualInputsTrims[cur_chn] = ed->srcRaw - MIXSRC_Rud; + else + virtualInputsTrims[cur_chn] = -1; + +#if defined(HELI) + if (ed->srcRaw == MIXSRC_Ele) + heliAnas[ELE_STICK] = v; + else if (ed->srcRaw == MIXSRC_Ail) + heliAnas[AIL_STICK] = v; +#endif +#endif + + anas[cur_chn] = v; + } + } + } +} + +// #define PREVENT_ARITHMETIC_OVERFLOW +// because of optimizations the reserves before overruns occurs is only the half +// this defines enables some checks the greatly improves this situation +// It should nearly prevent all overruns (is still a chance for it, but quite low) +// negative side is code cost 96 bytes flash + +// we do it now half way, only in applyLimits, which costs currently 50bytes +// according opinion poll this topic is currently not very important +// the change below improves already the situation +// the check inside mixer would slow down mix a little bit and costs additionally flash +// also the check inside mixer still is not bulletproof, there may be still situations a overflow could occur +// a bulletproof implementation would take about additional 100bytes flash +// therefore with go with this compromize, interested people could activate this define + +// @@@2 open.20.fsguruh ; +// channel = channelnumber -1; +// value = outputvalue with 100 mulitplied usual range -102400 to 102400; output -1024 to 1024 +// changed rescaling from *100 to *256 to optimize performance +// rescaled from -262144 to 262144 +int16_t applyLimits(uint8_t channel, int32_t value) +{ + LimitData * lim = limitAddress(channel); + +#if defined(PCBTARANIS) + if (lim->curve) { + // TODO we loose precision here, applyCustomCurve could work with int32_t on ARM boards... + if (lim->curve > 0) + value = 256 * applyCustomCurve(value/256, lim->curve-1); + else + value = 256 * applyCustomCurve(-value/256, -lim->curve-1); + } +#endif + + int16_t ofs = LIMIT_OFS_RESX(lim); + int16_t lim_p = LIMIT_MAX_RESX(lim); + int16_t lim_n = LIMIT_MIN_RESX(lim); + + if (ofs > lim_p) ofs = lim_p; + if (ofs < lim_n) ofs = lim_n; + + // because the rescaling optimization would reduce the calculation reserve we activate this for all builds + // it increases the calculation reserve from factor 20,25x to 32x, which it slightly better as original + // without it we would only have 16x which is slightly worse as original, we should not do this + + // thanks to gbirkus, he motivated this change, which greatly reduces overruns + // unfortunately the constants and 32bit compares generates about 50 bytes codes; didn't find a way to get it down. + value = limit(int32_t(-RESXl*256), value, int32_t(RESXl*256)); // saves 2 bytes compared to other solutions up to now + +#if defined(PPM_LIMITS_SYMETRICAL) + if (value) { + int16_t tmp; + if (lim->symetrical) + tmp = (value > 0) ? (lim_p) : (-lim_n); + else + tmp = (value > 0) ? (lim_p - ofs) : (-lim_n + ofs); + value = (int32_t) value * tmp; // div by 1024*256 -> output = -1024..1024 +#else + if (value) { + int16_t tmp = (value > 0) ? (lim_p - ofs) : (-lim_n + ofs); + value = (int32_t) value * tmp; // div by 1024*256 -> output = -1024..1024 +#endif + +#ifdef CORRECT_NEGATIVE_SHIFTS + int8_t sign = (value<0?1:0); + value -= sign; + tmp = value>>16; // that's quite tricky: the shiftright 16 operation is assmbled just with addressmove; just forget the two least significant bytes; + tmp >>= 2; // now one simple shift right for two bytes does the rest + tmp += sign; +#else + tmp = value>>16; // that's quite tricky: the shiftright 16 operation is assmbled just with addressmove; just forget the two least significant bytes; + tmp >>= 2; // now one simple shift right for two bytes does the rest +#endif + + ofs += tmp; // ofs can to added directly because already recalculated, + } + + if (ofs > lim_p) ofs = lim_p; + if (ofs < lim_n) ofs = lim_n; + + if (lim->revert) ofs = -ofs; // finally do the reverse. + + if (safetyCh[channel] != -128) // if safety channel available for channel check + ofs = calc100toRESX(safetyCh[channel]); + + return ofs; +} + +// TODO same naming convention than the putsMixerSource + +getvalue_t getValue(uint8_t i) +{ + if (i==MIXSRC_NONE) return 0; + +#if defined(PCBTARANIS) + else if (i <= MIXSRC_LAST_INPUT) { + return anas[i-MIXSRC_FIRST_INPUT]; + } +#endif + +#if defined(PCBTARANIS) + else if (i q) { + d = isqrt32(v); + } + } +#endif + + for (uint8_t i=0; i [-1024..1024] + uint8_t ch = (i < NUM_STICKS ? CONVERT_MODE(i) : i); + +#if defined(ROTARY_ENCODERS) + int16_t v = ((i < NUM_STICKS+NUM_POTS) ? anaIn(i) : getRotaryEncoder(i-(NUM_STICKS+NUM_POTS))); +#else + int16_t v = anaIn(i); +#endif + +#if !defined(SIMU) + if (i < NUM_STICKS+NUM_POTS) { + if (IS_POT_MULTIPOS(i)) { + v -= RESX; + } + else { + CalibData * calib = &g_eeGeneral.calib[i]; + v -= calib->mid; + v = v * (int32_t)RESX / (max((int16_t)100, (v>0 ? calib->spanPos : calib->spanNeg))); + } + } +#endif + + if (v < -RESX) v = -RESX; + if (v > RESX) v = RESX; + +#if defined(PCBTARANIS) + if (i==POT1 || i==SLIDER1) { + v = -v; + } +#endif + + if (g_model.throttleReversed && ch==THR_STICK) { + v = -v; + } + +#if defined(EXTRA_3POS) + if (i == POT1+EXTRA_3POS-1) { + if (v < -RESX/2) + v = -RESX; + else if (v > +RESX/2) + v = +RESX; + else + v = 0; + } +#endif + + BeepANACenter mask = (BeepANACenter)1 << ch; + + if (i < NUM_STICKS+NUM_POTS) { + + calibratedStick[ch] = v; // for show in expo + + // filtering for center beep + uint8_t tmp = (uint16_t)abs(v) / 16; +#if defined(CPUARM) + if (mode == e_perout_mode_normal) { + if (tmp==0 || (tmp==1 && (bpanaCenter & mask))) { + anaCenter |= mask; + if ((g_model.beepANACenter & mask) && !(bpanaCenter & mask)) { + AUDIO_POT_MIDDLE(i); + } + } + } +#else + if (tmp <= 1) anaCenter |= (tmp==0 ? mask : (bpanaCenter & mask)); +#endif + } + else { + // rotary encoders + if (v == 0) anaCenter |= mask; + } + + if (ch < NUM_STICKS) { //only do this for sticks +#if defined(PCBTARANIS) + if (mode & e_perout_mode_nosticks) { + calibratedStick[ch] = 0; + } +#endif + + if (mode <= e_perout_mode_inactive_phase && isFunctionActive(FUNCTION_TRAINER+ch) && ppmInValid) { + // trainer mode + TrainerMix* td = &g_eeGeneral.trainer.mix[ch]; + if (td->mode) { + uint8_t chStud = td->srcChn; + int32_t vStud = (g_ppmIns[chStud]- g_eeGeneral.trainer.calib[chStud]); + vStud *= td->studWeight; + vStud /= 50; + switch (td->mode) { + case 1: v += vStud; break; // add-mode + case 2: v = vStud; break; // subst-mode + } +#if defined(PCBTARANIS) + calibratedStick[ch] = v; +#endif + } + } + +#if defined(HELI) + if (d && (ch==ELE_STICK || ch==AIL_STICK)) { + v = (int32_t(v) * calc100toRESX(g_model.swashR.value)) / int32_t(d); + } +#if defined(PCBTARANIS) + heliAnas[ch] = v; +#endif +#endif + +#if !defined(PCBTARANIS) + rawAnas[ch] = v; + anas[ch] = v; //set values for mixer +#endif + } + } + + /* TRIMs */ + evalTrims(); + + /* EXPOs */ + applyExpos(anas, mode); + + if (mode == e_perout_mode_normal) { +#if !defined(CPUARM) + anaCenter &= g_model.beepANACenter; + if(((bpanaCenter ^ anaCenter) & anaCenter)) AUDIO_POT_MIDDLE(); +#endif + bpanaCenter = anaCenter; + } +} + +#if defined(PCBTARANIS) + #define HELI_ANAS_ARRAY heliAnas +#else + #define HELI_ANAS_ARRAY anas +#endif + +uint8_t s_perout_flight_mode; +void perOut(uint8_t mode, uint8_t tick10ms) +{ + evalInputs(mode); + +#if defined(MODULE_ALWAYS_SEND_PULSES) + checkStartupWarnings(); +#endif + +#if defined(HELI) + if (g_model.swashR.value) { + uint32_t v = ((int32_t)HELI_ANAS_ARRAY[ELE_STICK]*HELI_ANAS_ARRAY[ELE_STICK] + (int32_t)HELI_ANAS_ARRAY[AIL_STICK]*HELI_ANAS_ARRAY[AIL_STICK]); + uint32_t q = calc100toRESX(g_model.swashR.value); + q *= q; + if (v>q) { + uint16_t d = isqrt32(v); + int16_t tmp = calc100toRESX(g_model.swashR.value); + HELI_ANAS_ARRAY[ELE_STICK] = (int32_t) HELI_ANAS_ARRAY[ELE_STICK]*tmp/d; + HELI_ANAS_ARRAY[AIL_STICK] = (int32_t) HELI_ANAS_ARRAY[AIL_STICK]*tmp/d; + } + } + +#define REZ_SWASH_X(x) ((x) - (x)/8 - (x)/128 - (x)/512) // 1024*sin(60) ~= 886 +#define REZ_SWASH_Y(x) ((x)) // 1024 => 1024 + + if (g_model.swashR.type) { + getvalue_t vp = HELI_ANAS_ARRAY[ELE_STICK]+trims[ELE_STICK]; + getvalue_t vr = HELI_ANAS_ARRAY[AIL_STICK]+trims[AIL_STICK]; + getvalue_t vc = 0; + if (g_model.swashR.collectiveSource) + vc = getValue(g_model.swashR.collectiveSource); + + if (g_model.swashR.invertELE) vp = -vp; + if (g_model.swashR.invertAIL) vr = -vr; + if (g_model.swashR.invertCOL) vc = -vc; + + switch (g_model.swashR.type) { + case SWASH_TYPE_120: + vp = REZ_SWASH_Y(vp); + vr = REZ_SWASH_X(vr); + cyc_anas[0] = vc - vp; + cyc_anas[1] = vc + vp/2 + vr; + cyc_anas[2] = vc + vp/2 - vr; + break; + case SWASH_TYPE_120X: + vp = REZ_SWASH_X(vp); + vr = REZ_SWASH_Y(vr); + cyc_anas[0] = vc - vr; + cyc_anas[1] = vc + vr/2 + vp; + cyc_anas[2] = vc + vr/2 - vp; + break; + case SWASH_TYPE_140: + vp = REZ_SWASH_Y(vp); + vr = REZ_SWASH_Y(vr); + cyc_anas[0] = vc - vp; + cyc_anas[1] = vc + vp + vr; + cyc_anas[2] = vc + vp - vr; + break; + case SWASH_TYPE_90: + vp = REZ_SWASH_Y(vp); + vr = REZ_SWASH_Y(vr); + cyc_anas[0] = vc - vp; + cyc_anas[1] = vc + vr; + cyc_anas[2] = vc - vr; + break; + default: + break; + } + } +#endif + + memclear(chans, sizeof(chans)); // All outputs to 0 + + //========== MIXER LOOP =============== + uint8_t lv_mixWarning = 0; + + uint8_t pass = 0; + + bitfield_channels_t dirtyChannels = (bitfield_channels_t)-1; // all dirty when mixer starts + + do { + + bitfield_channels_t passDirtyChannels = 0; + + for (uint8_t i=0; isrcRaw == 0) break; + + uint8_t stickIndex = md->srcRaw - MIXSRC_Rud; + + if (!(dirtyChannels & ((bitfield_channels_t)1 << md->destCh))) continue; + + // if this is the first calculation for the destination channel, initialize it with 0 (otherwise would be random) + if (i == 0 || md->destCh != (md-1)->destCh) { + chans[md->destCh] = 0; + } + + //========== PHASE && SWITCH ===== + bool mixCondition = (md->phases != 0 || md->swtch); + delayval_t mixEnabled = !(md->phases & (1 << s_perout_flight_mode)) && getSwitch(md->swtch); + + if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !ppmInValid) { + mixEnabled = 0; + } + + //========== VALUE =============== + getvalue_t v = 0; + if (mode > e_perout_mode_inactive_phase) { +#if defined(PCBTARANIS) + if (!mixEnabled) { + continue; + } + else { + v = getValue(md->srcRaw); + } +#else + if (!mixEnabled || stickIndex >= NUM_STICKS || (stickIndex == THR_STICK && g_model.thrTrim)) { + continue; + } + else { + if (!(mode & e_perout_mode_nosticks)) v = anas[stickIndex]; + } +#endif + } + else { +#if !defined(PCBTARANIS) + if (stickIndex < NUM_STICKS) { + v = md->noExpo ? rawAnas[stickIndex] : anas[stickIndex]; + } + else +#endif + { + int8_t srcRaw = MIXSRC_Rud + stickIndex; + v = getValue(srcRaw); + srcRaw -= MIXSRC_CH1; + if (srcRaw>=0 && srcRaw<=MIXSRC_LAST_CH-MIXSRC_CH1 && md->destCh != srcRaw) { + if (dirtyChannels & ((bitfield_channels_t)1 << srcRaw) & (passDirtyChannels|~(((bitfield_channels_t) 1 << md->destCh)-1))) + passDirtyChannels |= (bitfield_channels_t) 1 << md->destCh; + if (srcRaw < md->destCh || pass > 0) + v = chans[srcRaw] >> 8; + } + } + if (!mixCondition) { + mixEnabled = v >> DELAY_POS_SHIFT; + } + } + + bool apply_offset_and_curve = true; + + //========== DELAYS =============== + delayval_t _swOn = swOn[i].now; + delayval_t _swPrev = swOn[i].prev; + bool swTog = (mixEnabled != _swOn); + if (mode==e_perout_mode_normal && swTog) { + if (!swOn[i].delay) _swPrev = _swOn; + swOn[i].delay = (mixEnabled > _swOn ? md->delayUp : md->delayDown) * (100/DELAY_STEP); + swOn[i].now = mixEnabled; + swOn[i].prev = _swPrev; + } + if (mode==e_perout_mode_normal && swOn[i].delay > 0) { + swOn[i].delay = max(0, (int16_t)swOn[i].delay - tick10ms); + if (!mixCondition) + v = _swPrev << DELAY_POS_SHIFT; + else if (mixEnabled) + continue; + } + else if (!mixEnabled) { + if ((md->speedDown || md->speedUp) && md->mltpx!=MLTPX_REP) { + if (mixCondition) { + v = (md->mltpx == MLTPX_ADD ? 0 : RESX); + apply_offset_and_curve = false; + } + } + else if (mixCondition) { + continue; + } + } + + if (mode==e_perout_mode_normal && (!mixCondition || mixEnabled || swOn[i].delay)) { + if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1); +#if defined(BOLD_FONT) + swOn[i].activeMix = true; +#endif + } + + if (apply_offset_and_curve) { +#if !defined(PCBTARANIS) // OFFSET is now applied AFTER weight on Taranis + //========== OFFSET / SOURCE =============== + int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode); + if (offset) v += calc100toRESX_16Bits(offset); +#endif + + //========== TRIMS ================ + if (!(mode & e_perout_mode_notrims)) { +#if defined(PCBTARANIS) + if (md->carryTrim == 0) { + int8_t mix_trim; + if (stickIndex < NUM_STICKS) + mix_trim = stickIndex; + else if (md->srcRaw <= MIXSRC_LAST_INPUT) + mix_trim = virtualInputsTrims[md->srcRaw-1]; + else + mix_trim = -1; + if (mix_trim >= 0) { + int16_t trim = trims[mix_trim]; + if (mix_trim == THR_STICK && g_model.throttleReversed) + v -= trim; + else + v += trim; + } + } +#else + int8_t mix_trim = md->carryTrim; + if (mix_trim < TRIM_ON) + mix_trim = -mix_trim - 1; + else if (mix_trim == TRIM_ON && stickIndex < NUM_STICKS) + mix_trim = stickIndex; + else + mix_trim = -1; + if (mix_trim >= 0) { + int16_t trim = trims[mix_trim]; + if (mix_trim == THR_STICK && g_model.throttleReversed) + v -= trim; + else + v += trim; + } +#endif + } + } + + // saves 12 bytes code if done here and not together with weight; unknown reason + int16_t weight = GET_GVAR(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode); + weight = calc100to256_16Bits(weight); + + //========== SPEED =============== + // now its on input side, but without weight compensation. More like other remote controls + // lower weight causes slower movement + + if (mode <= e_perout_mode_inactive_phase && (md->speedUp || md->speedDown)) { // there are delay values +#define DEL_MULT_SHIFT 8 + // we recale to a mult 256 higher value for calculation + int32_t tact = act[i]; + int16_t diff = v - (tact>>DEL_MULT_SHIFT); + if (diff) { + // open.20.fsguruh: speed is defined in % movement per second; In menu we specify the full movement (-100% to 100%) = 200% in total + // 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 + // because we get a tick each 10msec, we need 100 ticks for one second + // 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 + if (tick10ms || !s_mixer_first_run_done) { + // only if already time is passed add or substract a value according the speed configured + int32_t rate = (int32_t) tick10ms << (DEL_MULT_SHIFT+11); // = DEL_MULT*2048*tick10ms + // rate equals a full range for one second; if less time is passed rate is accordingly smaller + // if one second passed, rate would be 2048 (full motion)*256(recalculated weight)*100(100 ticks needed for one second) + int32_t currentValue = ((int32_t) v< 0) { + if (s_mixer_first_run_done && md->speedUp > 0) { + // if a speed upwards is defined recalculate the new value according configured speed; the higher the speed the smaller the add value is + int32_t newValue = tact+rate/((int16_t)(100/SLOW_STEP)*md->speedUp); + if (newValuespeedDown > 0) { + // see explanation in speedUp + int32_t newValue = tact-rate/((int16_t)(100/SLOW_STEP)*md->speedDown); + if (newValue>currentValue) currentValue = newValue; // Endposition; prevent toggling around the destination + } + } + act[i] = tact = currentValue; + // open.20.fsguruh: this implementation would save about 50 bytes code + } // endif tick10ms ; in case no time passed assign the old value, not the current value from source + v = (tact >> DEL_MULT_SHIFT); + } + } + + //========== CURVES =============== +#if defined(PCBTARANIS) + if (apply_offset_and_curve && md->curve.type != CURVE_REF_DIFF && md->curve.value) { + v = applyCurve(v, md->curve); + } +#else + if (apply_offset_and_curve && md->curveParam && md->curveMode == MODE_CURVE) { + v = applyCurve(v, md->curveParam); + } +#endif + + //========== WEIGHT =============== + int32_t dv = (int32_t) v * weight; + + //========== OFFSET / AFTER =============== +#if defined(PCBTARANIS) + if (apply_offset_and_curve) { + int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode); + if (offset) dv += calc100toRESX_16Bits(offset) << 8; + } +#endif + + //========== DIFFERENTIAL ========= +#if defined(PCBTARANIS) + if (md->curve.type == CURVE_REF_DIFF && md->curve.value) { + dv = applyCurve(dv, md->curve); + } +#else + if (md->curveMode == MODE_DIFFERENTIAL) { + // @@@2 also recalculate curveParam to a 256 basis which ease the calculation later a lot + int16_t curveParam = calc100to256(GET_GVAR(md->curveParam, -100, 100, s_perout_flight_mode)); + if (curveParam > 0 && dv < 0) + dv = (dv * (256 - curveParam)) >> 8; + else if (curveParam < 0 && dv > 0) + dv = (dv * (256 + curveParam)) >> 8; + } +#endif + + int32_t *ptr = &chans[md->destCh]; // Save calculating address several times + + switch (md->mltpx) { + case MLTPX_REP: + *ptr = dv; +#if defined(BOLD_FONT) + if (mode==e_perout_mode_normal) { + for (uint8_t m=i-1; mdestCh==md->destCh; m--) + swOn[m].activeMix = false; + } +#endif + break; + case MLTPX_MUL: + // @@@2 we have to remove the weight factor of 256 in case of 100%; now we use the new base of 256 + dv >>= 8; + dv *= *ptr; + dv >>= RESX_SHIFT; // same as dv /= RESXl; + *ptr = dv; + break; + default: // MLTPX_ADD + *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100); + break; + } //endswitch md->mltpx +#ifdef PREVENT_ARITHMETIC_OVERFLOW +/* + // a lot of assumptions must be true, for this kind of check; not really worth for only 4 bytes flash savings + // this solution would save again 4 bytes flash + int8_t testVar=(*ptr<<1)>>24; + if ( (testVar!=-1) && (testVar!=0 ) ) { + // this devices by 64 which should give a good balance between still over 100% but lower then 32x100%; should be OK + *ptr >>= 6; // this is quite tricky, reduces the value a lot but should be still over 100% and reduces flash need + } */ + + + PACK( union u_int16int32_t { + struct { + int16_t lo; + int16_t hi; + } words_t; + int32_t dword; + }); + + u_int16int32_t tmp; + tmp.dword=*ptr; + + if (tmp.dword<0) { + if ((tmp.words_t.hi&0xFF80)!=0xFF80) tmp.words_t.hi=0xFF86; // set to min nearly + } + else { + if ((tmp.words_t.hi|0x007F)!=0x007F) tmp.words_t.hi=0x0079; // set to max nearly + } + *ptr = tmp.dword; + // this implementation saves 18bytes flash + +/* dv=*ptr>>8; + if (dv>(32767-RESXl)) { + *ptr=(32767-RESXl)<<8; + } else if (dv<(-32767+RESXl)) { + *ptr=(-32767+RESXl)<<8; + }*/ + // *ptr=limit( int32_t(int32_t(-1)<<23), *ptr, int32_t(int32_t(1)<<23)); // limit code cost 72 bytes + // *ptr=limit( int32_t((-32767+RESXl)<<8), *ptr, int32_t((32767-RESXl)<<8)); // limit code cost 80 bytes +#endif + + } //endfor mixers + + tick10ms = 0; + dirtyChannels &= passDirtyChannels; + + } while (++pass < 5 && dirtyChannels); + + mixWarning = lv_mixWarning; +} + +int32_t sum_chans512[NUM_CHNOUT] = {0}; + +MIX_FUNCTION_RESULT doMixerCalculations() +{ +#if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE) + PORTH |= 0x40; // PORTH:6 LOW->HIGH signals start of mixer interrupt +#endif + + static tmr10ms_t lastTMR = 0; + + tmr10ms_t tmr10ms = get_tmr10ms(); + uint8_t tick10ms = (tmr10ms >= lastTMR ? tmr10ms - lastTMR : 1); + // handle tick10ms overrun + // correct overflow handling costs a lot of code; happens only each 11 min; + // therefore forget the exact calculation and use only 1 instead; good compromise + +#if !defined(CPUARM) + lastTMR = tmr10ms; +#endif + + getADC(); + + getSwitchesPosition(lastTMR == 0); + +#if defined(CPUARM) + lastTMR = tmr10ms; +#endif + +#if defined(PCBSKY9X) && !defined(REVA) && !defined(SIMU) + Current_analogue = (Current_analogue*31 + s_anaFilt[8] ) >> 5 ; + if (Current_analogue > Current_max) + Current_max = Current_analogue ; +#elif defined(CPUM2560) && !defined(SIMU) + // For PCB V4, use our own 1.2V, external reference (connected to ADC3) + ADCSRB &= ~(1<> 4) * fp_act[p]; + weight += fp_act[p]; + } + s_last_switch_used = 0; + } + assert(weight); + s_perout_flight_mode = phase; + } + else { + s_perout_flight_mode = phase; + perOut(e_perout_mode_normal, tick10ms); + } + + s_mixer_first_run_done = true; + + //========== FUNCTIONS =============== + // must be done after mixing because some functions use the inputs/channels values + // must be done before limits because of the applyLimit function: it checks for safety switches which would be not initialized otherwise + if (tick10ms) { +#if defined(CPUARM) + requiredSpeakerVolume = g_eeGeneral.speakerVolume + VOLUME_LEVEL_DEF; +#endif + evalFunctions(); + } + + //========== LIMITS =============== + for (uint8_t i=0; i 1024*256 + // later we multiply by the limit (up to 100) and then we need to normalize + // at the end chans[i] = chans[i]/256 => -1024..1024 + // interpolate value with min/max so we get smooth motion from center to stop + // this limits based on v original values and min=-1024, max=1024 RESX=1024 + int32_t q = (s_fade_flight_phases ? (sum_chans512[i] / weight) << 4 : chans[i]); + +#if defined(PCBSTD) + ex_chans[i] = q >> 8; +#else + ex_chans[i] = q / 256; +#endif + + int16_t value = applyLimits(i, q); // applyLimits will remove the 256 100% basis + + cli(); + channelOutputs[i] = value; // copy consistent word to int-level + sei(); + } + +#if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE) + PORTH &= ~0x40; // PORTH:6 HIGH->LOW signals end of mixer interrupt +#endif + + // Bandgap has had plenty of time to settle... +#if !defined(CPUARM) + getADC_bandgap(); +#endif + +#if defined(CPUARM) + if (!tick10ms) return false; //make sure the rest happen only every 10ms. +#else + if (!tick10ms) return; //make sure the rest happen only every 10ms. +#endif + +#if !defined(CPUM64) && !defined(ACCURAT_THROTTLE_TIMER) + // code cost is about 16 bytes for higher throttle accuracy for timer + // would not be noticable anyway, because all version up to this change had only 16 steps; + // now it has already 32 steps; this define would increase to 128 steps + #define ACCURAT_THROTTLE_TIMER +#endif + + /* Throttle trace */ + int16_t val; + + if (g_model.thrTraceSrc > NUM_POTS) { + uint8_t ch = g_model.thrTraceSrc-NUM_POTS-1; + val = channelOutputs[ch]; + + LimitData *lim = limitAddress(ch); + int16_t gModelMax = LIMIT_MAX_RESX(lim); + int16_t gModelMin = LIMIT_MIN_RESX(lim); + + if (lim->revert) + val = -val + gModelMax; + else + val = val - gModelMin; + +#if defined(PPM_LIMITS_SYMETRICAL) + if (lim->symetrical) + val -= calc1000toRESX(lim->offset); +#endif + + + gModelMax -= gModelMin; // we compare difference between Max and Mix for recaling needed; Max and Min are shifted to 0 by default + // usually max is 1024 min is -1024 --> max-min = 2048 full range + +#ifdef ACCURAT_THROTTLE_TIMER + if (gModelMax!=0 && gModelMax!=2048) val = (int32_t) (val << 11) / (gModelMax); // rescaling only needed if Min, Max differs +#else + // @@@ open.20.fsguruh optimized calculation; now *8 /8 instead of 10 base; (*16/16 already cause a overrun; unsigned calculation also not possible, because v may be negative) + gModelMax+=255; // force rounding up --> gModelMax is bigger --> val is smaller + gModelMax >>= (10-2); + + if (gModelMax!=0 && gModelMax!=8) { + val = (val << 3) / gModelMax; // rescaling only needed if Min, Max differs + } +#endif + + if (val<0) val=0; // prevent val be negative, which would corrupt throttle trace and timers; could occur if safetyswitch is smaller than limits + } + else { +#ifdef PCBTARANIS + val = RESX + calibratedStick[g_model.thrTraceSrc == 0 ? THR_STICK : g_model.thrTraceSrc+NUM_STICKS-1]; +#else + val = RESX + rawAnas[g_model.thrTraceSrc == 0 ? THR_STICK : g_model.thrTraceSrc+NUM_STICKS-1]; +#endif + } + +#if defined(ACCURAT_THROTTLE_TIMER) + val >>= (RESX_SHIFT-6); // calibrate it (resolution increased by factor 4) +#else + val >>= (RESX_SHIFT-4); // calibrate it +#endif + + // Timers start + for (uint8_t i=0; istate == TMR_OFF) { + timerState->state = TMR_RUNNING; + timerState->cnt = 0; + timerState->sum = 0; + } + + // value for time described in timer->mode + // OFFABSTHsTH%THt + if (tm == TMRMODE_THR_REL) { + timerState->cnt++; + timerState->sum+=val; + } + + if ((timerState->val_10ms += tick10ms) >= 100) { + timerState->val_10ms -= 100 ; + int16_t newTimerVal = timerState->val; + if (tv) newTimerVal = tv - newTimerVal; + + if (tm == TMRMODE_ABS) { + newTimerVal++; + } + else if (tm == TMRMODE_THR) { + if (val) newTimerVal++; + } + else if (tm == TMRMODE_THR_REL) { + // @@@ open.20.fsguruh: why so complicated? we have already a s_sum field; use it for the half seconds (not showable) as well + // check for s_cnt[i]==0 is not needed because we are shure it is at least 1 +#if defined(ACCURAT_THROTTLE_TIMER) + if ((timerState->sum/timerState->cnt)>=128) { // throttle was normalized to 0 to 128 value (throttle/64*2 (because - range is added as well) + newTimerVal++; // add second used of throttle + timerState->sum-=128*timerState->cnt; + } +#else + if ((timerState->sum/timerState->cnt)>=32) { // throttle was normalized to 0 to 32 value (throttle/16*2 (because - range is added as well) + newTimerVal++; // add second used of throttle + timerState->sum-=32*timerState->cnt; + } +#endif + timerState->cnt=0; + } + else if (tm == TMRMODE_THR_TRG) { + if (val || newTimerVal > 0) + newTimerVal++; + } + else { + if (tm > 0) tm -= (TMR_VAROFS-1); + if (getSwitch(tm)) + newTimerVal++; + } + + switch (timerState->state) { + case TMR_RUNNING: + if (tv && newTimerVal>=(int16_t)tv) { + AUDIO_TIMER_00(g_model.timers[i].countdownBeep); + timerState->state = TMR_NEGATIVE; + } + break; + case TMR_NEGATIVE: + if (newTimerVal >= (int16_t)tv + MAX_ALERT_TIME) timerState->state = TMR_STOPPED; + break; + } + + if (tv) newTimerVal = tv - newTimerVal; // if counting backwards - display backwards + + if (newTimerVal != timerState->val) { + timerState->val = newTimerVal; + if (timerState->state == TMR_RUNNING) { + if (g_model.timers[i].countdownBeep && g_model.timers[i].start) { + if (newTimerVal==30) AUDIO_TIMER_30(); + if (newTimerVal==20) AUDIO_TIMER_20(); + if (newTimerVal<=10) AUDIO_TIMER_LT10(g_model.timers[i].countdownBeep, newTimerVal); + } + if (g_model.timers[i].minuteBeep && (newTimerVal % 60)==0) { + AUDIO_TIMER_MINUTE(newTimerVal); + } + } + } + } + } + } //endfor timer loop (only two) + + static uint8_t s_cnt_100ms; + static uint8_t s_cnt_1s; + static uint8_t s_cnt_samples_thr_1s; + static uint16_t s_sum_samples_thr_1s; + + s_cnt_samples_thr_1s++; + s_sum_samples_thr_1s+=val; + + if ((s_cnt_100ms += tick10ms) >= 10) { // 0.1sec + s_cnt_100ms -= 10; + s_cnt_1s += 1; + + for (uint8_t i=0; ifunc == LS_FUNC_TIMER) { + int16_t *lastValue = &csLastValue[i]; + if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { + *lastValue = -cswTimerValue(cs->v1); + } + else if (*lastValue < 0) { + if (++(*lastValue) == 0) + *lastValue = cswTimerValue(cs->v2); + } + else { // if (*lastValue > 0) + *lastValue -= 1; + } + } + else if (cs->func == LS_FUNC_STICKY) { + PACK(typedef struct { + uint8_t state; + uint8_t last; + }) cs_sticky_struct; + cs_sticky_struct & lastValue = (cs_sticky_struct &)csLastValue[i]; + bool before = lastValue.last & 0x01; + if (lastValue.state) { + bool now = getSwitch(cs->v2); + if (now != before) { + lastValue.last ^= 1; + if (!before) { + lastValue.state = 0; + } + } + } + else { + bool now = getSwitch(cs->v1); + if (before != now) { + lastValue.last ^= 1; + if (!before) { + lastValue.state = 1; + } + } + } + } +#if defined(CPUARM) + else if (cs->func == LS_FUNC_STAY) { + PACK(typedef struct { + uint16_t state:1; + uint16_t duration:15; + }) cs_stay_struct; + + cs_stay_struct & lastValue = (cs_stay_struct &)csLastValue[i]; + lastValue.state = false; + bool state = getSwitch(cs->v1); + if (state) { + if (cs->v3 == 0 && lastValue.duration == cswTimerValue(cs->v2)) + lastValue.state = true; + if (lastValue.duration < 1000) + lastValue.duration++; + } + else { + if (lastValue.duration > cswTimerValue(cs->v2) && lastValue.duration <= cswTimerValue(cs->v2+cs->v3)) + lastValue.state = true; + lastValue.duration = 0; + } + } +#endif + } + + if (s_cnt_1s >= 10) { // 1sec + s_cnt_1s -= 10; + s_timeCumTot += 1; + + struct t_inactivity *ptrInactivity = &inactivity; + FORCE_INDIRECT(ptrInactivity) ; + ptrInactivity->counter++; + if ((((uint8_t)ptrInactivity->counter)&0x07)==0x01 && g_eeGeneral.inactivityTimer && g_vbat100mV>50 && ptrInactivity->counter > ((uint16_t)g_eeGeneral.inactivityTimer*60)) + AUDIO_INACTIVITY(); + +#if defined(AUDIO) + if (mixWarning & 1) if ((s_timeCumTot&0x03)==0) AUDIO_MIX_WARNING(1); + if (mixWarning & 2) if ((s_timeCumTot&0x03)==1) AUDIO_MIX_WARNING(2); + if (mixWarning & 4) if ((s_timeCumTot&0x03)==2) AUDIO_MIX_WARNING(3); +#endif + +#if defined(ACCURAT_THROTTLE_TIMER) + val = s_sum_samples_thr_1s / s_cnt_samples_thr_1s; + s_timeCum16ThrP += (val>>3); // s_timeCum16ThrP would overrun if we would store throttle value with higher accuracy; therefore stay with 16 steps + if (val) s_timeCumThr += 1; + s_sum_samples_thr_1s>>=2; // correct better accuracy now, because trace graph can show this information; in case thrtrace is not active, the compile should remove this +#else + val = s_sum_samples_thr_1s / s_cnt_samples_thr_1s; + s_timeCum16ThrP += (val>>1); + if (val) s_timeCumThr += 1; +#endif + +#if defined(THRTRACE) + // throttle trace is done every 10 seconds; Tracebuffer is adjusted to screen size. + // in case buffer runs out, it wraps around + // resolution for y axis is only 32, therefore no higher value makes sense + s_cnt_samples_thr_10s += s_cnt_samples_thr_1s; + s_sum_samples_thr_10s += s_sum_samples_thr_1s; + + if (++s_cnt_10s >= 10) { // 10s + s_cnt_10s -= 10; + val = s_sum_samples_thr_10s / s_cnt_samples_thr_10s; + s_sum_samples_thr_10s = 0; + s_cnt_samples_thr_10s = 0; + + s_traceBuf[s_traceWr++] = val; + if (s_traceWr >= MAXTRACE) s_traceWr = 0; + if (s_traceCnt >= 0) s_traceCnt++; + } +#endif + + s_cnt_samples_thr_1s = 0; + s_sum_samples_thr_1s = 0; + } + } + + if (s_fade_flight_phases) { + uint16_t tick_delta = delta * tick10ms; + for (uint8_t p=0; p tick_delta) + fp_act[p] += tick_delta; + else { + fp_act[p] = MAX_ACT; + s_fade_flight_phases -= phaseMask; + } + } + else { + if (fp_act[p] > tick_delta) + fp_act[p] -= tick_delta; + else { + fp_act[p] = 0; + s_fade_flight_phases -= phaseMask; + } + } + } + } + } //endif s_fade_fligh_phases + +#if defined(DSM2) + static uint8_t count_dsm_range = 0; + if (dsm2Flag & (DSM2_BIND_FLAG | DSM2_RANGECHECK_FLAG)) { + if (++count_dsm_range >= 200) { + AUDIO_PLAY(AU_FRSKY_CHEEP); + count_dsm_range = 0; + } + } +#endif + +#if defined(PXX) + static uint8_t count_pxx = 0; + for (uint8_t i = 0; i < NUM_MODULES; i++) { + if (pxxFlag[i] & (PXX_SEND_RANGECHECK | PXX_SEND_RXNUM)) { + if (++count_pxx >= 250) { + AUDIO_PLAY(AU_FRSKY_CHEEP); + count_pxx = 0; + } + } + } +#endif + +#if defined(CPUARM) + return true; +#endif +} diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 8afc6e5f1..6066433ed 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -36,26 +36,6 @@ #include "opentx.h" -// static variables used in perOut - moved here so they don't interfere with the stack -// It's also easier to initialize them here. -#if defined(PCBTARANIS) -int8_t virtualInputsTrims[NUM_INPUTS]; -#else -int16_t rawAnas[NUM_INPUTS] = {0}; -#endif -int16_t anas [NUM_INPUTS] = {0}; -int16_t trims[NUM_STICKS] = {0}; -int32_t chans[NUM_CHNOUT] = {0}; -BeepANACenter bpanaCenter = 0; - -int24_t act [MAX_MIXERS] = {0}; -SwOn swOn [MAX_MIXERS]; // TODO better name later... - -uint8_t mixWarning; -#if defined(MODULE_ALWAYS_SEND_PULSES) - uint8_t startupWarningState; -#endif - #if defined(CPUARM) #define MENUS_STACK_SIZE 2000 #define MIXER_STACK_SIZE 500 @@ -163,8 +143,6 @@ int8_t safetyCh[NUM_CHNOUT]; union ReusableBuffer reusableBuffer; -const pm_char s_charTab[] PROGMEM = "_-.,"; - const pm_uint8_t bchout_ar[] PROGMEM = { 0x1B, 0x1E, 0x27, 0x2D, 0x36, 0x39, 0x4B, 0x4E, 0x63, 0x6C, 0x72, 0x78, @@ -188,111 +166,6 @@ const pm_uint8_t modn12x3[] PROGMEM = { 3, 1, 2, 0, 3, 2, 1, 0 }; -char idx2char(int8_t idx) -{ - if (idx == 0) return ' '; - if (idx < 0) { - if (idx > -27) return 'a' - idx - 1; - idx = -idx; - } - if (idx < 27) return 'A' + idx - 1; - if (idx < 37) return '0' + idx - 27; - if (idx <= 40) return pgm_read_byte(s_charTab+idx-37); -#if LEN_SPECIAL_CHARS > 0 - if (idx <= ZCHAR_MAX) return 'z' + 5 + idx - 40; -#endif - return ' '; -} - -#if defined(CPUARM) || defined(SIMU) -int8_t char2idx(char c) -{ - if (c == '_') return 37; -#if LEN_SPECIAL_CHARS > 0 - if (c < 0 && c+128 <= LEN_SPECIAL_CHARS) return 41 + (c+128); -#endif - if (c >= 'a') return 'a' - c - 1; - if (c >= 'A') return c - 'A' + 1; - if (c >= '0') return c - '0' + 27; - if (c == '-') return 38; - if (c == '.') return 39; - if (c == ',') return 40; - return 0; -} - -void str2zchar(char *dest, const char *src, int size) -{ - memset(dest, 0, size); - for (int c=0; c= 0 && dest[size] == ' '); -} -#endif - -#if defined(CPUARM) -bool zexist(const char *str, uint8_t size) -{ - for (int i=0; i 0) { - if (str[size-1] != 0) - return size; - size--; - } - return size; -} - -char * strcat_zchar(char * dest, char * name, uint8_t size, const char *defaultName, uint8_t defaultNameSize, uint8_t defaultIdx) -{ - int8_t len = 0; - - if (name) { - memcpy(dest, name, size); - dest[size] = '\0'; - - int8_t i = size-1; - - while (i>=0) { - if (!len && dest[i]) - len = i+1; - if (len) { - if (dest[i]) - dest[i] = idx2char(dest[i]); - else - dest[i] = '_'; - } - i--; - } - } - - if (len == 0 && defaultName) { - strcpy(dest, defaultName); - dest[defaultNameSize] = (char)((defaultIdx / 10) + '0'); - dest[defaultNameSize + 1] = (char)((defaultIdx % 10) + '0'); - len = defaultNameSize + 2; - } - - return &dest[len]; -} -#endif - volatile tmr10ms_t g_tmr10ms; #if defined(CPUARM) @@ -555,403 +428,6 @@ void modelDefault(uint8_t id) g_model.mavlink.pc_rssi_en = 1; #endif } - - -#if defined(HELI) - int16_t cyc_anas[3] = {0}; -#if defined(PCBTARANIS) - int16_t heliAnas[4] = {0}; -#endif -#endif - -void applyExpos(int16_t *anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS) -{ -#if defined(PCBTARANIS) -#if defined(HELI) - int16_t heliAnasCopy[4]; - memcpy(heliAnasCopy, heliAnas, sizeof(heliAnasCopy)); -#endif -#else - int16_t anas2[NUM_INPUTS]; // values before expo, to ensure same expo base when multiple expo lines are used - memcpy(anas2, anas, sizeof(anas2)); -#endif - - int8_t cur_chn = -1; - - for (uint8_t i=0; ichn == cur_chn) - continue; - if (ed->phases & (1<swtch)) { -#if defined(PCBTARANIS) - int v; - if (ed->srcRaw == ovwrIdx) - v = ovwrValue; -#if defined(HELI) - else if (ed->srcRaw == MIXSRC_Ele) - v = heliAnasCopy[ELE_STICK]; - else if (ed->srcRaw == MIXSRC_Ail) - v = heliAnasCopy[AIL_STICK]; -#endif - else { - v = getValue(ed->srcRaw); - if (ed->srcRaw >= MIXSRC_FIRST_TELEM && ed->scale > 0) { - v = limit(-1024, int((v * 1024) / convertTelemValue(ed->srcRaw-MIXSRC_FIRST_TELEM+1, ed->scale)), 1024); - } - } -#else - int16_t v = anas2[ed->chn]; -#endif - if (EXPO_MODE_ENABLE(ed, v)) { -#if defined(BOLD_FONT) - if (mode==e_perout_mode_normal) swOn[i].activeExpo = true; -#endif - cur_chn = ed->chn; - - //========== CURVE================= -#if defined(PCBTARANIS) - if (ed->curve.value) { - v = applyCurve(v, ed->curve); - } -#else - int8_t curveParam = ed->curveParam; - if (curveParam) { - if (ed->curveMode == MODE_CURVE) - v = applyCurve(v, curveParam); - else - v = expo(v, GET_GVAR(curveParam, -100, 100, s_perout_flight_mode)); - } -#endif - - //========== WEIGHT =============== - int16_t weight = GET_GVAR(ed->weight, MIN_EXPO_WEIGHT, 100, s_perout_flight_mode); - weight = calc100to256(weight); - v = ((int32_t)v * weight) >> 8; - -#if defined(PCBTARANIS) - //========== OFFSET =============== - int16_t offset = GET_GVAR(ed->offset, -100, 100, s_perout_flight_mode); - if (offset) v += calc100toRESX(offset); - - //========== TRIMS ================ - if (ed->carryTrim < TRIM_ON) - virtualInputsTrims[cur_chn] = -ed->carryTrim - 1; - else if (ed->carryTrim == TRIM_ON && ed->srcRaw >= MIXSRC_Rud && ed->srcRaw <= MIXSRC_Ail) - virtualInputsTrims[cur_chn] = ed->srcRaw - MIXSRC_Rud; - else - virtualInputsTrims[cur_chn] = -1; - -#if defined(HELI) - if (ed->srcRaw == MIXSRC_Ele) - heliAnas[ELE_STICK] = v; - else if (ed->srcRaw == MIXSRC_Ail) - heliAnas[AIL_STICK] = v; -#endif -#endif - - anas[cur_chn] = v; - } - } - } -} - -#if !defined(CPUARM) - -// #define CORRECT_NEGATIVE_SHIFTS -// open.20.fsguruh; shift right operations do the rounding different for negative values compared to positive values -// so all negative divisions round always further down, which give absolute values bigger compared to a usual division -// this is noticable on the display, because instead of -100.0 -99.9 is shown; While in praxis I doublt somebody will notice a -// difference this is more a mental thing. Maybe people are distracted, because the easy calculations are obviously wrong -// this define would correct this, but costs 34 bytes code for stock version - -// currently we set this to active always, because it might cause a fault about 1% compared positive and negative values -// is done now in makefile - -int16_t calc100to256_16Bits(int16_t x) // return x*2.56 -{ - // y = 2*x + x/2 +x/16-x/512-x/2048 - // 512 and 2048 are out of scope from int8 input --> forget it -#ifdef CORRECT_NEGATIVE_SHIFTS - int16_t res=(int16_t)x<<1; - //int8_t sign=(uint8_t) x>>7; - int8_t sign=(x<0?1:0); - - x-=sign; - res+=(x>>1); - res+=sign; - res+=(x>>4); - res+=sign; - return res; -#else - return ((int16_t)x<<1)+(x>>1)+(x>>4); -#endif -} - -int16_t calc100to256(int8_t x) // return x*2.56 -{ - return calc100to256_16Bits(x); -} - -int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24 -{ -#ifdef CORRECT_NEGATIVE_SHIFTS - int16_t res= ((int16_t)x*41)>>2; - int8_t sign=(x<0?1:0); - //int8_t sign=(uint8_t) x>>7; - x-=sign; - res-=(x>>6); - res-=sign; - return res; -#else - // return (int16_t)x*10 + x/4 - x/64; - return ((x*41)>>2) - (x>>6); -#endif -} - -int16_t calc100toRESX(int8_t x) // return x*10.24 -{ - return calc100toRESX_16Bits(x); -} - -// return x*1.024 -int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie -{ - // return x + x/32 - x/128 + x/512; - int16_t y = x>>5; - x+=y; - y=y>>2; - x-=y; - return x+(y>>2); -} - -int16_t calcRESXto1000(int16_t x) // return x/1.024 -{ -// *1000/1024 = x - x/32 + x/128 - return (x - (x>>5) + (x>>7)); -} - -int8_t calcRESXto100(int16_t x) -{ - return (x*25) >> 8; -} - -#endif - - -// #define PREVENT_ARITHMETIC_OVERFLOW -// because of optimizations the reserves before overruns occurs is only the half -// this defines enables some checks the greatly improves this situation -// It should nearly prevent all overruns (is still a chance for it, but quite low) -// negative side is code cost 96 bytes flash - -// we do it now half way, only in applyLimits, which costs currently 50bytes -// according opinion poll this topic is currently not very important -// the change below improves already the situation -// the check inside mixer would slow down mix a little bit and costs additionally flash -// also the check inside mixer still is not bulletproof, there may be still situations a overflow could occur -// a bulletproof implementation would take about additional 100bytes flash -// therefore with go with this compromize, interested people could activate this define - -// @@@2 open.20.fsguruh ; -// channel = channelnumber -1; -// value = outputvalue with 100 mulitplied usual range -102400 to 102400; output -1024 to 1024 -// changed rescaling from *100 to *256 to optimize performance -// rescaled from -262144 to 262144 -int16_t applyLimits(uint8_t channel, int32_t value) -{ - LimitData * lim = limitAddress(channel); - -#if defined(PCBTARANIS) - if (lim->curve) { - // TODO we loose precision here, applyCustomCurve could work with int32_t on ARM boards... - if (lim->curve > 0) - value = 256 * applyCustomCurve(value/256, lim->curve-1); - else - value = 256 * applyCustomCurve(-value/256, -lim->curve-1); - } -#endif - - int16_t ofs = LIMIT_OFS_RESX(lim); - int16_t lim_p = LIMIT_MAX_RESX(lim); - int16_t lim_n = LIMIT_MIN_RESX(lim); - - if (ofs > lim_p) ofs = lim_p; - if (ofs < lim_n) ofs = lim_n; - - // because the rescaling optimization would reduce the calculation reserve we activate this for all builds - // it increases the calculation reserve from factor 20,25x to 32x, which it slightly better as original - // without it we would only have 16x which is slightly worse as original, we should not do this - - // thanks to gbirkus, he motivated this change, which greatly reduces overruns - // unfortunately the constants and 32bit compares generates about 50 bytes codes; didn't find a way to get it down. - value = limit(int32_t(-RESXl*256), value, int32_t(RESXl*256)); // saves 2 bytes compared to other solutions up to now - -#if defined(PPM_LIMITS_SYMETRICAL) - if (value) { - int16_t tmp; - if (lim->symetrical) - tmp = (value > 0) ? (lim_p) : (-lim_n); - else - tmp = (value > 0) ? (lim_p - ofs) : (-lim_n + ofs); - value = (int32_t) value * tmp; // div by 1024*256 -> output = -1024..1024 -#else - if (value) { - int16_t tmp = (value > 0) ? (lim_p - ofs) : (-lim_n + ofs); - value = (int32_t) value * tmp; // div by 1024*256 -> output = -1024..1024 -#endif - -#ifdef CORRECT_NEGATIVE_SHIFTS - int8_t sign = (value<0?1:0); - value -= sign; - tmp = value>>16; // that's quite tricky: the shiftright 16 operation is assmbled just with addressmove; just forget the two least significant bytes; - tmp >>= 2; // now one simple shift right for two bytes does the rest - tmp += sign; -#else - tmp = value>>16; // that's quite tricky: the shiftright 16 operation is assmbled just with addressmove; just forget the two least significant bytes; - tmp >>= 2; // now one simple shift right for two bytes does the rest -#endif - - ofs += tmp; // ofs can to added directly because already recalculated, - } - - if (ofs > lim_p) ofs = lim_p; - if (ofs < lim_n) ofs = lim_n; - - if (lim->revert) ofs = -ofs; // finally do the reverse. - - if (safetyCh[channel] != -128) // if safety channel available for channel check - ofs = calc100toRESX(safetyCh[channel]); - - return ofs; -} - -int16_t calibratedStick[NUM_STICKS+NUM_POTS]; -int16_t channelOutputs[NUM_CHNOUT] = {0}; -int16_t ex_chans[NUM_CHNOUT] = {0}; // Outputs (before LIMITS) of the last perMain; - -// TODO same naming convention than the putsMixerSource - -getvalue_t getValue(uint8_t i) -{ - if (i==MIXSRC_NONE) return 0; - -#if defined(PCBTARANIS) - else if (i <= MIXSRC_LAST_INPUT) { - return anas[i-MIXSRC_FIRST_INPUT]; - } -#endif - -#if defined(PCBTARANIS) - else if (i n) - g ^= c; - c >>= 1; - if(c == 0) - return g; - g |= c; - } -} -#endif - FORCEINLINE void evalTrims() { uint8_t phase = s_perout_flight_mode; @@ -2165,152 +1622,6 @@ FORCEINLINE void evalTrims() } } -void evalInputs(uint8_t mode) -{ - BeepANACenter anaCenter = 0; - -#if defined(HELI) - uint16_t d = 0; - if (g_model.swashR.value) { - uint32_t v = (int32_t(calibratedStick[ELE_STICK])*calibratedStick[ELE_STICK] + int32_t(calibratedStick[AIL_STICK])*calibratedStick[AIL_STICK]); - uint32_t q = calc100toRESX(g_model.swashR.value); - q *= q; - if (v > q) { - d = isqrt32(v); - } - } -#endif - - for (uint8_t i=0; i [-1024..1024] - uint8_t ch = (i < NUM_STICKS ? CONVERT_MODE(i) : i); - -#if defined(ROTARY_ENCODERS) - int16_t v = ((i < NUM_STICKS+NUM_POTS) ? anaIn(i) : getRotaryEncoder(i-(NUM_STICKS+NUM_POTS))); -#else - int16_t v = anaIn(i); -#endif - -#if !defined(SIMU) - if (i < NUM_STICKS+NUM_POTS) { - if (IS_POT_MULTIPOS(i)) { - v -= RESX; - } - else { - CalibData * calib = &g_eeGeneral.calib[i]; - v -= calib->mid; - v = v * (int32_t)RESX / (max((int16_t)100, (v>0 ? calib->spanPos : calib->spanNeg))); - } - } -#endif - - if (v < -RESX) v = -RESX; - if (v > RESX) v = RESX; - -#if defined(PCBTARANIS) - if (i==POT1 || i==SLIDER1) { - v = -v; - } -#endif - - if (g_model.throttleReversed && ch==THR_STICK) { - v = -v; - } - -#if defined(EXTRA_3POS) - if (i == POT1+EXTRA_3POS-1) { - if (v < -RESX/2) - v = -RESX; - else if (v > +RESX/2) - v = +RESX; - else - v = 0; - } -#endif - - BeepANACenter mask = (BeepANACenter)1 << ch; - - if (i < NUM_STICKS+NUM_POTS) { - - calibratedStick[ch] = v; // for show in expo - - // filtering for center beep - uint8_t tmp = (uint16_t)abs(v) / 16; -#if defined(CPUARM) - if (mode == e_perout_mode_normal) { - if (tmp==0 || (tmp==1 && (bpanaCenter & mask))) { - anaCenter |= mask; - if ((g_model.beepANACenter & mask) && !(bpanaCenter & mask)) { - AUDIO_POT_MIDDLE(i); - } - } - } -#else - if (tmp <= 1) anaCenter |= (tmp==0 ? mask : (bpanaCenter & mask)); -#endif - } - else { - // rotary encoders - if (v == 0) anaCenter |= mask; - } - - if (ch < NUM_STICKS) { //only do this for sticks -#if defined(PCBTARANIS) - if (mode & e_perout_mode_nosticks) { - calibratedStick[ch] = 0; - } -#endif - - if (mode <= e_perout_mode_inactive_phase && isFunctionActive(FUNCTION_TRAINER+ch) && ppmInValid) { - // trainer mode - TrainerMix* td = &g_eeGeneral.trainer.mix[ch]; - if (td->mode) { - uint8_t chStud = td->srcChn; - int32_t vStud = (g_ppmIns[chStud]- g_eeGeneral.trainer.calib[chStud]); - vStud *= td->studWeight; - vStud /= 50; - switch (td->mode) { - case 1: v += vStud; break; // add-mode - case 2: v = vStud; break; // subst-mode - } -#if defined(PCBTARANIS) - calibratedStick[ch] = v; -#endif - } - } - -#if defined(HELI) - if (d && (ch==ELE_STICK || ch==AIL_STICK)) { - v = (int32_t(v) * calc100toRESX(g_model.swashR.value)) / int32_t(d); - } -#if defined(PCBTARANIS) - heliAnas[ch] = v; -#endif -#endif - -#if !defined(PCBTARANIS) - rawAnas[ch] = v; - anas[ch] = v; //set values for mixer -#endif - } - } - - /* TRIMs */ - evalTrims(); - - /* EXPOs */ - applyExpos(anas, mode); - - if (mode == e_perout_mode_normal) { -#if !defined(CPUARM) - anaCenter &= g_model.beepANACenter; - if(((bpanaCenter ^ anaCenter) & anaCenter)) AUDIO_POT_MIDDLE(); -#endif - bpanaCenter = anaCenter; - } -} - #if defined(DEBUG) /* * This is a test function for debugging purpose, you may insert there your code and compile with the option DEBUG=YES @@ -2781,883 +2092,9 @@ void evalFunctions() #endif } -#if defined(PCBTARANIS) - #define HELI_ANAS_ARRAY heliAnas -#else - #define HELI_ANAS_ARRAY anas -#endif - -uint8_t s_perout_flight_mode; -void perOut(uint8_t mode, uint8_t tick10ms) -{ - evalInputs(mode); - -#if defined(MODULE_ALWAYS_SEND_PULSES) - checkStartupWarnings(); -#endif - -#if defined(HELI) - if (g_model.swashR.value) { - uint32_t v = ((int32_t)HELI_ANAS_ARRAY[ELE_STICK]*HELI_ANAS_ARRAY[ELE_STICK] + (int32_t)HELI_ANAS_ARRAY[AIL_STICK]*HELI_ANAS_ARRAY[AIL_STICK]); - uint32_t q = calc100toRESX(g_model.swashR.value); - q *= q; - if (v>q) { - uint16_t d = isqrt32(v); - int16_t tmp = calc100toRESX(g_model.swashR.value); - HELI_ANAS_ARRAY[ELE_STICK] = (int32_t) HELI_ANAS_ARRAY[ELE_STICK]*tmp/d; - HELI_ANAS_ARRAY[AIL_STICK] = (int32_t) HELI_ANAS_ARRAY[AIL_STICK]*tmp/d; - } - } - -#define REZ_SWASH_X(x) ((x) - (x)/8 - (x)/128 - (x)/512) // 1024*sin(60) ~= 886 -#define REZ_SWASH_Y(x) ((x)) // 1024 => 1024 - - if (g_model.swashR.type) { - getvalue_t vp = HELI_ANAS_ARRAY[ELE_STICK]+trims[ELE_STICK]; - getvalue_t vr = HELI_ANAS_ARRAY[AIL_STICK]+trims[AIL_STICK]; - getvalue_t vc = 0; - if (g_model.swashR.collectiveSource) - vc = getValue(g_model.swashR.collectiveSource); - - if (g_model.swashR.invertELE) vp = -vp; - if (g_model.swashR.invertAIL) vr = -vr; - if (g_model.swashR.invertCOL) vc = -vc; - - switch (g_model.swashR.type) { - case SWASH_TYPE_120: - vp = REZ_SWASH_Y(vp); - vr = REZ_SWASH_X(vr); - cyc_anas[0] = vc - vp; - cyc_anas[1] = vc + vp/2 + vr; - cyc_anas[2] = vc + vp/2 - vr; - break; - case SWASH_TYPE_120X: - vp = REZ_SWASH_X(vp); - vr = REZ_SWASH_Y(vr); - cyc_anas[0] = vc - vr; - cyc_anas[1] = vc + vr/2 + vp; - cyc_anas[2] = vc + vr/2 - vp; - break; - case SWASH_TYPE_140: - vp = REZ_SWASH_Y(vp); - vr = REZ_SWASH_Y(vr); - cyc_anas[0] = vc - vp; - cyc_anas[1] = vc + vp + vr; - cyc_anas[2] = vc + vp - vr; - break; - case SWASH_TYPE_90: - vp = REZ_SWASH_Y(vp); - vr = REZ_SWASH_Y(vr); - cyc_anas[0] = vc - vp; - cyc_anas[1] = vc + vr; - cyc_anas[2] = vc - vr; - break; - default: - break; - } - } -#endif - - memclear(chans, sizeof(chans)); // All outputs to 0 - - //========== MIXER LOOP =============== - uint8_t lv_mixWarning = 0; - - uint8_t pass = 0; - - bitfield_channels_t dirtyChannels = (bitfield_channels_t)-1; // all dirty when mixer starts - - do { - - bitfield_channels_t passDirtyChannels = 0; - - for (uint8_t i=0; isrcRaw == 0) break; - - uint8_t stickIndex = md->srcRaw - MIXSRC_Rud; - - if (!(dirtyChannels & ((bitfield_channels_t)1 << md->destCh))) continue; - - // if this is the first calculation for the destination channel, initialize it with 0 (otherwise would be random) - if (i == 0 || md->destCh != (md-1)->destCh) { - chans[md->destCh] = 0; - } - - //========== PHASE && SWITCH ===== - bool mixCondition = (md->phases != 0 || md->swtch); - delayval_t mixEnabled = !(md->phases & (1 << s_perout_flight_mode)) && getSwitch(md->swtch); - - if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !ppmInValid) { - mixEnabled = 0; - } - - //========== VALUE =============== - getvalue_t v = 0; - if (mode > e_perout_mode_inactive_phase) { -#if defined(PCBTARANIS) - if (!mixEnabled) { - continue; - } - else { - v = getValue(md->srcRaw); - } -#else - if (!mixEnabled || stickIndex >= NUM_STICKS || (stickIndex == THR_STICK && g_model.thrTrim)) { - continue; - } - else { - if (!(mode & e_perout_mode_nosticks)) v = anas[stickIndex]; - } -#endif - } - else { -#if !defined(PCBTARANIS) - if (stickIndex < NUM_STICKS) { - v = md->noExpo ? rawAnas[stickIndex] : anas[stickIndex]; - } - else -#endif - { - int8_t srcRaw = MIXSRC_Rud + stickIndex; - v = getValue(srcRaw); - srcRaw -= MIXSRC_CH1; - if (srcRaw>=0 && srcRaw<=MIXSRC_LAST_CH-MIXSRC_CH1 && md->destCh != srcRaw) { - if (dirtyChannels & ((bitfield_channels_t)1 << srcRaw) & (passDirtyChannels|~(((bitfield_channels_t) 1 << md->destCh)-1))) - passDirtyChannels |= (bitfield_channels_t) 1 << md->destCh; - if (srcRaw < md->destCh || pass > 0) - v = chans[srcRaw] >> 8; - } - } - if (!mixCondition) { - mixEnabled = v >> DELAY_POS_SHIFT; - } - } - - bool apply_offset_and_curve = true; - - //========== DELAYS =============== - delayval_t _swOn = swOn[i].now; - delayval_t _swPrev = swOn[i].prev; - bool swTog = (mixEnabled != _swOn); - if (mode==e_perout_mode_normal && swTog) { - if (!swOn[i].delay) _swPrev = _swOn; - swOn[i].delay = (mixEnabled > _swOn ? md->delayUp : md->delayDown) * (100/DELAY_STEP); - swOn[i].now = mixEnabled; - swOn[i].prev = _swPrev; - } - if (mode==e_perout_mode_normal && swOn[i].delay > 0) { - swOn[i].delay = max(0, (int16_t)swOn[i].delay - tick10ms); - if (!mixCondition) - v = _swPrev << DELAY_POS_SHIFT; - else if (mixEnabled) - continue; - } - else if (!mixEnabled) { - if ((md->speedDown || md->speedUp) && md->mltpx!=MLTPX_REP) { - if (mixCondition) { - v = (md->mltpx == MLTPX_ADD ? 0 : RESX); - apply_offset_and_curve = false; - } - } - else if (mixCondition) { - continue; - } - } - - if (mode==e_perout_mode_normal && (!mixCondition || mixEnabled || swOn[i].delay)) { - if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1); -#if defined(BOLD_FONT) - swOn[i].activeMix = true; -#endif - } - - if (apply_offset_and_curve) { -#if !defined(PCBTARANIS) // OFFSET is now applied AFTER weight on Taranis - //========== OFFSET / SOURCE =============== - int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode); - if (offset) v += calc100toRESX_16Bits(offset); -#endif - - //========== TRIMS ================ - if (!(mode & e_perout_mode_notrims)) { -#if defined(PCBTARANIS) - if (md->carryTrim == 0) { - int8_t mix_trim; - if (stickIndex < NUM_STICKS) - mix_trim = stickIndex; - else if (md->srcRaw <= MIXSRC_LAST_INPUT) - mix_trim = virtualInputsTrims[md->srcRaw-1]; - else - mix_trim = -1; - if (mix_trim >= 0) { - int16_t trim = trims[mix_trim]; - if (mix_trim == THR_STICK && g_model.throttleReversed) - v -= trim; - else - v += trim; - } - } -#else - int8_t mix_trim = md->carryTrim; - if (mix_trim < TRIM_ON) - mix_trim = -mix_trim - 1; - else if (mix_trim == TRIM_ON && stickIndex < NUM_STICKS) - mix_trim = stickIndex; - else - mix_trim = -1; - if (mix_trim >= 0) { - int16_t trim = trims[mix_trim]; - if (mix_trim == THR_STICK && g_model.throttleReversed) - v -= trim; - else - v += trim; - } -#endif - } - } - - // saves 12 bytes code if done here and not together with weight; unknown reason - int16_t weight = GET_GVAR(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode); - weight = calc100to256_16Bits(weight); - - //========== SPEED =============== - // now its on input side, but without weight compensation. More like other remote controls - // lower weight causes slower movement - - if (mode <= e_perout_mode_inactive_phase && (md->speedUp || md->speedDown)) { // there are delay values -#define DEL_MULT_SHIFT 8 - // we recale to a mult 256 higher value for calculation - int32_t tact = act[i]; - int16_t diff = v - (tact>>DEL_MULT_SHIFT); - if (diff) { - // open.20.fsguruh: speed is defined in % movement per second; In menu we specify the full movement (-100% to 100%) = 200% in total - // 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 - // because we get a tick each 10msec, we need 100 ticks for one second - // 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 - if (tick10ms || !s_mixer_first_run_done) { - // only if already time is passed add or substract a value according the speed configured - int32_t rate = (int32_t) tick10ms << (DEL_MULT_SHIFT+11); // = DEL_MULT*2048*tick10ms - // rate equals a full range for one second; if less time is passed rate is accordingly smaller - // if one second passed, rate would be 2048 (full motion)*256(recalculated weight)*100(100 ticks needed for one second) - int32_t currentValue = ((int32_t) v< 0) { - if (s_mixer_first_run_done && md->speedUp > 0) { - // if a speed upwards is defined recalculate the new value according configured speed; the higher the speed the smaller the add value is - int32_t newValue = tact+rate/((int16_t)(100/SLOW_STEP)*md->speedUp); - if (newValuespeedDown > 0) { - // see explanation in speedUp - int32_t newValue = tact-rate/((int16_t)(100/SLOW_STEP)*md->speedDown); - if (newValue>currentValue) currentValue = newValue; // Endposition; prevent toggling around the destination - } - } - act[i] = tact = currentValue; - // open.20.fsguruh: this implementation would save about 50 bytes code - } // endif tick10ms ; in case no time passed assign the old value, not the current value from source - v = (tact >> DEL_MULT_SHIFT); - } - } - - //========== CURVES =============== -#if defined(PCBTARANIS) - if (apply_offset_and_curve && md->curve.type != CURVE_REF_DIFF && md->curve.value) { - v = applyCurve(v, md->curve); - } -#else - if (apply_offset_and_curve && md->curveParam && md->curveMode == MODE_CURVE) { - v = applyCurve(v, md->curveParam); - } -#endif - - //========== WEIGHT =============== - int32_t dv = (int32_t) v * weight; - - //========== OFFSET / AFTER =============== -#if defined(PCBTARANIS) - if (apply_offset_and_curve) { - int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode); - if (offset) dv += calc100toRESX_16Bits(offset) << 8; - } -#endif - - //========== DIFFERENTIAL ========= -#if defined(PCBTARANIS) - if (md->curve.type == CURVE_REF_DIFF && md->curve.value) { - dv = applyCurve(dv, md->curve); - } -#else - if (md->curveMode == MODE_DIFFERENTIAL) { - // @@@2 also recalculate curveParam to a 256 basis which ease the calculation later a lot - int16_t curveParam = calc100to256(GET_GVAR(md->curveParam, -100, 100, s_perout_flight_mode)); - if (curveParam > 0 && dv < 0) - dv = (dv * (256 - curveParam)) >> 8; - else if (curveParam < 0 && dv > 0) - dv = (dv * (256 + curveParam)) >> 8; - } -#endif - - int32_t *ptr = &chans[md->destCh]; // Save calculating address several times - - switch (md->mltpx) { - case MLTPX_REP: - *ptr = dv; -#if defined(BOLD_FONT) - if (mode==e_perout_mode_normal) { - for (uint8_t m=i-1; mdestCh==md->destCh; m--) - swOn[m].activeMix = false; - } -#endif - break; - case MLTPX_MUL: - // @@@2 we have to remove the weight factor of 256 in case of 100%; now we use the new base of 256 - dv >>= 8; - dv *= *ptr; - dv >>= RESX_SHIFT; // same as dv /= RESXl; - *ptr = dv; - break; - default: // MLTPX_ADD - *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100); - break; - } //endswitch md->mltpx -#ifdef PREVENT_ARITHMETIC_OVERFLOW -/* - // a lot of assumptions must be true, for this kind of check; not really worth for only 4 bytes flash savings - // this solution would save again 4 bytes flash - int8_t testVar=(*ptr<<1)>>24; - if ( (testVar!=-1) && (testVar!=0 ) ) { - // this devices by 64 which should give a good balance between still over 100% but lower then 32x100%; should be OK - *ptr >>= 6; // this is quite tricky, reduces the value a lot but should be still over 100% and reduces flash need - } */ - - - PACK( union u_int16int32_t { - struct { - int16_t lo; - int16_t hi; - } words_t; - int32_t dword; - }); - - u_int16int32_t tmp; - tmp.dword=*ptr; - - if (tmp.dword<0) { - if ((tmp.words_t.hi&0xFF80)!=0xFF80) tmp.words_t.hi=0xFF86; // set to min nearly - } - else { - if ((tmp.words_t.hi|0x007F)!=0x007F) tmp.words_t.hi=0x0079; // set to max nearly - } - *ptr = tmp.dword; - // this implementation saves 18bytes flash - -/* dv=*ptr>>8; - if (dv>(32767-RESXl)) { - *ptr=(32767-RESXl)<<8; - } else if (dv<(-32767+RESXl)) { - *ptr=(-32767+RESXl)<<8; - }*/ - // *ptr=limit( int32_t(int32_t(-1)<<23), *ptr, int32_t(int32_t(1)<<23)); // limit code cost 72 bytes - // *ptr=limit( int32_t((-32767+RESXl)<<8), *ptr, int32_t((32767-RESXl)<<8)); // limit code cost 80 bytes -#endif - - } //endfor mixers - - tick10ms = 0; - dirtyChannels &= passDirtyChannels; - - } while (++pass < 5 && dirtyChannels); - - mixWarning = lv_mixWarning; -} #define TIME_TO_WRITE() (s_eeDirtyMsk && (tmr10ms_t)(get_tmr10ms() - s_eeDirtyTime10ms) >= (tmr10ms_t)WRITE_DELAY_10MS) -int32_t sum_chans512[NUM_CHNOUT] = {0}; - -#if defined(CPUARM) -bool doMixerCalculations() -#else -void doMixerCalculations() -#endif -{ -#if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE) - PORTH |= 0x40; // PORTH:6 LOW->HIGH signals start of mixer interrupt -#endif - - static tmr10ms_t lastTMR = 0; - - tmr10ms_t tmr10ms = get_tmr10ms(); - uint8_t tick10ms = (tmr10ms >= lastTMR ? tmr10ms - lastTMR : 1); - // handle tick10ms overrun - // correct overflow handling costs a lot of code; happens only each 11 min; - // therefore forget the exact calculation and use only 1 instead; good compromise - -#if !defined(CPUARM) - lastTMR = tmr10ms; -#endif - - getADC(); - - getSwitchesPosition(lastTMR == 0); - -#if defined(CPUARM) - lastTMR = tmr10ms; -#endif - -#if defined(PCBSKY9X) && !defined(REVA) && !defined(SIMU) - Current_analogue = (Current_analogue*31 + s_anaFilt[8] ) >> 5 ; - if (Current_analogue > Current_max) - Current_max = Current_analogue ; -#elif defined(CPUM2560) && !defined(SIMU) - // For PCB V4, use our own 1.2V, external reference (connected to ADC3) - ADCSRB &= ~(1<> 4) * fp_act[p]; - weight += fp_act[p]; - } - s_last_switch_used = 0; - } - assert(weight); - s_perout_flight_mode = phase; - } - else { - s_perout_flight_mode = phase; - perOut(e_perout_mode_normal, tick10ms); - } - - s_mixer_first_run_done = true; - - //========== FUNCTIONS =============== - // must be done after mixing because some functions use the inputs/channels values - // must be done before limits because of the applyLimit function: it checks for safety switches which would be not initialized otherwise - if (tick10ms) { -#if defined(CPUARM) - requiredSpeakerVolume = g_eeGeneral.speakerVolume + VOLUME_LEVEL_DEF; -#endif - evalFunctions(); - } - - //========== LIMITS =============== - for (uint8_t i=0; i 1024*256 - // later we multiply by the limit (up to 100) and then we need to normalize - // at the end chans[i] = chans[i]/256 => -1024..1024 - // interpolate value with min/max so we get smooth motion from center to stop - // this limits based on v original values and min=-1024, max=1024 RESX=1024 - int32_t q = (s_fade_flight_phases ? (sum_chans512[i] / weight) << 4 : chans[i]); - -#if defined(PCBSTD) - ex_chans[i] = q >> 8; -#else - ex_chans[i] = q / 256; -#endif - - int16_t value = applyLimits(i, q); // applyLimits will remove the 256 100% basis - - cli(); - channelOutputs[i] = value; // copy consistent word to int-level - sei(); - } - -#if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE) - PORTH &= ~0x40; // PORTH:6 HIGH->LOW signals end of mixer interrupt -#endif - - // Bandgap has had plenty of time to settle... -#if !defined(CPUARM) - getADC_bandgap(); -#endif - -#if defined(CPUARM) - if (!tick10ms) return false; //make sure the rest happen only every 10ms. -#else - if (!tick10ms) return; //make sure the rest happen only every 10ms. -#endif - -#if !defined(CPUM64) && !defined(ACCURAT_THROTTLE_TIMER) - // code cost is about 16 bytes for higher throttle accuracy for timer - // would not be noticable anyway, because all version up to this change had only 16 steps; - // now it has already 32 steps; this define would increase to 128 steps - #define ACCURAT_THROTTLE_TIMER -#endif - - /* Throttle trace */ - int16_t val; - - if (g_model.thrTraceSrc > NUM_POTS) { - uint8_t ch = g_model.thrTraceSrc-NUM_POTS-1; - val = channelOutputs[ch]; - - LimitData *lim = limitAddress(ch); - int16_t gModelMax = LIMIT_MAX_RESX(lim); - int16_t gModelMin = LIMIT_MIN_RESX(lim); - - if (lim->revert) - val = -val + gModelMax; - else - val = val - gModelMin; - -#if defined(PPM_LIMITS_SYMETRICAL) - if (lim->symetrical) - val -= calc1000toRESX(lim->offset); -#endif - - - gModelMax -= gModelMin; // we compare difference between Max and Mix for recaling needed; Max and Min are shifted to 0 by default - // usually max is 1024 min is -1024 --> max-min = 2048 full range - -#ifdef ACCURAT_THROTTLE_TIMER - if (gModelMax!=0 && gModelMax!=2048) val = (int32_t) (val << 11) / (gModelMax); // rescaling only needed if Min, Max differs -#else - // @@@ open.20.fsguruh optimized calculation; now *8 /8 instead of 10 base; (*16/16 already cause a overrun; unsigned calculation also not possible, because v may be negative) - gModelMax+=255; // force rounding up --> gModelMax is bigger --> val is smaller - gModelMax >>= (10-2); - - if (gModelMax!=0 && gModelMax!=8) { - val = (val << 3) / gModelMax; // rescaling only needed if Min, Max differs - } -#endif - - if (val<0) val=0; // prevent val be negative, which would corrupt throttle trace and timers; could occur if safetyswitch is smaller than limits - } - else { -#ifdef PCBTARANIS - val = RESX + calibratedStick[g_model.thrTraceSrc == 0 ? THR_STICK : g_model.thrTraceSrc+NUM_STICKS-1]; -#else - val = RESX + rawAnas[g_model.thrTraceSrc == 0 ? THR_STICK : g_model.thrTraceSrc+NUM_STICKS-1]; -#endif - } - -#if defined(ACCURAT_THROTTLE_TIMER) - val >>= (RESX_SHIFT-6); // calibrate it (resolution increased by factor 4) -#else - val >>= (RESX_SHIFT-4); // calibrate it -#endif - - // Timers start - for (uint8_t i=0; istate == TMR_OFF) { - timerState->state = TMR_RUNNING; - timerState->cnt = 0; - timerState->sum = 0; - } - - // value for time described in timer->mode - // OFFABSTHsTH%THt - if (tm == TMRMODE_THR_REL) { - timerState->cnt++; - timerState->sum+=val; - } - - if ((timerState->val_10ms += tick10ms) >= 100) { - timerState->val_10ms -= 100 ; - int16_t newTimerVal = timerState->val; - if (tv) newTimerVal = tv - newTimerVal; - - if (tm == TMRMODE_ABS) { - newTimerVal++; - } - else if (tm == TMRMODE_THR) { - if (val) newTimerVal++; - } - else if (tm == TMRMODE_THR_REL) { - // @@@ open.20.fsguruh: why so complicated? we have already a s_sum field; use it for the half seconds (not showable) as well - // check for s_cnt[i]==0 is not needed because we are shure it is at least 1 -#if defined(ACCURAT_THROTTLE_TIMER) - if ((timerState->sum/timerState->cnt)>=128) { // throttle was normalized to 0 to 128 value (throttle/64*2 (because - range is added as well) - newTimerVal++; // add second used of throttle - timerState->sum-=128*timerState->cnt; - } -#else - if ((timerState->sum/timerState->cnt)>=32) { // throttle was normalized to 0 to 32 value (throttle/16*2 (because - range is added as well) - newTimerVal++; // add second used of throttle - timerState->sum-=32*timerState->cnt; - } -#endif - timerState->cnt=0; - } - else if (tm == TMRMODE_THR_TRG) { - if (val || newTimerVal > 0) - newTimerVal++; - } - else { - if (tm > 0) tm -= (TMR_VAROFS-1); - if (getSwitch(tm)) - newTimerVal++; - } - - switch (timerState->state) { - case TMR_RUNNING: - if (tv && newTimerVal>=(int16_t)tv) { - AUDIO_TIMER_00(g_model.timers[i].countdownBeep); - timerState->state = TMR_NEGATIVE; - } - break; - case TMR_NEGATIVE: - if (newTimerVal >= (int16_t)tv + MAX_ALERT_TIME) timerState->state = TMR_STOPPED; - break; - } - - if (tv) newTimerVal = tv - newTimerVal; // if counting backwards - display backwards - - if (newTimerVal != timerState->val) { - timerState->val = newTimerVal; - if (timerState->state == TMR_RUNNING) { - if (g_model.timers[i].countdownBeep && g_model.timers[i].start) { - if (newTimerVal==30) AUDIO_TIMER_30(); - if (newTimerVal==20) AUDIO_TIMER_20(); - if (newTimerVal<=10) AUDIO_TIMER_LT10(g_model.timers[i].countdownBeep, newTimerVal); - } - if (g_model.timers[i].minuteBeep && (newTimerVal % 60)==0) { - AUDIO_TIMER_MINUTE(newTimerVal); - } - } - } - } - } - } //endfor timer loop (only two) - - static uint8_t s_cnt_100ms; - static uint8_t s_cnt_1s; - static uint8_t s_cnt_samples_thr_1s; - static uint16_t s_sum_samples_thr_1s; - - s_cnt_samples_thr_1s++; - s_sum_samples_thr_1s+=val; - - if ((s_cnt_100ms += tick10ms) >= 10) { // 0.1sec - s_cnt_100ms -= 10; - s_cnt_1s += 1; - - for (uint8_t i=0; ifunc == LS_FUNC_TIMER) { - int16_t *lastValue = &csLastValue[i]; - if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) { - *lastValue = -cswTimerValue(cs->v1); - } - else if (*lastValue < 0) { - if (++(*lastValue) == 0) - *lastValue = cswTimerValue(cs->v2); - } - else { // if (*lastValue > 0) - *lastValue -= 1; - } - } - else if (cs->func == LS_FUNC_STICKY) { - PACK(typedef struct { - uint8_t state; - uint8_t last; - }) cs_sticky_struct; - cs_sticky_struct & lastValue = (cs_sticky_struct &)csLastValue[i]; - bool before = lastValue.last & 0x01; - if (lastValue.state) { - bool now = getSwitch(cs->v2); - if (now != before) { - lastValue.last ^= 1; - if (!before) { - lastValue.state = 0; - } - } - } - else { - bool now = getSwitch(cs->v1); - if (before != now) { - lastValue.last ^= 1; - if (!before) { - lastValue.state = 1; - } - } - } - } -#if defined(CPUARM) - else if (cs->func == LS_FUNC_STAY) { - PACK(typedef struct { - uint16_t state:1; - uint16_t duration:15; - }) cs_stay_struct; - - cs_stay_struct & lastValue = (cs_stay_struct &)csLastValue[i]; - lastValue.state = false; - bool state = getSwitch(cs->v1); - if (state) { - if (cs->v3 == 0 && lastValue.duration == cswTimerValue(cs->v2)) - lastValue.state = true; - if (lastValue.duration < 1000) - lastValue.duration++; - } - else { - if (lastValue.duration > cswTimerValue(cs->v2) && lastValue.duration <= cswTimerValue(cs->v2+cs->v3)) - lastValue.state = true; - lastValue.duration = 0; - } - } -#endif - } - - if (s_cnt_1s >= 10) { // 1sec - s_cnt_1s -= 10; - s_timeCumTot += 1; - - struct t_inactivity *ptrInactivity = &inactivity; - FORCE_INDIRECT(ptrInactivity) ; - ptrInactivity->counter++; - if ((((uint8_t)ptrInactivity->counter)&0x07)==0x01 && g_eeGeneral.inactivityTimer && g_vbat100mV>50 && ptrInactivity->counter > ((uint16_t)g_eeGeneral.inactivityTimer*60)) - AUDIO_INACTIVITY(); - -#if defined(AUDIO) - if (mixWarning & 1) if ((s_timeCumTot&0x03)==0) AUDIO_MIX_WARNING(1); - if (mixWarning & 2) if ((s_timeCumTot&0x03)==1) AUDIO_MIX_WARNING(2); - if (mixWarning & 4) if ((s_timeCumTot&0x03)==2) AUDIO_MIX_WARNING(3); -#endif - -#if defined(ACCURAT_THROTTLE_TIMER) - val = s_sum_samples_thr_1s / s_cnt_samples_thr_1s; - s_timeCum16ThrP += (val>>3); // s_timeCum16ThrP would overrun if we would store throttle value with higher accuracy; therefore stay with 16 steps - if (val) s_timeCumThr += 1; - s_sum_samples_thr_1s>>=2; // correct better accuracy now, because trace graph can show this information; in case thrtrace is not active, the compile should remove this -#else - val = s_sum_samples_thr_1s / s_cnt_samples_thr_1s; - s_timeCum16ThrP += (val>>1); - if (val) s_timeCumThr += 1; -#endif - -#if defined(THRTRACE) - // throttle trace is done every 10 seconds; Tracebuffer is adjusted to screen size. - // in case buffer runs out, it wraps around - // resolution for y axis is only 32, therefore no higher value makes sense - s_cnt_samples_thr_10s += s_cnt_samples_thr_1s; - s_sum_samples_thr_10s += s_sum_samples_thr_1s; - - if (++s_cnt_10s >= 10) { // 10s - s_cnt_10s -= 10; - val = s_sum_samples_thr_10s / s_cnt_samples_thr_10s; - s_sum_samples_thr_10s = 0; - s_cnt_samples_thr_10s = 0; - - s_traceBuf[s_traceWr++] = val; - if (s_traceWr >= MAXTRACE) s_traceWr = 0; - if (s_traceCnt >= 0) s_traceCnt++; - } -#endif - - s_cnt_samples_thr_1s = 0; - s_sum_samples_thr_1s = 0; - } - } - - if (s_fade_flight_phases) { - uint16_t tick_delta = delta * tick10ms; - for (uint8_t p=0; p tick_delta) - fp_act[p] += tick_delta; - else { - fp_act[p] = MAX_ACT; - s_fade_flight_phases -= phaseMask; - } - } - else { - if (fp_act[p] > tick_delta) - fp_act[p] -= tick_delta; - else { - fp_act[p] = 0; - s_fade_flight_phases -= phaseMask; - } - } - } - } - } //endif s_fade_fligh_phases - -#if defined(DSM2) - static uint8_t count_dsm_range = 0; - if (dsm2Flag & (DSM2_BIND_FLAG | DSM2_RANGECHECK_FLAG)) { - if (++count_dsm_range >= 200) { - AUDIO_PLAY(AU_FRSKY_CHEEP); - count_dsm_range = 0; - } - } -#endif - -#if defined(PXX) - static uint8_t count_pxx = 0; - for (uint8_t i = 0; i < NUM_MODULES; i++) { - if (pxxFlag[i] & (PXX_SEND_RANGECHECK | PXX_SEND_RXNUM)) { - if (++count_pxx >= 250) { - AUDIO_PLAY(AU_FRSKY_CHEEP); - count_pxx = 0; - } - } - } -#endif - -#if defined(CPUARM) - return true; -#endif -} #if defined(NAVIGATION_STICKS) uint8_t StickScrollAllowed; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 1431448fa..25d0e6e07 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -668,6 +668,15 @@ extern uint8_t s_perout_flight_mode; #endif void perOut(uint8_t mode, uint8_t tick10ms); + +#if defined(CPUARM) + #define MIX_FUNCTION_RESULT bool +#else + #define MIX_FUNCTION_RESULT void +#endif + +MIX_FUNCTION_RESULT doMixerCalculations(); + void perMain(); NOINLINE void per10ms(); @@ -905,6 +914,8 @@ enum Analogs { NUMBER_ANALOG }; +void checkBacklight(); + #if defined(PCBSTD) && defined(VOICE) && !defined(SIMU) #define BACKLIGHT_ON() (Voice.Backlight = 1) #define BACKLIGHT_OFF() (Voice.Backlight = 0) @@ -1095,6 +1106,20 @@ struct CurveInfo { extern CurveInfo curveInfo(uint8_t idx); #endif +// static variables used in perOut - moved here so they don't interfere with the stack +// It's also easier to initialize them here. +#if defined(PCBTARANIS) + extern int8_t virtualInputsTrims[NUM_INPUTS]; +#else + extern int16_t rawAnas[NUM_INPUTS]; +#endif + +extern int16_t anas [NUM_INPUTS]; +extern int16_t trims[NUM_STICKS]; +extern BeepANACenter bpanaCenter; + +extern bool s_mixer_first_run_done; + extern int8_t s_currCh; uint8_t getExpoMixCount(uint8_t expo); void deleteExpoMix(uint8_t expo, uint8_t idx); @@ -1103,9 +1128,12 @@ void applyDefaultTemplate(); void incSubtrim(uint8_t idx, int16_t inc); void instantTrim(); +FORCEINLINE void evalTrims(); void copyTrimsToOffset(uint8_t ch); void moveTrimsToOffsets(); +void evalFunctions(); + #if defined(CPUARM) #define ACTIVE_PHASES_TYPE uint16_t #define DELAY_POS_SHIFT 0 diff --git a/radio/src/strhelpers.cpp b/radio/src/strhelpers.cpp index cec0e031a..c0868b66b 100755 --- a/radio/src/strhelpers.cpp +++ b/radio/src/strhelpers.cpp @@ -1,5 +1,149 @@ -#include +/* + * Authors (alphabetical order) + * - Andre Bernet + * - Andreas Weitl + * - Bertrand Songis + * - Bryan J. Rentoul (Gruvin) + * - Cameron Weeks + * - Erez Raviv + * - Gabriel Birkus + * - Jean-Pierre Parisy + * - Karl Szmutny + * - Michael Blandford + * - Michal Hlavinka + * - Pat Mackenzie + * - Philip Moss + * - Rob Thomson + * - Romolo Manfredini + * - Thomas Husterer + * + * opentx is based on code named + * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, + * er9x by Erez Raviv: http://code.google.com/p/er9x/, + * and the original (and ongoing) project by + * Thomas Husterer, th9x: http://code.google.com/p/th9x/ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ +#include "opentx.h" + +const pm_char s_charTab[] PROGMEM = "_-.,"; + +char idx2char(int8_t idx) +{ + if (idx == 0) return ' '; + if (idx < 0) { + if (idx > -27) return 'a' - idx - 1; + idx = -idx; + } + if (idx < 27) return 'A' + idx - 1; + if (idx < 37) return '0' + idx - 27; + if (idx <= 40) return pgm_read_byte(s_charTab+idx-37); +#if LEN_SPECIAL_CHARS > 0 + if (idx <= ZCHAR_MAX) return 'z' + 5 + idx - 40; +#endif + return ' '; +} + +#if defined(CPUARM) || defined(SIMU) +int8_t char2idx(char c) +{ + if (c == '_') return 37; +#if LEN_SPECIAL_CHARS > 0 + if (c < 0 && c+128 <= LEN_SPECIAL_CHARS) return 41 + (c+128); +#endif + if (c >= 'a') return 'a' - c - 1; + if (c >= 'A') return c - 'A' + 1; + if (c >= '0') return c - '0' + 27; + if (c == '-') return 38; + if (c == '.') return 39; + if (c == ',') return 40; + return 0; +} + +void str2zchar(char *dest, const char *src, int size) +{ + memset(dest, 0, size); + for (int c=0; c= 0 && dest[size] == ' '); +} +#endif + +#if defined(CPUARM) +bool zexist(const char *str, uint8_t size) +{ + for (int i=0; i 0) { + if (str[size-1] != 0) + return size; + size--; + } + return size; +} + +char * strcat_zchar(char * dest, char * name, uint8_t size, const char *defaultName, uint8_t defaultNameSize, uint8_t defaultIdx) +{ + int8_t len = 0; + + if (name) { + memcpy(dest, name, size); + dest[size] = '\0'; + + int8_t i = size-1; + + while (i>=0) { + if (!len && dest[i]) + len = i+1; + if (len) { + if (dest[i]) + dest[i] = idx2char(dest[i]); + else + dest[i] = '_'; + } + i--; + } + } + + if (len == 0 && defaultName) { + strcpy(dest, defaultName); + dest[defaultNameSize] = (char)((defaultIdx / 10) + '0'); + dest[defaultNameSize + 1] = (char)((defaultIdx % 10) + '0'); + len = defaultNameSize + 2; + } + + return &dest[len]; +} +#endif + +#if defined(CPUARM) || defined(SDCARD) char * strAppend(char * dest, const char * source) { while ((*dest++ = *source++)) @@ -52,5 +196,5 @@ char * strAppendDate(char * str, bool time) return &str[11]; } } - +#endif #endif