Merge pull request #8126 from opentx/3djc/fix-trims2subtrims
Crosstrims fixes
This commit is contained in:
+22
-8
@@ -539,14 +539,25 @@ int getStickTrimValue(int stick, int stickValue)
|
|||||||
return trim;
|
return trim;
|
||||||
}
|
}
|
||||||
|
|
||||||
int getSourceTrimValue(int source, int stickValue=0)
|
int getSourceTrimOrigin(int source)
|
||||||
{
|
{
|
||||||
if (source >= MIXSRC_Rud && source <= MIXSRC_Ail)
|
if (source >= MIXSRC_Rud && source <= MIXSRC_Ail)
|
||||||
return getStickTrimValue(source - MIXSRC_Rud, stickValue);
|
return source - MIXSRC_Rud;
|
||||||
else if (source >= MIXSRC_FIRST_INPUT && source <= MIXSRC_LAST_INPUT)
|
else if (source >= MIXSRC_FIRST_INPUT && source <= MIXSRC_LAST_INPUT)
|
||||||
return getStickTrimValue(virtualInputsTrims[source - MIXSRC_FIRST_INPUT], stickValue);
|
return virtualInputsTrims[source - MIXSRC_FIRST_INPUT];
|
||||||
else
|
else
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
int getSourceTrimValue(int source, int stickValue=0)
|
||||||
|
{
|
||||||
|
auto origin = getSourceTrimOrigin(source);
|
||||||
|
if (origin >= 0) {
|
||||||
|
return getStickTrimValue(origin, stickValue);
|
||||||
|
}
|
||||||
|
else {
|
||||||
return 0;
|
return 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t mixerCurrentFlightMode;
|
uint8_t mixerCurrentFlightMode;
|
||||||
@@ -741,13 +752,16 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (applyOffsetAndCurve) {
|
if (applyOffsetAndCurve) {
|
||||||
|
bool applyTrims = !(mode & e_perout_mode_notrims);
|
||||||
//========== TRIMS ================
|
if (!applyTrims && g_model.thrTrim) {
|
||||||
if (!(mode & e_perout_mode_notrims)) {
|
auto origin = getSourceTrimOrigin(md->srcRaw);
|
||||||
if (md->carryTrim == 0) {
|
if (origin == g_model.getThrottleStickTrimSource() - MIXSRC_FIRST_TRIM) {
|
||||||
v += getSourceTrimValue(md->srcRaw, v);
|
applyTrims = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if (applyTrims && md->carryTrim == 0) {
|
||||||
|
v += getSourceTrimValue(md->srcRaw, v);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int32_t weight = GET_GVAR_PREC1(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);
|
int32_t weight = GET_GVAR_PREC1(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);
|
||||||
|
|||||||
+11
-8
@@ -1119,7 +1119,7 @@ void checkTrims()
|
|||||||
else {
|
else {
|
||||||
phase = getTrimFlightMode(mixerCurrentFlightMode, idx);
|
phase = getTrimFlightMode(mixerCurrentFlightMode, idx);
|
||||||
before = getTrimValue(phase, idx);
|
before = getTrimValue(phase, idx);
|
||||||
thro = (idx==THR_STICK && g_model.thrTrim);
|
thro = (idx == (g_model.getThrottleStickTrimSource() - MIXSRC_FIRST_TRIM) && g_model.thrTrim);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
phase = getTrimFlightMode(mixerCurrentFlightMode, idx);
|
phase = getTrimFlightMode(mixerCurrentFlightMode, idx);
|
||||||
@@ -1824,24 +1824,27 @@ void moveTrimsToOffsets() // copy state of 3 primary to subtrim
|
|||||||
pauseMixerCalculations();
|
pauseMixerCalculations();
|
||||||
|
|
||||||
evalFlightModeMixes(e_perout_mode_noinput, 0); // do output loop - zero input sticks and trims
|
evalFlightModeMixes(e_perout_mode_noinput, 0); // do output loop - zero input sticks and trims
|
||||||
for (uint8_t i=0; i<MAX_OUTPUT_CHANNELS; i++) {
|
|
||||||
|
for (uint8_t i = 0; i < MAX_OUTPUT_CHANNELS; i++) {
|
||||||
zeros[i] = applyLimits(i, chans[i]);
|
zeros[i] = applyLimits(i, chans[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
evalFlightModeMixes(e_perout_mode_noinput-e_perout_mode_notrims, 0); // do output loop - only trims
|
evalFlightModeMixes(e_perout_mode_noinput-e_perout_mode_notrims, 0); // do output loop - only trims
|
||||||
|
|
||||||
for (uint8_t i=0; i<MAX_OUTPUT_CHANNELS; i++) {
|
for (uint8_t i = 0; i < MAX_OUTPUT_CHANNELS; i++) {
|
||||||
int16_t output = applyLimits(i, chans[i]) - zeros[i];
|
int16_t diff = applyLimits(i, chans[i]) - zeros[i];
|
||||||
int16_t v = g_model.limitData[i].offset;
|
int16_t v = g_model.limitData[i].offset;
|
||||||
if (g_model.limitData[i].revert)
|
if (g_model.limitData[i].revert)
|
||||||
output = -output;
|
diff = -diff;
|
||||||
v += (output * 125) / 128;
|
v += (diff * 125) / 128;
|
||||||
g_model.limitData[i].offset = limit((int16_t)-1000, (int16_t)v, (int16_t)1000); // make sure the offset doesn't go haywire
|
|
||||||
|
g_model.limitData[i].offset = limit((int16_t) -1000, (int16_t) v, (int16_t) 1000); // make sure the offset doesn't go haywire
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset all trims, except throttle (if throttle trim)
|
// reset all trims, except throttle (if throttle trim)
|
||||||
for (uint8_t i=0; i<NUM_TRIMS; i++) {
|
for (uint8_t i=0; i<NUM_TRIMS; i++) {
|
||||||
if (i != THR_STICK || !g_model.thrTrim) {
|
auto thrStick = g_model.getThrottleStickTrimSource() - MIXSRC_FIRST_TRIM;
|
||||||
|
if (i != thrStick || !g_model.thrTrim) {
|
||||||
int16_t original_trim = getTrimValue(mixerCurrentFlightMode, i);
|
int16_t original_trim = getTrimValue(mixerCurrentFlightMode, i);
|
||||||
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
|
for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
|
||||||
trim_t trim = getRawTrimValue(fm, i);
|
trim_t trim = getRawTrimValue(fm, i);
|
||||||
|
|||||||
@@ -339,6 +339,106 @@ TEST_F(TrimsTest, CopySticksToOffset)
|
|||||||
EXPECT_EQ(g_model.limitData[1].offset, -97);
|
EXPECT_EQ(g_model.limitData[1].offset, -97);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(TrimsTest, MoveTrimsToOffsets)
|
||||||
|
{
|
||||||
|
// No trim idle only
|
||||||
|
g_model.thrTrim = 0;
|
||||||
|
anaInValues[THR_STICK] = 0;
|
||||||
|
setTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM, 100);
|
||||||
|
setTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM, -100);
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], 200); // THR output value is reflecting 100 trim
|
||||||
|
moveTrimsToOffsets();
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM), 0); // back to neutral
|
||||||
|
EXPECT_EQ(g_model.limitData[2].offset, 195); // value transferred
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM), 0); // back to neutral
|
||||||
|
EXPECT_EQ(g_model.limitData[1].offset, -195); // value transferred
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], 200); // THR output value is still reflecting 100 trim
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(TrimsTest, MoveTrimsToOffsetsWithTrimIdle)
|
||||||
|
{
|
||||||
|
// Trim idle only
|
||||||
|
g_model.thrTrim = 1;
|
||||||
|
anaInValues[THR_STICK] = -1024; // Min stick
|
||||||
|
g_model.limitData[2].offset = 0;
|
||||||
|
g_model.limitData[1].offset = 0;
|
||||||
|
setTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM, 100);
|
||||||
|
setTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM, -100);
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], -574); // THR output value is reflecting 100 trim idle
|
||||||
|
moveTrimsToOffsets();
|
||||||
|
|
||||||
|
// Trim affecting Throttle should not be affected
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM), 100); // unchanged
|
||||||
|
EXPECT_EQ(g_model.limitData[2].offset, 0); // unchanged
|
||||||
|
|
||||||
|
// Other trims should
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM), 0); // back to neutral
|
||||||
|
EXPECT_EQ(g_model.limitData[1].offset, -195); // value transferred
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], -574); // THR output value is still reflecting 100 trim idle
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(TrimsTest, MoveTrimsToOffsetsWithCrossTrims)
|
||||||
|
{
|
||||||
|
// No trim idle only
|
||||||
|
// Cross trims
|
||||||
|
g_model.thrTrim = 0;
|
||||||
|
g_model.limitData[2].offset = 0;
|
||||||
|
g_model.limitData[1].offset = 0;
|
||||||
|
g_model.thrTrimSw = MIXSRC_TrimEle - MIXSRC_FIRST_TRIM;
|
||||||
|
ExpoData *expo = expoAddress(THR_STICK);
|
||||||
|
expo->carryTrim = TRIM_ELE;
|
||||||
|
expo = expoAddress(ELE_STICK);
|
||||||
|
expo->carryTrim = TRIM_THR;
|
||||||
|
|
||||||
|
anaInValues[THR_STICK] = 0;
|
||||||
|
setTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM, 100);
|
||||||
|
setTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM, -100);
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], 200); // THR output value is reflecting 100 Ele trim
|
||||||
|
moveTrimsToOffsets();
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], 200); // THR output value remains unchanged
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM), 0); // back to neutral
|
||||||
|
EXPECT_EQ(g_model.limitData[2].offset, 195); // value transferred
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM), 0); // back to neutral
|
||||||
|
EXPECT_EQ(g_model.limitData[1].offset, -195); // value transferred
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(TrimsTest, MoveTrimsToOffsetsWithCrosstrimsAndTrimIdle)
|
||||||
|
{
|
||||||
|
// Trim idle only
|
||||||
|
// Cross trims
|
||||||
|
g_model.limitData[2].offset = 0;
|
||||||
|
g_model.limitData[1].offset = 0;
|
||||||
|
g_model.thrTrim = 1;
|
||||||
|
g_model.thrTrimSw = MIXSRC_TrimEle - MIXSRC_FIRST_TRIM;
|
||||||
|
ExpoData *expo = expoAddress(THR_STICK);
|
||||||
|
expo->carryTrim = TRIM_ELE;
|
||||||
|
expo = expoAddress(ELE_STICK);
|
||||||
|
expo->carryTrim = TRIM_THR;
|
||||||
|
|
||||||
|
anaInValues[THR_STICK] = -1024; // Min stick
|
||||||
|
setTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM, 100);
|
||||||
|
setTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM, -100);
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], -574); // THR output value is reflecting 100 ele trim idle
|
||||||
|
moveTrimsToOffsets();
|
||||||
|
|
||||||
|
// Trim affecting Throttle (now Ele because of crosstrims) should not be affected
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM), 100); // unchanged
|
||||||
|
EXPECT_EQ(g_model.limitData[2].offset, 0); // THR chan offset unchanged
|
||||||
|
|
||||||
|
// Other trims should
|
||||||
|
EXPECT_EQ(getTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM), 0); // back to neutral
|
||||||
|
EXPECT_EQ(g_model.limitData[1].offset, -195); // Ele chan offset transfered
|
||||||
|
evalMixes(1);
|
||||||
|
EXPECT_EQ(channelOutputs[2], -574); // THR output value is still reflecting 100 trim idle
|
||||||
|
}
|
||||||
|
|
||||||
TEST_F(TrimsTest, InstantTrim)
|
TEST_F(TrimsTest, InstantTrim)
|
||||||
{
|
{
|
||||||
anaInValues[AIL_STICK] = 50;
|
anaInValues[AIL_STICK] = 50;
|
||||||
|
|||||||
Reference in New Issue
Block a user