diff --git a/docs/Settings.md b/docs/Settings.md index da7b82b5c4e..b4b84d029e0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4290,7 +4290,7 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within ### nav_fw_turn_ff_gain -Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven. +Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4330,7 +4330,7 @@ Unmodelled roll-response lag (servo + airframe inertia) added to the computed ro ### nav_fw_wp_turn_max_lead_time -COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint. +COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint. Available only on targets with more than 512 KB flash. | Default | Min | Max | | --- | --- | --- | @@ -4340,7 +4340,7 @@ COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. Th ### nav_fw_wp_turn_mode -How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries). +How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries). On targets with 512 KB flash or less, the arc coordinator is not built in to save flash: the COORD modes remain selectable but silently fly a plain DIRECT turn. | Allowed Values | | | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7227a4fa03f..f7d9403b509 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2793,18 +2793,20 @@ groups: min: 30 max: 80 - name: nav_fw_wp_turn_mode - description: "How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries)." + description: "How the aircraft turns at waypoints during FW WP missions. DIRECT uses the legacy heading-PID turn. The COORD modes fly coordinated arcs of the real turn radius (from speed and nav_fw_bank_angle): COORD_FLYBY cuts the corner and passes the waypoint abeam, COORD_FLYOVER overflies the waypoint before turning onto the next leg, COORD_FLYINTO crosses the waypoint already aligned with the outbound leg (survey line entries). On targets with 512 KB flash or less, the arc coordinator is not built in to save flash: the COORD modes remain selectable but silently fly a plain DIRECT turn." default_value: "COORD_FLYBY" field: fw.wp_turn_mode table: nav_fw_wp_turn_mode - name: nav_fw_turn_ff_gain - description: "Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven." + description: "Turn coordination feed-forward gain [%]. Feeds the geometrically required bank for the current turn radius forward to the roll controller so the PID only trims the residual. 0 disables the feed-forward (pure PID). Default fits most models; tuning candidate to be fixed once field-proven. Available only on targets with more than 512 KB flash." + condition: USE_FW_TURN_PREDICTOR default_value: 100 field: fw.turn_ff_gain min: 0 max: 200 - name: nav_fw_wp_turn_max_lead_time - description: "COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint." + description: "COORD_FLYBY only. Cap on how early a turn may start before the waypoint [ms]. The required lead time grows with speed and turn angle (up to ~10 s for fast models in sharp corners); a too-low cap forces late turn-ins and overshoot. Raise towards 12000 for sluggish models, lower towards 3000 to keep turns close to the waypoint. Available only on targets with more than 512 KB flash." + condition: USE_FW_TURN_PREDICTOR default_value: 6000 field: fw.wp_turn_max_lead_time min: 3000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 38f40895b47..ee2f247c285 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -283,8 +283,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // COORD_FLYBY, WP mission turn mode +#ifdef USE_FW_TURN_PREDICTOR .turn_ff_gain = SETTING_NAV_FW_TURN_FF_GAIN_DEFAULT, // 100, turn FF .wp_turn_max_lead_time = SETTING_NAV_FW_WP_TURN_MAX_LEAD_TIME_DEFAULT, // 3000 ms +#endif .wp_turn_control_ease = SETTING_NAV_FW_WP_TURN_CONTROL_EASE_DEFAULT, // 100 ms } ); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 482f19b6950..14aade54962 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -125,19 +125,24 @@ static timeUs_t fwLastNavRollCmdTimeUs = 0; // nav-to-nav transition apart from static float fwEffectiveBankLimit = 0.0f; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised static float fwActiveLoiterRadius = 0.0f; // effective loiter radius in use (cm), for the loiter circle controller static bool fwArcActive = false; // arc turn coordinator is driving the turn (-> bank headroom, suppress cross-track, roll override) -static bool fwArcEngaged = false; // arc coordinator latch across loops; must be cleared on controller reset or a stale arc resumes after a nav interruption -static int32_t fwArcPrevLegBearing = -1; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) -static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc +static bool fwArcEngaged = false; // arc coordinator latch across loops; also read by the loiter arc to avoid fighting an active WP-turn arc static float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive static float fwArcHandbackCmdCd = 0.0f; // arc command at release; the PID/FF command is faded in from it so the static float fwArcHandbackMs = 0.0f; // seam is continuous regardless of nav_fw_control_smoothness (0 = no fade) static float fwArcHandbackDurMs = 0.0f; static float fwArcEaseMs = 0.0f; // ease time of the arc in progress, sizes the handback fade static bool fwArcWasActive = false; // arc drove the roll last frame, to catch the release edge +#ifdef USE_FW_TURN_PREDICTOR +// WP-turn arc coordinator + turn feed-forward state. Gated out on <=512KB flash targets, which fly +// wp_turn_mode as if it were DIRECT (the arc coordinator's own no-op path for that mode) - see +// updateFwTurnArc()/getFwTurnFeedForward() below. +static int32_t fwArcPrevLegBearing = -1; // last seen WP leg bearing [centideg] for leg-change detection (-1 = unseeded) +static bool fwFlyByCappedLatch = false; // the pending FLY_BY turn hit the lead-time cap -> fly it direct, not as an arc static float fwTurnFFCmdCd = 0.0f; // slew-limited turn feed-forward command [centideg] static bool fwTurnFFArmed = false; // FF assists the turn a leg change begins, not later tracking corrections static int32_t fwTurnFFPrevLegBearing = -1; // last leg bearing seen by the FF arming logic (-1 = unseeded) static float fwArcPickupAlong = 0.0f; // along-track distance to the second-arc pickup [cm], for the log +#endif static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -346,11 +351,13 @@ void resetFixedWingPositionController(void) fwArcEngaged = false; fwArcWasActive = false; fwArcHandbackDurMs = 0.0f; +#ifdef USE_FW_TURN_PREDICTOR fwTurnFFCmdCd = 0.0f; fwTurnFFArmed = false; fwTurnFFPrevLegBearing = -1; fwArcPrevLegBearing = -1; fwFlyByCappedLatch = false; +#endif navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_heading); @@ -564,6 +571,7 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwEffectiveBankLimit)); // energy-guard bank ceiling [deg] } +#ifdef USE_FW_TURN_PREDICTOR // Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. static float getFwCoordinatedTurnRadius(void) { @@ -620,6 +628,7 @@ static float getFwTurnFeedForward(int32_t navHeadingError, timeDelta_t deltaMicr DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(fwTurnFFCmdCd)); // turn/loiter roll feed-forward [centideg] return fwTurnFFCmdCd; } +#endif // USE_FW_TURN_PREDICTOR // Stabilised loiter-radius floor [cm]: the raw requirement swings with wind (v^2) and would make the // tracker thrash - ratchet up instantly, hold the peak one revolution, ease down at <= DECAY @@ -703,6 +712,7 @@ static float applyFwArcHandbackFade(float rollTargetCd, timeDelta_t deltaMicros) return fwArcHandbackCmdCd + (rollTargetCd - fwArcHandbackCmdCd) * s; } +#ifdef USE_FW_TURN_PREDICTOR // Arc turn coordinator: bank ramp -> coordinated arc (radius + tangent feedback) -> predictive // capture roll-out. Sets fwArcActive (drives the roll directly). static void updateFwTurnArc(timeDelta_t deltaMicros) @@ -1146,6 +1156,7 @@ static void updateFwTurnArc(timeDelta_t deltaMicros) } fwArcActive = true; } +#endif // USE_FW_TURN_PREDICTOR // Loiter circle controller: once established on the hold circle, the steady arc law replaces the // carrot PID - live FF bank plus radial/tangent feedback hold the stabilised radius exactly @@ -1237,9 +1248,13 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t } /* FLY_BY corner cut: start the turn R*tan(angle/2) before the WP so the arc joins the next leg - * at any speed. FLY_BY legs only - the landing approach forces FLY_BY in every mode. */ - int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); + * at any speed. FLY_BY legs only - the landing approach forces FLY_BY in every mode. + * Gated out with the arc coordinator on <=512KB flash targets: without it to fly the corner cut, + * there is nothing to anticipate the turn for, so wpTurnSmoothingActive simply never engages and + * the leg behaves as a plain DIRECT turn at the waypoint. */ posControl.flags.wpTurnSmoothingActive = false; +#ifdef USE_FW_TURN_PREDICTOR + int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); const bool flyByLeg = navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_COORD_FLY_BY || posControl.navState == NAV_STATE_FW_LANDING_APPROACH; if (flyByLeg && waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { @@ -1258,6 +1273,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t fwFlyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead } } +#endif // USE_FW_TURN_PREDICTOR // We are closing in on a waypoint, calculate circular loiter if required if (needToCalculateCircularLoiter) { @@ -1272,7 +1288,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t } // Arc turn coordinator: manages the turn state and commands the roll bank directly +#ifdef USE_FW_TURN_PREDICTOR updateFwTurnArc(deltaMicros); +#endif updateFwLoiterArc(deltaMicros); // Calculate virtual waypoint @@ -1459,11 +1477,13 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta fwRollSmoothReseed = true; fwArcHandbackDurMs = 0.0f; // an arc that re-engages cancels a fade still in progress fwArcWasActive = true; +#ifdef USE_FW_TURN_PREDICTOR // The FF is not called while the arc drives: clear and disarm it, and consume leg changes the // arc handles itself - the carrot error left at hand-back is a capture correction, not a turn fwTurnFFCmdCd = 0.0f; fwTurnFFArmed = false; fwTurnFFPrevLegBearing = posControl.activeWaypoint.bearing; +#endif } else { if (fwArcWasActive) { // falling edge: arm the crossfade from the arc's last command fwArcHandbackCmdCd = fwLastNavRollCmdCd; @@ -1471,8 +1491,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta fwArcHandbackDurMs = fwArcEaseMs; fwArcWasActive = false; } +#ifdef USE_FW_TURN_PREDICTOR // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. rollAdjustment += getFwTurnFeedForward(navHeadingError, deltaMicros); +#endif rollAdjustment = applyFwArcHandbackFade(rollAdjustment, deltaMicros); rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); fwRollSmoothReseed = false; diff --git a/src/main/target/common.h b/src/main/target/common.h index 4b23d187099..6beb532aa0e 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -440,6 +440,7 @@ // Keep larger optional features off 512 KB targets to preserve flash space. #if (MCU_FLASH_SIZE > 512) +#define USE_FW_TURN_PREDICTOR #define USE_AUTO_TRANSITION #define USE_TELEMETRY_MAVLINK #define USE_SERIALRX_MAVLINK