mirror of
https://github.com/opentx/opentx.git
synced 2025-07-13 11:29:51 +03:00
Fixes #2032
This commit is contained in:
bsongis
parent
7e9435c721
commit
4a9c87cc9c
24 changed files with 173 additions and 125 deletions
|
|
@ -63,20 +63,11 @@ int16_t ex_chans[NUM_CHNOUT] = {0}; // Outputs (before LIMITS) of the last perMa
|
|||
|
||||
#if defined(HELI)
|
||||
int16_t cyc_anas[3] = {0};
|
||||
#if defined(PCBTARANIS)
|
||||
int16_t heliAnas[4] = {0};
|
||||
int8_t heliTrims[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
|
||||
#if !defined(PCBTARANIS)
|
||||
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
|
||||
|
|
@ -96,14 +87,9 @@ void applyExpos(int16_t *anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS)
|
|||
if (getSwitch(ed->swtch)) {
|
||||
#if defined(PCBTARANIS)
|
||||
int v;
|
||||
if (ed->srcRaw == ovwrIdx)
|
||||
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) {
|
||||
|
|
@ -152,17 +138,6 @@ void applyExpos(int16_t *anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS)
|
|||
virtualInputsTrims[cur_chn] = ed->srcRaw - MIXSRC_Rud;
|
||||
else
|
||||
virtualInputsTrims[cur_chn] = -1;
|
||||
|
||||
#if defined(HELI)
|
||||
if (ed->srcRaw == MIXSRC_Ele) {
|
||||
heliAnas[ELE_STICK] = v;
|
||||
heliTrims[ELE_STICK] = virtualInputsTrims[cur_chn];
|
||||
}
|
||||
else if (ed->srcRaw == MIXSRC_Ail) {
|
||||
heliAnas[AIL_STICK] = v;
|
||||
heliTrims[AIL_STICK] = virtualInputsTrims[cur_chn];
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
anas[cur_chn] = v;
|
||||
|
|
@ -426,7 +401,13 @@ void evalInputs(uint8_t mode)
|
|||
#if defined(HELI)
|
||||
uint16_t d = 0;
|
||||
if (g_model.swashR.value) {
|
||||
#if defined(PCBTARANIS)
|
||||
int ele = (int16_t)anaIn(ELE_STICK);
|
||||
int ail = (int16_t)anaIn(AIL_STICK);
|
||||
uint32_t v = (ele*ele) + (ail*ail);
|
||||
#else
|
||||
uint32_t v = (int32_t(calibratedStick[ELE_STICK])*calibratedStick[ELE_STICK] + int32_t(calibratedStick[AIL_STICK])*calibratedStick[AIL_STICK]);
|
||||
#endif
|
||||
uint32_t q = calc100toRESX(g_model.swashR.value);
|
||||
q *= q;
|
||||
if (v > q) {
|
||||
|
|
@ -514,7 +495,7 @@ void evalInputs(uint8_t mode)
|
|||
if (ch < NUM_STICKS) { //only do this for sticks
|
||||
#if defined(PCBTARANIS)
|
||||
if (mode & e_perout_mode_nosticks) {
|
||||
v = calibratedStick[ch] = 0;
|
||||
v = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -536,9 +517,6 @@ void evalInputs(uint8_t mode)
|
|||
v = vStud;
|
||||
break;
|
||||
}
|
||||
#if defined(PCBTARANIS)
|
||||
calibratedStick[ch] = v;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -546,12 +524,11 @@ void evalInputs(uint8_t mode)
|
|||
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)
|
||||
#if defined(PCBTARANIS)
|
||||
calibratedStick[ch] = v;
|
||||
#else
|
||||
rawAnas[ch] = v;
|
||||
anas[ch] = v; // set values for mixer
|
||||
#endif
|
||||
|
|
@ -567,18 +544,38 @@ void evalInputs(uint8_t mode)
|
|||
if (mode == e_perout_mode_normal) {
|
||||
#if !defined(CPUARM)
|
||||
anaCenter &= g_model.beepANACenter;
|
||||
if(((bpanaCenter ^ anaCenter) & anaCenter)) AUDIO_POT_MIDDLE();
|
||||
if (((bpanaCenter ^ anaCenter) & anaCenter)) AUDIO_POT_MIDDLE();
|
||||
#endif
|
||||
bpanaCenter = anaCenter;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
#define HELI_ANAS_ARRAY(x) heliAnas[x]
|
||||
#define HELI_TRIMS_ARRAY(x) ((heliTrims[x]>=0) ? trims[heliTrims[x]] : 0)
|
||||
#else
|
||||
#define HELI_ANAS_ARRAY(x) anas[x]
|
||||
#define HELI_TRIMS_ARRAY(x) trims[x]
|
||||
int getStickTrimValue(int stick)
|
||||
{
|
||||
int trim = trims[stick];
|
||||
if (stick == THR_STICK) {
|
||||
if (g_model.thrTrim) {
|
||||
int trimMin = g_model.extendedTrims ? 2*TRIM_EXTENDED_MIN : 2*TRIM_MIN;
|
||||
int v = calibratedStick[THR_STICK];
|
||||
trim = (((g_model.throttleReversed)?(trim+trimMin):(trim-v)) * (RESX-v)) >> (RESX_SHIFT+1);
|
||||
}
|
||||
if (g_model.throttleReversed) {
|
||||
trim = -trim;
|
||||
}
|
||||
}
|
||||
return trim;
|
||||
}
|
||||
|
||||
int getSourceTrimValue(int source)
|
||||
{
|
||||
if (source >= MIXSRC_Rud && source <= MIXSRC_Ail)
|
||||
return getStickTrimValue(source - MIXSRC_Rud);
|
||||
else if (source >= MIXSRC_FIRST_INPUT && source <= MIXSRC_LAST_INPUT)
|
||||
return getStickTrimValue(virtualInputsTrims[source - MIXSRC_FIRST_INPUT]);
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
uint8_t mixerCurrentFlightMode;
|
||||
|
|
@ -593,15 +590,22 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
|
|||
#endif
|
||||
|
||||
#if defined(HELI)
|
||||
#if defined(PCBTARANIS)
|
||||
int heliEleValue = getValue(g_model.swashR.elevatorSource);
|
||||
int heliAilValue = getValue(g_model.swashR.aileronSource);
|
||||
#else
|
||||
int16_t heliEleValue = anas[ELE_STICK];
|
||||
int16_t heliAilValue = anas[AIL_STICK];
|
||||
#endif
|
||||
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 v = ((int32_t)heliEleValue*heliEleValue + (int32_t)heliAilValue*heliAilValue);
|
||||
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;
|
||||
heliEleValue = (int32_t) heliEleValue*tmp/d;
|
||||
heliAilValue = (int32_t) heliAilValue*tmp/d;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -609,8 +613,13 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
|
|||
#define REZ_SWASH_Y(x) ((x)) // 1024 => 1024
|
||||
|
||||
if (g_model.swashR.type) {
|
||||
getvalue_t vp = HELI_ANAS_ARRAY(ELE_STICK) + HELI_TRIMS_ARRAY(ELE_STICK);
|
||||
getvalue_t vr = HELI_ANAS_ARRAY(AIL_STICK) + HELI_TRIMS_ARRAY(AIL_STICK);
|
||||
#if defined(PCBTARANIS)
|
||||
getvalue_t vp = heliEleValue + getSourceTrimValue(g_model.swashR.elevatorSource);
|
||||
getvalue_t vr = heliAilValue + getSourceTrimValue(g_model.swashR.aileronSource);
|
||||
#else
|
||||
getvalue_t vp = heliEleValue + trims[ELE_STICK];
|
||||
getvalue_t vr = heliAilValue + trims[AIL_STICK];
|
||||
#endif
|
||||
getvalue_t vc = 0;
|
||||
if (g_model.swashR.collectiveSource)
|
||||
vc = getValue(g_model.swashR.collectiveSource);
|
||||
|
|
@ -797,26 +806,7 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
|
|||
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) {
|
||||
int32_t trim = trims[mix_trim];
|
||||
if (mix_trim == THR_STICK) {
|
||||
if (g_model.thrTrim) {
|
||||
int16_t trimMin = g_model.extendedTrims ? 2*TRIM_EXTENDED_MIN : 2*TRIM_MIN;
|
||||
trim = (((g_model.throttleReversed)?(trim+trimMin):(trim-trimMin)) * (RESX-v)) >> (RESX_SHIFT+1);
|
||||
}
|
||||
if (g_model.throttleReversed) {
|
||||
trim = -trim;
|
||||
}
|
||||
}
|
||||
v += trim;
|
||||
}
|
||||
v += getSourceTrimValue(md->srcRaw);
|
||||
}
|
||||
#else
|
||||
int8_t mix_trim = md->carryTrim;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue