Fixes #1058
This commit is contained in:
bsongis
2014-05-05 16:07:07 +02:00
parent 7119d069da
commit f178748090
34 changed files with 327 additions and 277 deletions
+48 -42
View File
@@ -769,7 +769,7 @@ int applyCurve(int x, CurveRef & curve)
switch (curve.type) {
case CURVE_REF_DIFF:
{
int curveParam = calc100to256(GET_GVAR(curve.value, -100, 100, s_perout_flight_phase));
int curveParam = calc100to256(GET_GVAR(curve.value, -100, 100, s_perout_flight_mode));
if (curveParam > 0 && x < 0)
x = (x * (256 - curveParam)) >> 8;
else if (curveParam < 0 && x > 0)
@@ -778,7 +778,7 @@ int applyCurve(int x, CurveRef & curve)
}
case CURVE_REF_EXPO:
return expo(x, GET_GVAR(curve.value, -100, 100, s_perout_flight_phase));
return expo(x, GET_GVAR(curve.value, -100, 100, s_perout_flight_mode));
case CURVE_REF_FUNC:
switch (curve.value) {
@@ -981,7 +981,7 @@ void applyExpos(int16_t *anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS)
if (!EXPO_VALID(ed)) break; // end of list
if (ed->chn == cur_chn)
continue;
if (ed->phases & (1<<s_perout_flight_phase))
if (ed->phases & (1<<s_perout_flight_mode))
continue;
if (getSwitch(ed->swtch)) {
#if defined(PCBTARANIS)
@@ -1020,18 +1020,18 @@ void applyExpos(int16_t *anas, uint8_t mode APPLY_EXPOS_EXTRA_PARAMS)
if (ed->curveMode == MODE_CURVE)
v = applyCurve(v, curveParam);
else
v = expo(v, GET_GVAR(curveParam, -100, 100, s_perout_flight_phase));
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_phase);
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_phase);
int16_t offset = GET_GVAR(ed->offset, -100, 100, s_perout_flight_mode);
if (offset) v += calc100toRESX(offset);
//========== TRIMS ================
@@ -1268,7 +1268,7 @@ getvalue_t getValue(uint8_t i)
return 0;
#endif
else if (i<=MIXSRC_TrimAil) return calc1000toRESX((int16_t)8 * getTrimValue(s_perout_flight_phase, i-MIXSRC_TrimRud));
else if (i<=MIXSRC_TrimAil) return calc1000toRESX((int16_t)8 * getTrimValue(s_perout_flight_mode, i-MIXSRC_TrimRud));
#if defined(PCBTARANIS)
else if (i==MIXSRC_SA) return (switchState(SW_SA0) ? -1024 : (switchState(SW_SA1) ? 0 : 1024));
@@ -1279,7 +1279,7 @@ getvalue_t getValue(uint8_t i)
else if (i==MIXSRC_SF) return (switchState(SW_SF0) ? -1024 : 1024);
else if (i==MIXSRC_SG) return (switchState(SW_SG0) ? -1024 : (switchState(SW_SG1) ? 0 : 1024));
else if (i==MIXSRC_SH) return (switchState(SW_SH0) ? -1024 : 1024);
else if (i<=MIXSRC_LAST_LOGICAL_SWITCH) return getSwitch(SWSRC_FIRST_CSW+i-MIXSRC_FIRST_LOGICAL_SWITCH) ? 1024 : -1024;
else if (i<=MIXSRC_LAST_LOGICAL_SWITCH) return getSwitch(SWSRC_FIRST_LOGICAL_SWITCH+i-MIXSRC_FIRST_LOGICAL_SWITCH) ? 1024 : -1024;
#else
else if (i==MIXSRC_3POS) return (getSwitch(SW_ID0-SW_BASE+1) ? -1024 : (getSwitch(SW_ID1-SW_BASE+1) ? 0 : 1024));
// don't use switchState directly to give getSwitch possibility to hack values if needed for switch warning
@@ -1294,7 +1294,7 @@ getvalue_t getValue(uint8_t i)
else if (i<=MIXSRC_LAST_CH) return ex_chans[i-MIXSRC_CH1];
#if defined(GVARS)
else if (i<=MIXSRC_LAST_GVAR) return GVAR_VALUE(i-MIXSRC_GVAR1, getGVarFlightPhase(s_perout_flight_phase, i-MIXSRC_GVAR1));
else if (i<=MIXSRC_LAST_GVAR) return GVAR_VALUE(i-MIXSRC_GVAR1, getGVarFlightPhase(s_perout_flight_mode, i-MIXSRC_GVAR1));
#endif
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_TX_VOLTAGE) return g_vbat100mV;
@@ -1513,9 +1513,15 @@ bool getSwitch(int8_t swtch)
else if (cs_idx == SWSRC_REb) {
result = REB_DOWN();
}
#endif
#if defined(CPUARM) && defined(FLIGHT_MODES)
else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) {
uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE;
result = (idx == s_perout_flight_mode);
}
#endif
else {
cs_idx -= SWSRC_FIRST_CSW;
cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH;
GETSWITCH_RECURSIVE_TYPE mask = ((GETSWITCH_RECURSIVE_TYPE)1 << cs_idx);
if (s_last_switch_used & mask) {
@@ -1801,7 +1807,7 @@ int8_t getMovedSource(GET_MOVED_SOURCE_PARAMS)
#if defined(FLIGHT_MODES)
uint8_t getFlightPhase()
{
for (uint8_t i=1; i<MAX_PHASES; i++) {
for (uint8_t i=1; i<MAX_FLIGHT_MODES; i++) {
PhaseData *phase = &g_model.phaseData[i];
if (phase->swtch && getSwitch(phase->swtch)) {
return i;
@@ -1825,7 +1831,7 @@ int getTrimValue(uint8_t phase, uint8_t idx)
{
#if defined(PCBTARANIS)
int result = 0;
for (uint8_t i=0; i<MAX_PHASES; i++) {
for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
trim_t v = getRawTrimValue(phase, idx);
if (v.mode == TRIM_MODE_NONE) {
return result;
@@ -1852,7 +1858,7 @@ int getTrimValue(uint8_t phase, uint8_t idx)
void setTrimValue(uint8_t phase, uint8_t idx, int trim)
{
#if defined(PCBTARANIS)
for (uint8_t i=0; i<MAX_PHASES; i++) {
for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
trim_t & v = phaseAddress(phase)->trim[idx];
if (v.mode == TRIM_MODE_NONE)
return;
@@ -1884,7 +1890,7 @@ void setTrimValue(uint8_t phase, uint8_t idx, int trim)
#if !defined(PCBTARANIS)
uint8_t getTrimFlightPhase(uint8_t phase, uint8_t idx)
{
for (uint8_t i=0; i<MAX_PHASES; i++) {
for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
if (phase == 0) return 0;
trim_t trim = getRawTrimValue(phase, idx);
if (trim <= TRIM_EXTENDED_MAX) return phase;
@@ -1899,8 +1905,8 @@ uint8_t getTrimFlightPhase(uint8_t phase, uint8_t idx)
#if defined(ROTARY_ENCODERS)
uint8_t getRotaryEncoderFlightPhase(uint8_t idx)
{
uint8_t phase = s_perout_flight_phase;
for (uint8_t i=0; i<MAX_PHASES; i++) {
uint8_t phase = s_perout_flight_mode;
for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
if (phase == 0) return 0;
#if ROTARY_ENCODERS > 2
int16_t value;
@@ -1990,7 +1996,7 @@ uint8_t s_gvar_last = 0;
uint8_t getGVarFlightPhase(uint8_t phase, uint8_t idx)
{
for (uint8_t i=0; i<MAX_PHASES; i++) {
for (uint8_t i=0; i<MAX_FLIGHT_MODES; i++) {
if (phase == 0) return 0;
int16_t val = GVAR_VALUE(idx, phase); // TODO phase at the end everywhere to be consistent!
if (val <= GVAR_MAX) return phase;
@@ -2691,13 +2697,13 @@ uint8_t checkTrim(uint8_t event)
#if defined(PCBSTD)
phase = 0;
#else
phase = getGVarFlightPhase(s_perout_flight_phase, trimGvar[idx]);
phase = getGVarFlightPhase(s_perout_flight_mode, trimGvar[idx]);
#endif
before = GVAR_VALUE(trimGvar[idx], phase);
thro = false;
}
else {
phase = getTrimFlightPhase(s_perout_flight_phase, idx);
phase = getTrimFlightPhase(s_perout_flight_mode, idx);
#if defined(PCBTARANIS)
before = getTrimValue(phase, idx);
#else
@@ -2706,7 +2712,7 @@ uint8_t checkTrim(uint8_t event)
thro = (idx==THR_STICK && g_model.thrTrim);
}
#else
phase = getTrimFlightPhase(s_perout_flight_phase, idx);
phase = getTrimFlightPhase(s_perout_flight_mode, idx);
#if defined(PCBTARANIS)
before = getTrimValue(phase, idx);
#else
@@ -3081,7 +3087,7 @@ uint16_t isqrt32(uint32_t n)
FORCEINLINE void evalTrims()
{
uint8_t phase = s_perout_flight_phase;
uint8_t phase = s_perout_flight_mode;
for (uint8_t i=0; i<NUM_STICKS; i++) {
// do trim -> throttle trim if applicable
int16_t trim = getTrimValue(phase, i);
@@ -3531,14 +3537,14 @@ void evalFunctions()
#if defined(GVARS)
case FUNC_ADJUST_GVAR:
if (CFN_GVAR_MODE(sd) == 0) {
SET_GVAR(CFN_GVAR_INDEX(sd), CFN_PARAM(sd), s_perout_flight_phase);
SET_GVAR(CFN_GVAR_INDEX(sd), CFN_PARAM(sd), s_perout_flight_mode);
}
else if (CFN_GVAR_MODE(sd) == 2) {
SET_GVAR(CFN_GVAR_INDEX(sd), GVAR_VALUE(CFN_PARAM(sd), s_perout_flight_phase), s_perout_flight_phase);
SET_GVAR(CFN_GVAR_INDEX(sd), GVAR_VALUE(CFN_PARAM(sd), s_perout_flight_mode), s_perout_flight_mode);
}
else if (CFN_GVAR_MODE(sd) == 3) {
if (!(activeFnSwitches & switch_mask)) {
SET_GVAR(CFN_GVAR_INDEX(sd), GVAR_VALUE(CFN_GVAR_INDEX(sd), getGVarFlightPhase(s_perout_flight_phase, CFN_GVAR_INDEX(sd))) + (CFN_PARAM(sd) ? +1 : -1), s_perout_flight_phase);
SET_GVAR(CFN_GVAR_INDEX(sd), GVAR_VALUE(CFN_GVAR_INDEX(sd), getGVarFlightPhase(s_perout_flight_mode, CFN_GVAR_INDEX(sd))) + (CFN_PARAM(sd) ? +1 : -1), s_perout_flight_mode);
}
}
else if (CFN_PARAM(sd) >= MIXSRC_TrimRud && CFN_PARAM(sd) <= MIXSRC_TrimAil) {
@@ -3548,12 +3554,12 @@ void evalFunctions()
else if (CFN_PARAM(sd) >= MIXSRC_REa && CFN_PARAM(sd) < MIXSRC_TrimRud) {
int8_t scroll = rePreviousValues[CFN_PARAM(sd)-MIXSRC_REa] - (g_rotenc[CFN_PARAM(sd)-MIXSRC_REa] / ROTARY_ENCODER_GRANULARITY);
if (scroll) {
SET_GVAR(CFN_GVAR_INDEX(sd), GVAR_VALUE(CFN_GVAR_INDEX(sd), getGVarFlightPhase(s_perout_flight_phase, CFN_GVAR_INDEX(sd))) + scroll, s_perout_flight_phase);
SET_GVAR(CFN_GVAR_INDEX(sd), GVAR_VALUE(CFN_GVAR_INDEX(sd), getGVarFlightPhase(s_perout_flight_mode, CFN_GVAR_INDEX(sd))) + scroll, s_perout_flight_mode);
}
}
#endif
else {
SET_GVAR(CFN_GVAR_INDEX(sd), limit((getvalue_t)-LIMIT_EXT_MAX, getValue(CFN_PARAM(sd)), (getvalue_t)LIMIT_EXT_MAX) / 10, s_perout_flight_phase);
SET_GVAR(CFN_GVAR_INDEX(sd), limit((getvalue_t)-LIMIT_EXT_MAX, getValue(CFN_PARAM(sd)), (getvalue_t)LIMIT_EXT_MAX) / 10, s_perout_flight_mode);
}
break;
#endif
@@ -3636,7 +3642,7 @@ void evalFunctions()
else {
#if defined(GVARS)
if (CFN_FUNC(sd) == FUNC_PLAY_TRACK && param > 250)
param = GVAR_VALUE(param-251, getGVarFlightPhase(s_perout_flight_phase, param-251));
param = GVAR_VALUE(param-251, getGVarFlightPhase(s_perout_flight_mode, param-251));
#endif
PUSH_CUSTOM_PROMPT(active ? param : param+1, i+1);
}
@@ -3717,7 +3723,7 @@ void evalFunctions()
#define HELI_ANAS_ARRAY anas
#endif
uint8_t s_perout_flight_phase;
uint8_t s_perout_flight_mode;
void perOut(uint8_t mode, uint8_t tick10ms)
{
evalInputs(mode);
@@ -3822,7 +3828,7 @@ void perOut(uint8_t mode, uint8_t tick10ms)
//========== PHASE && SWITCH =====
bool mixCondition = (md->phases != 0 || md->swtch);
delayval_t mixEnabled = !(md->phases & (1 << s_perout_flight_phase)) && getSwitch(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;
@@ -3911,7 +3917,7 @@ void perOut(uint8_t mode, uint8_t tick10ms)
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_phase);
int16_t offset = GET_GVAR(MD_OFFSET(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode);
if (offset) v += calc100toRESX_16Bits(offset);
#endif
@@ -3954,7 +3960,7 @@ void perOut(uint8_t mode, uint8_t tick10ms)
}
// 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_phase);
int16_t weight = GET_GVAR(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, s_perout_flight_mode);
weight = calc100to256_16Bits(weight);
//========== SPEED ===============
@@ -4015,7 +4021,7 @@ void perOut(uint8_t mode, uint8_t tick10ms)
//========== 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_phase);
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
@@ -4028,7 +4034,7 @@ void perOut(uint8_t mode, uint8_t tick10ms)
#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_phase));
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)
@@ -4157,7 +4163,7 @@ void doMixerCalculations()
#endif
#define MAX_ACT 0xffff
static uint16_t fp_act[MAX_PHASES] = {0};
static uint16_t fp_act[MAX_FLIGHT_MODES] = {0};
static uint16_t delta = 0;
static ACTIVE_PHASES_TYPE s_fade_flight_phases = 0;
static uint8_t s_last_phase = 255; // TODO reinit everything here when the model changes, no???
@@ -4192,10 +4198,10 @@ void doMixerCalculations()
int32_t weight = 0;
if (s_fade_flight_phases) {
memclear(sum_chans512, sizeof(sum_chans512));
for (uint8_t p=0; p<MAX_PHASES; p++) {
for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
s_last_switch_used = 0;
if (s_fade_flight_phases & ((ACTIVE_PHASES_TYPE)1 << p)) {
s_perout_flight_phase = p;
s_perout_flight_mode = p;
perOut(p==phase ? e_perout_mode_normal : e_perout_mode_inactive_phase, p==phase ? tick10ms : 0);
for (uint8_t i=0; i<NUM_CHNOUT; i++)
sum_chans512[i] += (chans[i] >> 4) * fp_act[p];
@@ -4204,10 +4210,10 @@ void doMixerCalculations()
s_last_switch_used = 0;
}
assert(weight);
s_perout_flight_phase = phase;
s_perout_flight_mode = phase;
}
else {
s_perout_flight_phase = phase;
s_perout_flight_mode = phase;
perOut(e_perout_mode_normal, tick10ms);
}
@@ -4539,7 +4545,7 @@ void doMixerCalculations()
if (s_fade_flight_phases) {
uint16_t tick_delta = delta * tick10ms;
for (uint8_t p=0; p<MAX_PHASES; p++) {
for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
ACTIVE_PHASES_TYPE phaseMask = ((ACTIVE_PHASES_TYPE)1 << p);
if (s_fade_flight_phases & phaseMask) {
if (p == phase) {
@@ -5232,7 +5238,7 @@ void instantTrim()
for (uint8_t i=0; i<NUM_STICKS; i++) {
if (i!=THR_STICK) {
// don't instant trim the throttle stick
uint8_t trim_phase = getTrimFlightPhase(s_perout_flight_phase, i);
uint8_t trim_phase = getTrimFlightPhase(s_perout_flight_mode, i);
#if defined(PCBTARANIS)
int16_t trim = limit<int16_t>(TRIM_EXTENDED_MIN, (calibratedStick[i] + trims[i]) / 2, TRIM_EXTENDED_MAX);
#else
@@ -5296,8 +5302,8 @@ void moveTrimsToOffsets() // copy state of 3 primary to subtrim
// reset all trims, except throttle (if throttle trim)
for (uint8_t i=0; i<NUM_STICKS; i++) {
if (i!=THR_STICK || !g_model.thrTrim) {
int16_t original_trim = getTrimValue(s_perout_flight_phase, i);
for (uint8_t phase=0; phase<MAX_PHASES; phase++) {
int16_t original_trim = getTrimValue(s_perout_flight_mode, i);
for (uint8_t phase=0; phase<MAX_FLIGHT_MODES; phase++) {
#if defined(PCBTARANIS)
trim_t trim = getRawTrimValue(phase, i);
if (trim.mode / 2 == phase)