Skip to content

FW Auto level trim bug fix #9393

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
breadoven merged 3 commits into from
Oct 26, 2023
Merged

FW Auto level trim bug fix #9393

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
013de45
fix autoleveltrim
breadoven Oct 23, 2023
b6b5a98
fix delayed save + exclude manual mode
breadoven Oct 23, 2023
fa6468d
Update pid.c
breadoven Oct 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion src/main/fc/fc_core.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -873,7 +873,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
if (!ARMING_FLAG(ARMED)) {
armTime = 0;

processDelayedSave();
// Delay saving for 0.5s to allow other functions to process save actions on disarm
if (currentTimeUs - lastDisarmTimeUs > USECS_PER_SEC / 2) {
processDelayedSave();
}
}

#if defined(SITL_BUILD)
Expand Down
53 changes: 28 additions & 25 deletions src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM bool levelingEnabled = false;

#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f
#define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f
#define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER
#define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE

Expand Down Expand Up @@ -1243,7 +1243,7 @@ void pidInit(void)
navPidInit(
&fixedWingLevelTrimController,
0.0f,
(float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
(float)pidProfile()->fixedWingLevelTrimGain / 100.0f,
0.0f,
0.0f,
2.0f,
Expand All @@ -1255,47 +1255,52 @@ void pidInit(void)
const pidBank_t * pidBank(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
}

pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
}

bool isFixedWingLevelTrimActive(void)
{
return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
(FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
!FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
!navigationIsControllingAltitude();
}

void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
{
if (!STATE(AIRPLANE)) {
return;
}

static timeUs_t previousUpdateTimeUs;
static bool previousArmingState;
const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
static bool previousArmingState = false;

/*
* On every ARM reset the controller
*/
if (ARMING_FLAG(ARMED) && !previousArmingState) {
navPidReset(&fixedWingLevelTrimController);
if (ARMING_FLAG(ARMED)) {
if (!previousArmingState) { // On every ARM reset the controller
navPidReset(&fixedWingLevelTrimController);
}
} else if (previousArmingState) { // On disarm update the default value
pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
}
previousArmingState = ARMING_FLAG(ARMED);

/*
* On disarm update the default value
*/
if (!ARMING_FLAG(ARMED) && previousArmingState) {
pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
// return if not active or disarmed
if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
return;
}

static timeUs_t previousUpdateTimeUs;
const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
previousUpdateTimeUs = currentTimeUs;

/*
* Prepare flags for the PID controller
*/
pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;

//Iterm should freeze when sticks are deflected
if (
!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
areSticksDeflected() ||
(!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) ||
FLIGHT_MODE(SOARING_MODE) ||
navigationIsControllingAltitude()
) {
// Iterm should freeze when conditions for setting level trim aren't met
if (!isFixedWingLevelTrimActive()) {
flags |= PID_FREEZE_INTEGRATOR;
}

Expand All @@ -1313,8 +1318,6 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)

DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);

previousArmingState = !!ARMING_FLAG(ARMED);
}

float getFixedWingLevelTrim(void)
Expand Down
7 changes: 4 additions & 3 deletions src/main/flight/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,8 +106,8 @@ typedef struct pidProfile_s {
pidBank_t bank_mc;

uint8_t dterm_lpf_type; // Dterm LPF type: PT1, BIQUAD
uint16_t dterm_lpf_hz;
uint16_t dterm_lpf_hz;

uint8_t yaw_lpf_hz;

uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
Expand All @@ -129,7 +129,7 @@ typedef struct pidProfile_s {
float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point
uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees

float navVelXyDTermLpfHz;
uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity
Expand Down Expand Up @@ -221,5 +221,6 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa

pidType_e pidIndexGetType(pidIndex_e pidIndex);

bool isFixedWingLevelTrimActive(void);
void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
float getFixedWingLevelTrim(void);
18 changes: 9 additions & 9 deletions src/main/io/osd.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1911,7 +1911,7 @@ static bool osdDrawSingleElement(uint8_t item)
osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0);
break;

case OSD_ODOMETER:
case OSD_ODOMETER:
{
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_ODOMETER);
uint32_t odometerDist = (uint32_t)(getTotalTravelDistance() / 100);
Expand Down Expand Up @@ -3955,7 +3955,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)

/**
* @brief Draws the INAV and/or pilot logos on the display
*
*
* @param singular If true, only one logo will be drawn. If false, both logos will be drawn, separated by osdConfig()->inav_to_pilot_logo_spacing characters
* @param row The row number to start drawing the logos. If not singular, both logos are drawn on the same rows.
* @return uint8_t The row number after the logo(s).
Expand Down Expand Up @@ -4005,7 +4005,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
} else {
logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
}

for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) {
displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++);
Expand Down Expand Up @@ -4129,7 +4129,7 @@ static void osdCompleteAsyncInitialization(void)

if (fontHasMetadata && metadata.charCount > 256) {
hasExtendedFont = true;

y = drawLogos(false, y);
y++;
} else if (!fontHasMetadata) {
Expand Down Expand Up @@ -4568,12 +4568,12 @@ static void osdShowHDArmScreen(void)

displayWrite(osdDisplayPort, col, armScreenRow, buf);
displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2);

int digits = osdConfig()->plus_code_digits;
olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
}

#if defined (USE_SAFE_HOME)
if (posControl.safehomeState.distance) { // safehome found during arming
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
Expand Down Expand Up @@ -4653,7 +4653,7 @@ static void osdShowSDArmScreen(void)
uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2;
displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf);
displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2);

int digits = osdConfig()->plus_code_digits;
olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
Expand Down Expand Up @@ -5156,8 +5156,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO);
}
}
if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || (navigationRequiresAngleMode() && !navigationIsControllingAltitude()))) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
if (isFixedWingLevelTrimActive()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
Expand Down