diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 7b227705757..1e28aa0b837 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -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) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3049fbb8224..d3990821cff 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 @@ -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, @@ -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; } @@ -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) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index de0e3126b7c..b4441b86830 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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 @@ -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 @@ -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); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 24a6bc6cb24..2d176be0af2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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); @@ -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). @@ -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++); @@ -4129,7 +4129,7 @@ static void osdCompleteAsyncInitialization(void) if (fontHasMetadata && metadata.charCount > 256) { hasExtendedFont = true; - + y = drawLogos(false, y); y++; } else if (!fontHasMetadata) { @@ -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) { @@ -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); @@ -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);