diff --git a/docs/Settings.md b/docs/Settings.md index 2df6b7ebf4f..eaf52e6e8b6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3507,6 +3507,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci --- +### nav_cruise_lock_on_level + +Fixed wing only: when ON the COURSE HOLD/CRUISE course is locked only once the aircraft has rolled out level (below 10 deg bank) after a heading adjustment or a banked mode entry, following the actual course until then. Prevents overshooting the locked course during the level-off. OFF locks the course as soon as the sticks are centered (legacy behaviour). + +| Default | Min | Max | +| --- | --- | --- | +| ON | OFF | ON | + +--- + ### nav_cruise_yaw_rate Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps] @@ -3640,7 +3650,7 @@ Max pitch angle when climbing in GPS assisted modes, is also restrained by globa ### nav_fw_control_smoothness -How smoothly the autopilot controls the airplane to correct the navigation error +How smoothly the autopilot corrects the navigation error. Pitch uses a low-pass filter. Roll uses an S-curve easing window of n x 100 ms (max 900 ms) applied only when the commanded bank changes abruptly, so steady course tracking is never lagged. 0 = no roll smoothing. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8849ccdaf28..5f6eae0cbbc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2869,6 +2869,11 @@ groups: field: general.cruise_yaw_rate min: 0 max: 120 + - name: nav_cruise_lock_on_level + description: "Fixed wing only: when ON the COURSE HOLD/CRUISE course is locked only once the aircraft has rolled out level (below 10 deg bank) after a heading adjustment or a banked mode entry, following the actual course until then. Prevents overshooting the locked course during the level-off. OFF locks the course as soon as the sticks are centered (legacy behaviour)." + default_value: ON + field: general.cruise_lock_on_level + type: bool - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" default_value: 35 @@ -3056,7 +3061,7 @@ groups: min: 1 max: 10 - name: nav_fw_control_smoothness - description: "How smoothly the autopilot controls the airplane to correct the navigation error" + description: "How smoothly the autopilot corrects the navigation error. Pitch uses a low-pass filter. Roll uses an S-curve easing window of n x 100 ms (max 900 ms) applied only when the commanded bank changes abruptly, so steady course tracking is never lagged. 0 = no roll smoothing." default_value: 0 field: fw.control_smoothness min: 0 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e112ce8a1b9..51cf54a8e73 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -83,6 +83,9 @@ #define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec) #define FW_LAND_LOITER_ALT_TOLERANCE 150 +#define FW_COURSE_LOCK_MAX_BANK_DECIDEG 100 // lock the cruise course only once rolled out below this bank angle (10 deg) +#define FW_COURSE_LOCK_FORCE_TIMEOUT_MS 2500 // failsafe: force the lock this long after stick centre even if still banked + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,7 +122,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -177,6 +180,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT, .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps .rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing + .cruise_lock_on_level = SETTING_NAV_CRUISE_LOCK_ON_LEVEL_DEFAULT, }, // MC-specific @@ -1371,6 +1375,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( return NAV_FSM_EVENT_NONE; } +// FW course hold: the course lock is pending while a turn is still being rolled out (mode entry from +// a banked turn or heading adjustment just released) - the course follows the actual COG until then. +// Gated by nav_cruise_lock_on_level; when OFF the course locks as soon as the sticks are centered. +static bool fwCruiseCourseLockPending = false; +static bool fwCruiseHeadingAdjustActive = false; // yaw adjustment running: its sustained course lead winds up the nav PID +static timeMs_t fwCruiseStickCentreTimeMs = 0; // last time the roll/yaw sticks went to centre + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1389,6 +1400,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( if (STATE(AIRPLANE)) { posControl.cruise.course = posControl.actualState.cog; // Store the course to follow + // Entering from a banked turn (e.g. mode switch out of RTH mid-turn): course hold means + // "fly straight from here", so follow the COG until the roll-out is complete, then lock. + fwCruiseCourseLockPending = navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG; + fwCruiseHeadingAdjustActive = false; + fwCruiseStickCentreTimeMs = millis(); } else { // Multicopter posControl.cruise.course = posControl.actualState.yaw; posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); @@ -1419,7 +1435,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS } const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband; - static bool adjustmentWasActive = false; // User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR // We record the desired course and change the desired target in the meanwhile @@ -1440,13 +1455,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS } posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; - adjustmentWasActive = true; + fwCruiseCourseLockPending = true; + fwCruiseHeadingAdjustActive = true; + fwCruiseStickCentreTimeMs = currentTimeMs; DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); - } else if (STATE(AIRPLANE) && adjustmentWasActive) { - posControl.cruise.course = posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW)); - resetPositionController(); - adjustmentWasActive = false; + } else if (STATE(AIRPLANE) && fwCruiseCourseLockPending) { + // Locking while still banked overshoots the course; the timeout covers a roll-out that never completes + const bool rolledOut = !navConfig()->general.cruise_lock_on_level + || ABS(attitude.values.roll) <= FW_COURSE_LOCK_MAX_BANK_DECIDEG + || currentTimeMs - fwCruiseStickCentreTimeMs >= FW_COURSE_LOCK_FORCE_TIMEOUT_MS; + // Without the level gate the turn continues through the roll-out: keep the legacy yaw-rate lead + posControl.cruise.course = navConfig()->general.cruise_lock_on_level ? posControl.actualState.cog + : posControl.actualState.cog - DEGREES_TO_CENTIDEGREES(gyroRateDps(YAW)); + + if (fwCruiseHeadingAdjustActive || rolledOut) { + resetPositionController(); // the adjustment wound up the integrator; a zero error cannot unwind it + fwCruiseHeadingAdjustActive = false; + } + fwCruiseCourseLockPending = !rolledOut; } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; } @@ -1461,10 +1488,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n UNUSED(previousState); DEBUG_SET(DEBUG_CRUISE, 0, 3); - // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE + // User is rolling, changing manually direction. Wait until it is done AND the roll-out is + // complete before locking the course and re-engaging: a course locked while still banked is + // overshot during the level-off (the turn continues), forcing a reverse correction. if (posControl.flags.isAdjustingPosition) { posControl.cruise.course = posControl.actualState.cog; //store current course posControl.cruise.lastCourseAdjustmentTime = millis(); + fwCruiseStickCentreTimeMs = millis(); + return NAV_FSM_EVENT_NONE; // reprocess the state + } + if (STATE(AIRPLANE) && navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG + && millis() - fwCruiseStickCentreTimeMs < FW_COURSE_LOCK_FORCE_TIMEOUT_MS) { + posControl.cruise.course = posControl.actualState.cog; + posControl.cruise.lastCourseAdjustmentTime = millis(); return NAV_FSM_EVENT_NONE; // reprocess the state } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b6cf4692b66..55c34dc6d64 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -441,6 +441,7 @@ typedef struct navConfig_s { uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately) uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled uint16_t rth_fs_landing_delay; // Delay upon reaching home before starting landing if in FS (0 = immediate) + bool cruise_lock_on_level; // FW: lock the course hold course only once rolled out level (OFF = lock on stick release) } general; struct { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0451f8a94ab..c58bd00fc7b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -56,9 +56,16 @@ #include "sensors/battery.h" -// Base frequencies for smoothing pitch and roll +// Base frequency for smoothing the pitch command and the pitch-to-throttle correction #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f -#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f + +// Roll-command S-curve smoothing: control_smoothness (0..9) -> easing window = n*100 ms (0 = off, +// max 900 ms). Triggered only on an abrupt commanded-bank step (>20% of the configured roll +// rate between nav loops), then eased over the window and passed 1:1 afterwards. Unlike the previous +// PT1 low-pass this never lags steady tracking, so the controller command stays deterministic. +#define NAV_FW_SMOOTH_TCONST_PER_STEP_MS 100.0f +#define NAV_FW_SMOOTH_TCONST_MAX_MS 900.0f +#define NAV_FW_SMOOTH_STEP_FRACTION 0.2f // If we are going slower than the minimum ground speed (navConfig()->general.min_ground_speed) - boost throttle to fight against the wind #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f @@ -72,6 +79,10 @@ static bool isYawAdjustmentValid = false; static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; static float navCrossTrackError; +static bool fwRollSmoothReseed = false; // re-sync the roll S-curve smoother on the next frame (after a controller reset) +static float fwRollSmoothSeedCd = 0.0f; // baseline the smoother re-seeds to (set by the controller reset) +static float fwLastNavRollCmdCd = 0.0f; // last applied nav roll command [centideg] + timestamp, to tell a +static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from a pilot handover at reset time static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -261,7 +272,6 @@ bool adjustFixedWingHeadingFromRCInput(void) * XY-position controller *-----------------------------------------------------------*/ static fpVector3_t virtualDesiredPosition; -static pt1Filter_t fwPosControllerCorrectionFilterState; static pt1Filter_t fwCrossTrackErrorRateFilterState; /* @@ -280,10 +290,14 @@ void resetFixedWingPositionController(void) isRollAdjustmentValid = false; isYawAdjustmentValid = false; + // Re-seed the roll S-curve smoother. If nav commanded roll until just now (nav-mode to nav-mode + // transition, e.g. RTH -> CRUISE) seed from the last applied command so the level-off/turn change + // is eased; after a pilot-flown phase seed neutral so a roll-out in progress is not re-commanded. + fwRollSmoothSeedCd = ((micros() - fwLastNavRollCmdTimeUs) < MAX_POSITION_UPDATE_INTERVAL_US) ? fwLastNavRollCmdCd : 0.0f; + fwRollSmoothReseed = true; + pt1FilterSetCutoff(&fwCrossTrackErrorRateFilterState, 3.0f); pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f); - pt1FilterSetCutoff(&fwPosControllerCorrectionFilterState, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ)); - pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f); } static int8_t loiterDirection(void) { @@ -319,6 +333,62 @@ static int8_t loiterDirection(void) { return dir; } +// Triggered S-curve roll-in [centideg]: on an abrupt commanded-bank step (new heading), ease toward the +// target with a smoothstep over a control_smoothness-derived time constant, then pass 1:1. The timer is +// not reset by further steps mid-ramp, so we never get stuck damping steady tracking. +static float applyFwRollInSmoothing(float rollTargetCd, timeDelta_t deltaMicros, bool reseed) +{ + static float prevTarget = 0.0f; + static float prevRate = 0.0f; + static float rampStart = 0.0f; + static float prevOut = 0.0f; + static float elapsedMs = 0.0f; + static bool active = false; + + if (reseed) { // controller reset: re-seed to the baseline chosen at reset time + active = false; // (last nav command on a nav-to-nav transition, else neutral), so + elapsedMs = 0.0f; // the cross-mode command step is detected and eased while stale + prevTarget = fwRollSmoothSeedCd; // state can never fire a spurious ramp + prevRate = 0.0f; + prevOut = fwRollSmoothSeedCd; + } + + const float tConstMs = MIN((float)navConfig()->fw.control_smoothness * NAV_FW_SMOOTH_TCONST_PER_STEP_MS, NAV_FW_SMOOTH_TCONST_MAX_MS); + const float dtS = US2S(deltaMicros); + if (tConstMs <= 0.0f || dtS <= 0.0f) { // smoothing off: pass through + active = false; + prevTarget = rollTargetCd; + prevRate = 0.0f; + prevOut = rollTargetCd; + return rollTargetCd; + } + + const float cmdRate = (rollTargetCd - prevTarget) / dtS; // commanded bank rate [centideg/s] + const float stepThreshold = NAV_FW_SMOOTH_STEP_FRACTION * (currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f) * 100.0f; // 20% of roll rate [centideg/s] + if (!active && fabsf(cmdRate - prevRate) > stepThreshold) { // abrupt setpoint-rate change -> start the S-curve + active = true; + elapsedMs = 0.0f; + rampStart = prevOut; + } + + float out = rollTargetCd; // default: 1:1 pass-through + if (active) { + elapsedMs += dtS * 1000.0f; // timer does NOT reset on further steps + if (elapsedMs >= tConstMs) { + active = false; // window elapsed -> back to 1:1 + } else { + const float p = elapsedMs / tConstMs; + const float s = p * p * (3.0f - 2.0f * p); // smoothstep (S-curve) + out = rampStart + s * (rollTargetCd - rampStart); + } + } + + prevTarget = rollTargetCd; + prevRate = cmdRate; + prevOut = out; + return out; +} + static void calculateVirtualPositionTarget_FW(float trackingPeriod) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { @@ -560,11 +630,15 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), pidFlags); - // Apply low-pass filter to prevent rapid correction - rollAdjustment = pt1FilterApply3(&fwPosControllerCorrectionFilterState, rollAdjustment, US2S(deltaMicros)); + // Triggered S-curve smoothing on the roll command (control_smoothness); re-seeded after a + // controller reset so stale smoother state cannot fire a spurious ramp. + rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); + fwRollSmoothReseed = false; // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees) posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment); + fwLastNavRollCmdCd = rollAdjustment; + fwLastNavRollCmdTimeUs = currentTimeUs; /* * Yaw adjustment