void ModelData::setTrimValue(int phaseIdx, int trimIdx, int value)
{
for (uint8_t i=0; i<C9X_MAX_FLIGHT_MODES; i++) {
FlightModeData & phase = flightModeData[phaseIdx];
int mode = phase.trimMode[trimIdx];
int p = phase.trimRef[trimIdx];
int & trim = phase.trim[trimIdx];
if (mode < 0)
return;
if (p == phaseIdx || phaseIdx == 0) {
trim = value;
break;;
}
else if (mode == 0) {
phaseIdx = p;
}
else {
trim = value - getTrimValue(p, trimIdx);
if (trim < -500)
trim = -500;
if (trim > 500)
trim = 500;
break;
}
}
}
TEST(Trims, InstantTrim)
{
MODEL_RESET();
modelDefault(0);
anaInValues[AIL_STICK] = 50;
instantTrim();
EXPECT_EQ(25, getTrimValue(0, AIL_STICK));
}
TEST(Trims, CopyTrimsToOffset)
{
MODEL_RESET();
modelDefault(0);
setTrimValue(0, ELE_STICK, -100); // -100 on elevator
#if defined(CPUARM)
evalFunctions(g_model.customFn, modelFunctionsContext); // it disables all safety channels
copyTrimsToOffset(1);
EXPECT_EQ(getTrimValue(0, ELE_STICK), -100); // unchanged
EXPECT_EQ(g_model.limitData[1].offset, -195);
#else
evalFunctions(); // it disables all safety channels
copyTrimsToOffset(1);
EXPECT_EQ(getTrimValue(0, ELE_STICK), -100); // unchanged
EXPECT_EQ(g_model.limitData[1].offset, -200);
#endif
}
void OpenTxSimulator::getTrims(Trims & trims)
{
::uint8_t phase = getFlightMode();
trims.extended = hasExtendedTrims();
for (::uint8_t idx=0; idx<4; idx++) {
trims.values[idx] = getTrimValue(getTrimFlightPhase(phase, idx), idx);
}
for (int i=0; i<2; i++) {
::uint8_t idx = NAMESPACE::modn12x3[4*getStickMode() + i];
::int16_t tmp = trims.values[i];
trims.values[i] = trims.values[idx];
trims.values[idx] = tmp;
}
}
TEST(Trims, InstantTrimNegativeCurve)
{
MODEL_RESET();
modelDefault(0);
ExpoData *expo = expoAddress(AIL_STICK);
expo->curve.type = CURVE_REF_CUSTOM;
expo->curve.value = 1;
g_model.points[0] = -100;
g_model.points[1] = -75;
g_model.points[2] = -50;
g_model.points[3] = -25;
g_model.points[4] = 0;
anaInValues[AIL_STICK] = 512;
instantTrim();
EXPECT_EQ(128, getTrimValue(0, AIL_STICK));
}
void displayTrims(uint8_t phase)
{
for (uint8_t i=0; i<4; i++) {
static coord_t x[4] = {TRIM_LH_X, TRIM_LV_X, TRIM_RV_X, TRIM_RH_X};
static uint8_t vert[4] = {0,1,1,0};
coord_t xm, ym;
uint8_t stickIndex = CONVERT_MODE(i);
xm = x[stickIndex];
uint8_t att = ROUND;
int16_t val = getTrimValue(phase, i);
#if !defined(CPUM64) || !defined(FRSKY)
int16_t dir = val;
bool exttrim = false;
if (val < TRIM_MIN || val > TRIM_MAX) {
exttrim = true;
}
#endif
if (val < -(TRIM_LEN+1)*4) {
val = -(TRIM_LEN+1);
}
else if (val > (TRIM_LEN+1)*4) {
val = TRIM_LEN+1;
}
else {
val /= 4;
}
if (vert[i]) {
ym = 31;
lcd_vline(xm, ym-TRIM_LEN, TRIM_LEN*2);
if (i!=2 || !g_model.thrTrim) {
lcd_vline(xm-1, ym-1, 3);
lcd_vline(xm+1, ym-1, 3);
}
ym -= val;
#if !defined(CPUM64) || !defined(FRSKY)
drawFilledRect(xm-3, ym-3, 7, 7, SOLID, att|ERASE);
if (dir >= 0) {
lcd_hline(xm-1, ym-1, 3);
}
if (dir <= 0) {
lcd_hline(xm-1, ym+1, 3);
}
if (exttrim) {
lcd_hline(xm-1, ym, 3);
}
#endif
#if defined(CPUARM)
if (g_model.displayTrims != DISPLAY_TRIMS_NEVER && dir != 0) {
if (g_model.displayTrims == DISPLAY_TRIMS_ALWAYS || (trimsDisplayTimer > 0 && (trimsDisplayMask & (1<<i)))) {
lcd_outdezAtt(dir>0 ? 22 : 54, xm-2, -abs(dir/5), TINSIZE|VERTICAL);
}
}
#endif
}
else {
ym = 60;
lcd_hline(xm-TRIM_LEN, ym, TRIM_LEN*2);
lcd_hline(xm-1, ym-1, 3);
lcd_hline(xm-1, ym+1, 3);
xm += val;
#if !defined(CPUM64) || !defined(FRSKY)
drawFilledRect(xm-3, ym-3, 7, 7, SOLID, att|ERASE);
if (dir >= 0) {
lcd_vline(xm+1, ym-1, 3);
}
if (dir <= 0) {
lcd_vline(xm-1, ym-1, 3);
}
if (exttrim) {
lcd_vline(xm, ym-1, 3);
}
#endif
#if defined(CPUARM)
if (g_model.displayTrims != DISPLAY_TRIMS_NEVER && dir != 0) {
if (g_model.displayTrims == DISPLAY_TRIMS_ALWAYS || (trimsDisplayTimer > 0 && (trimsDisplayMask & (1<<i)))) {
lcd_outdezAtt((stickIndex==0 ? TRIM_LH_X : TRIM_RH_X)+(dir>0 ? -11 : 20), ym-2, -abs(dir/5), TINSIZE);
}
}
#endif
}
lcd_square(xm-3, ym-3, 7, att);
}
}
