diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md index 7f4ce455f78..9b5d28c608f 100644 --- a/docs/Fixed Wing Landing.md +++ b/docs/Fixed Wing Landing.md @@ -20,6 +20,8 @@ This enables up to 4 different approach directions, based on the landing site an 7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held 8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`. +All turns between the approach legs are flown as coordinated FLY_BY corner cuts: the turn start is anticipated from the aircraft's actual turn radius (speed and `nav_fw_bank_angle`) so the plane rolls out already aligned on the next leg. This applies during the landing approach regardless of the configured `nav_fw_wp_turn_mode`. + To activate the automatic landing, the parameter `nav_rth_allow_landing` must be set to `ALWAYS` or `FAILSAFE`. > [!WARNING] @@ -90,8 +92,8 @@ If WP-Tracking is not used, the Plane will head straight to the landiung locatio * `nav_fw_pitch2thr`: The navigation throttle modifier has to be tuned well to allow stable navigation during climbs and descents to prevent a stall. Make sure your plane maintains Ground or Airspeed, when climbing in any navigation mode. The Craft should not get slower and not speed ub significantly during a navigation climb, if P2T is tuned properly. See `Fixed Wing Pitch To Throttle Tuning.md` for a full tuning procedure. -* `nav_wp_radius`: This parameter might be too high if you have set up your craft with INAV 6 or INAV 7. With a too high value, the turning points for the Crosswind-Leg and Final Approach are hit too early and make it difficult for the plane to align to the runway or cut short the approach. -Make sure this parameter is not set greater than 1000 (cm). The better your craft and navigation system is tuned, the lower this value can be. We recommend to start with 1000 for flying wings and 800 for a Plane with Tail. +* `nav_wp_radius`: The turning points for the Crosswind-Leg and Final Approach are calculated automatically from the aircraft's coordinated turn radius; `nav_wp_radius` does not shape the approach corners. +Make sure this parameter is not set greater than 1000 (cm) so approach waypoints are not detected as reached too early. * Test your Navigation-Tuning: A better Navigation-Tune will reward you with smoother and more reliable landings. To test your nav systems limit, we recommend to create a waypoint missions with many 90° turn angles with shorter and shorter tracks. With this Method, you can find out how well your plane can follow a navigation path and how long it takes to align to a waypoint track. A well tuned plane should be able to pull of a WP Mission that looks like this, where the distance between WP6 and WP7 si recommended to be the minimum approach length: diff --git a/docs/Settings.md b/docs/Settings.md index 2df6b7ebf4f..fef6e2dd2b5 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -795,6 +795,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | LULU | | | SBUS2 | | | OSD_REFRESH | | +| FW_TURN | | --- @@ -3507,6 +3508,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] @@ -3620,7 +3631,7 @@ P gain of auto speed PID controller. ### nav_fw_bank_angle -Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll +Maximum sustained roll angle when turning in GPS assisted modes: the target bank that turn and loiter radii are planned for. Corrections may exceed it temporarily; the absolute ceiling remains max_angle_inclination_rll | Default | Min | Max | | --- | --- | --- | @@ -3640,7 +3651,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 | | --- | --- | --- | @@ -4138,6 +4149,16 @@ 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. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + ### nav_fw_wp_tracking_accuracy Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy. @@ -4158,15 +4179,36 @@ Sets the maximum allowed alignment convergence angle to the waypoint course line --- -### nav_fw_wp_turn_smoothing +### nav_fw_wp_turn_control_ease -Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner). +Unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramps; increase for large or slow-responding airframes. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 500 | + +--- + +### 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. + +| Default | Min | Max | +| --- | --- | --- | +| 6000 | 3000 | 12000 | + +--- + +### 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). | Allowed Values | | | --- | --- | -| OFF | Default | -| ON | | -| ON-CUT | | +| DIRECT | | +| COORD_FLYBY | Default | +| COORD_FLYOVER | | +| COORD_FLYINTO | | --- diff --git a/docs/development/fw-turn-debugging.md b/docs/development/fw-turn-debugging.md new file mode 100644 index 00000000000..49ec76eacd9 --- /dev/null +++ b/docs/development/fw-turn-debugging.md @@ -0,0 +1,59 @@ +# Fixed-wing coordinated turn debugging (`DEBUG_FW_TURN`) + +`set debug_mode = FW_TURN` exposes the fixed-wing coordinated WP turn system: the arc turn +coordinator, the S sequencer (COORD_FLYOVER with path tracking / COORD_FLYINTO), the loiter +radius stabiliser, the turn/loiter feed-forward and the energy bank guard. Values are available +via the CLI `debug` command, the `OSD_DEBUG` element, MSP `DEBUGMSG`/`DEBUG` telemetry and +blackbox logging. + +## Channels + +| debug[] | Value | Unit | Written by | +|---|---|---|---| +| 0 | Active turn/loiter radius | cm | Loiter stabiliser every cycle; overridden by the COORD_FLYBY planning radius while a corner approach is active, and by the engaged arc's radius while the coordinator flies the turn (latest writer wins) | +| 1 | Coordinator state (see below) | – | Arc coordinator | +| 2 | Exit course of the active arc | centideg | Arc coordinator, only while engaged | +| 3 | Remaining heading to the exit course | centideg | Arc coordinator, only while engaged | +| 4 | Arc bank command | centideg | Arc coordinator, only while engaged; clamped to the effective bank ceiling | +| 5 | WP-turn roll feed-forward | centideg | Feed-forward (0 when disabled, loitering, or inside the heading deadband) | +| 6 | Energy-guard bank ceiling | deg | Energy bank guard (sits at `max_angle_inclination_rll` unless the guard is reducing it) | +| 7 | Roll ease time | ms | Arc coordinator, only while engaged (sizes the entry/exit ramps and the turn-start lead) | + +Channels 2, 3, 4 and 7 hold their last value after the arc disengages; check channel 1 to know +whether the coordinator is active. + +## Channel 1: coordinator state + +While the arc coordinator is engaged the value is `(arc phase + 1) * 10 + S stage`; while idle +it is the S stage alone. Two loiter states use their own values: `4` = holding, approach on the +stock carrot guidance; `40` = loiter circle controller engaged (steady arc law holds the +radius). + +Arc phase (tens digit): + +| Digit | Phase | Meaning | +|---|---|---| +| 1 | RAMP_IN | Smoothstep bank ramp onto the pre-placed circle | +| 2 | STEADY | Coordinated arc: live feed-forward bank + radius/tangent feedback | +| 3 | CAPTURE | Predictive roll-out onto the exit course | + +S stage (ones digit; 0 outside the S modes): + +| Digit | Stage | Meaning | +|---|---|---| +| 0 | – | Plain single-arc turn (COORD_FLYBY, COORD_FLYOVER tangent exit) | +| 1 | AWAY | First arc of the S; the second arc is staged | +| 2 | MAIN | Second arc of the S (COORD_FLYINTO: the aligned WP crossing; COORD_FLYOVER + tracking: the corner-cut onto the leg) | +| 3 | DONE | S completed for this leg, waiting for the next leg switch | + +Examples: `20` = flying a plain coordinated turn; `21` = steady on the first S arc; `32` = +rolling out of the S's second arc; `3` (idle) = S finished, coordinator handed back. + +## Reading a turn + +A healthy COORD_FLYBY corner shows: channel 1 stepping `10 → 20 → 30 → 0`, channel 4 ramping to +the nominal bank, holding, then collapsing as channel 3 converges to 0 without overshoot. The +hand-back happens aligned (|ch3| small) and nearly level (|ch4| < 10% of nominal). Channel 4 +pegged at channel 6 × 100 indicates the command is saturated at the flyable ceiling — expected +briefly downwind, a problem if sustained. Channel 6 dropping below `max_angle_inclination_rll` +means the energy guard is trading bank for climb capability. diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b33868af8b2..dbde5291924 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,6 +80,7 @@ typedef enum { DEBUG_LULU, DEBUG_SBUS2, DEBUG_OSD_REFRESH, + DEBUG_FW_TURN, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index f5b12301028..41e3ca59d7e 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -202,7 +202,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), #endif OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART), - OSD_SETTING_ENTRY("WP TURN SMOOTHING", SETTING_NAV_FW_WP_TURN_SMOOTHING), + OSD_SETTING_ENTRY("WP TURN MODE", SETTING_NAV_FW_WP_TURN_MODE), OSD_SETTING_ENTRY("WP TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 4d0e5006506..289c9983b53 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -228,7 +228,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "GPS", "LULU", "SBUS2", - "OSD_REFRESH" + "OSD_REFRESH", + "FW_TURN" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8849ccdaf28..b5469a08bad 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "FW_TURN"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -180,9 +180,9 @@ tables: - name: dynamic_gyro_notch_mode values: ["2D", "3D"] enum: dynamicGyroNotchMode_e - - name: nav_fw_wp_turn_smoothing - values: ["OFF", "ON", "ON-CUT"] - enum: wpFwTurnSmoothing_e + - name: nav_fw_wp_turn_mode + values: ["DIRECT", "COORD_FLYBY", "COORD_FLYOVER", "COORD_FLYINTO"] + enum: navFwWpTurnMode_e - name: gps_auto_baud_max values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600'] enum: gpsBaudRate_e @@ -2670,11 +2670,29 @@ groups: field: fw.wp_tracking_max_angle min: 30 max: 80 - - name: nav_fw_wp_turn_smoothing - description: "Smooths turns during WP missions by switching to a loiter turn at waypoints. When set to ON the craft will reach the waypoint during the turn. When set to ON-CUT the craft will turn inside the waypoint without actually reaching it (cuts the corner)." - default_value: "OFF" - field: fw.wp_turn_smoothing - table: nav_fw_wp_turn_smoothing + - 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)." + 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." + 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." + default_value: 6000 + field: fw.wp_turn_max_lead_time + min: 3000 + max: 12000 + - name: nav_fw_wp_turn_control_ease + description: "Unmodelled roll-response lag (servo + airframe inertia) added to the computed roll-in/out ease time [ms] for coordinated WP turns. Sizes and anticipates the entry/exit ramps; increase for large or slow-responding airframes." + default_value: 100 + field: fw.wp_turn_control_ease + min: 0 + max: 500 - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" default_value: 500 @@ -2869,6 +2887,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 @@ -2967,7 +2990,7 @@ groups: field: mc.slowDownForTurning type: bool - name: nav_fw_bank_angle - description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll" + description: "Maximum sustained roll angle when turning in GPS assisted modes: the target bank that turn and loiter radii are planned for. Corrections may exceed it temporarily; the absolute ceiling remains max_angle_inclination_rll" default_value: 35 field: fw.max_bank_angle min: 5 @@ -3056,7 +3079,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..9b7a55339a0 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -83,6 +83,8 @@ #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) + /*----------------------------------------------------------- * Compatibility for home position *-----------------------------------------------------------*/ @@ -119,7 +121,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 +179,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 @@ -247,7 +250,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled .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_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions + .wp_turn_mode = SETTING_NAV_FW_WP_TURN_MODE_DEFAULT, // COORD_FLYBY, WP mission turn mode + .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 + .wp_turn_control_ease = SETTING_NAV_FW_WP_TURN_CONTROL_EASE_DEFAULT, // 100 ms } ); @@ -1371,6 +1377,11 @@ 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 navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1389,6 +1400,9 @@ 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; } else { // Multicopter posControl.cruise.course = posControl.actualState.yaw; posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); @@ -1419,7 +1433,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 +1453,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS } posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; - adjustmentWasActive = true; + fwCruiseCourseLockPending = true; 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) { + if (navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG) { + // Still banked (adjustment turn or banked mode entry): keep following the actual course + // until the roll-out is complete, else the locked course is overshot and reverse-corrected. + posControl.cruise.course = posControl.actualState.cog; + } else { + // Rolled out: lock to the current COG. The former yaw-rate lead term mixed a rate into an + // angle; with the bank gate the residual turn rate at lock time is negligible anyway. + posControl.cruise.course = posControl.actualState.cog; + resetPositionController(); + fwCruiseCourseLockPending = false; + } } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; } @@ -1461,8 +1482,10 @@ 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 - if (posControl.flags.isAdjustingPosition) { + // 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 || (STATE(AIRPLANE) && navConfig()->general.cruise_lock_on_level && ABS(attitude.values.roll) > FW_COURSE_LOCK_MAX_BANK_DECIDEG)) { posControl.cruise.course = posControl.actualState.cog; //store current course posControl.cruise.lastCourseAdjustmentTime = millis(); return NAV_FSM_EVENT_NONE; // reprocess the state @@ -3082,16 +3105,13 @@ bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBe posControl.wpDistance = calculateDistanceToDestination(waypointPos); // Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing. - // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active + // Angular limit = 100 degs. uint16_t relativeBearingTargetAngle = 10000; if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) { - // If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) { - posControl.flags.wpTurnSmoothingActive = false; - return true; - } - relativeBearingTargetAngle = 6000; + // FLY_BY turn: the waypoint is reached when the anticipated corner-cut turn is initiated + posControl.flags.wpTurnSmoothingActive = false; + return true; } @@ -4283,7 +4303,8 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint) mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); calculateAndSetActiveWaypointToLocalPosition(&localPos); - if (navConfig()->fw.wp_turn_smoothing) { + // Turn anticipation (nextTurnAngle) is needed for FLY_BY and FLY_INTO; FLY_OVER flies to the WP then turns. + if (navConfig()->fw.wp_turn_mode != NAV_FW_WP_TURN_COORD_FLY_OVER) { fpVector3_t posNextWp; if (getLocalPosNextWaypoint(&posNextWp)) { int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp); @@ -5527,7 +5548,9 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos { calculateAndSetActiveWaypointToLocalPosition(pos); - if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) { + // Landing approach always uses FLY_BY turns (clean cut onto the next approach leg), + // so the turn angle is set whenever a following approach waypoint exists. + if (nextWpPos != NULL) { int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, nextWpPos); posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing); } else { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b6cf4692b66..a06cc993480 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -335,10 +335,11 @@ typedef enum { } rthTrackbackMode_e; typedef enum { - WP_TURN_SMOOTHING_OFF, - WP_TURN_SMOOTHING_ON, - WP_TURN_SMOOTHING_CUT, -} wpFwTurnSmoothing_e; + NAV_FW_WP_TURN_DIRECT = 0, // legacy heading-PID turn (fallback) + NAV_FW_WP_TURN_COORD_FLY_BY = 1, // corner cut: turn anticipated so the arc joins the next leg, WP passed abeam + NAV_FW_WP_TURN_COORD_FLY_OVER = 2, // fly over the WP, then roll out on the tangent line to the next WP + NAV_FW_WP_TURN_COORD_FLY_INTO = 3, // ease away before the WP, then cross it already aligned on the outbound course +} navFwWpTurnMode_e; typedef enum { MC_ALT_HOLD_STICK, @@ -441,6 +442,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 { @@ -506,7 +508,10 @@ typedef struct navConfig_s { uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg) uint8_t wp_tracking_accuracy; // fixed wing tracking accuracy response factor uint8_t wp_tracking_max_angle; // fixed wing tracking accuracy max alignment angle [degs] - uint8_t wp_turn_smoothing; // WP mission turn smoothing options + uint8_t wp_turn_mode; // WP mission turn mode (navFwWpTurnMode_e: DIRECT / COORD_FLYBY / COORD_FLYOVER / COORD_FLYINTO) + uint8_t turn_ff_gain; // turn coordination feed-forward gain [%] (0 = off; dev tuning, to be hardcoded) + uint16_t wp_turn_max_lead_time; // FLY_BY: cap on how early the turn may start before the WP [ms] (dev tuning) + uint16_t wp_turn_control_ease; // unmodelled roll-response lag added to the computed turn ease time [ms] } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0451f8a94ab..732acdbbf99 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -56,13 +56,53 @@ #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 +// FW waypoint turn predictor: clamps for the coordinated-turn radius used to time the FLY_BY turn +#define NAV_FW_TURN_MIN_SPEED 500.0f // [cm/s] speed floor for the radius calc (low-speed noise guard) +#define NAV_FW_TURN_RADIUS_MIN 1000.0f // [cm] 10 m lower clamp +#define NAV_FW_TURN_RADIUS_MAX 30000.0f // [cm] 300 m upper clamp (matches loiter_radius max) +#define NAV_FW_LOITER_RADIUS_DECAY 100.0f // [cm/s] max rate the held loiter radius eases back down (1 m/s) +#define NAV_FW_TURN_LEAD_TAN_MAX 3.7f // tan(half turn angle) cap (~150 deg) to bound the lead distance +#define NAV_FW_ARC_MIN_TURN_ANGLE_CD 3000 // [centideg] only fly the coordinated arc for turns sharper than 30 deg +#define NAV_FW_ARC_RADIAL_GAIN 0.5f // [centideg bank / cm radial error] pull back onto the arc radius (TBD from flight) +#define NAV_FW_ARC_HEADING_GAIN 0.3f // [centideg bank / centideg tangent heading error] align to the arc (TBD from flight) +#define NAV_FW_ARC_EXIT_GAIN 2.0f // [centideg bank / centideg heading error] proportional roll-out capture: bank -> 0 as cog reaches the out-leg (no overshoot) +#define NAV_FW_ARC_EXIT_HANDOFF_CD 150 // [centideg] hand back to the PID within this heading error of the out-leg (keep low: residual bank = gain*this) +#define NAV_FW_ARC_SHARP_TURN_CD 15000 // [centideg] beyond this the tangent points explode toward the 180 deg reversal -> capture-only turn + +// FW energy/altitude bank guard thresholds (conservative; observable via DEBUG_FW_TURN) +#define NAV_FW_GUARD_PHI_FLOOR_DEG 15.0f // minimum effective bank limit +#define NAV_FW_GUARD_MIN_BANK_DEG 10.0f // "banked" threshold +#define NAV_FW_GUARD_VZ_CLIMB_MIN 50.0f // only guard when commanding a climb above this +#define NAV_FW_GUARD_VZ_DEFICIT_ENTER 150.0f // filtered Vz deficit to start guarding (Schmitt high) +#define NAV_FW_GUARD_VZ_DEFICIT_EXIT 50.0f // filtered Vz deficit to stop guarding (Schmitt low) +#define NAV_FW_GUARD_VZ_FILTER_HZ 0.5f // deficit/rise low-pass cutoff (rejects wind/thermal/noise) +#define NAV_FW_GUARD_PITCH_FRAC 0.85f // "near max climb pitch" fraction +#define NAV_FW_GUARD_THROTTLE_MARGIN 50 // "near max throttle" margin +#define NAV_FW_GUARD_REDUCE_RATE_DPS 20.0f // bank-limit reduce rate +#define NAV_FW_GUARD_RECOVER_RATE_DPS 5.0f // bank-limit recover rate +#define NAV_FW_GUARD_RECOVER_HOLDOFF_MS 1000 // healthy time before recovery starts + +// Turn-coordination feed-forward: heading-error window over which the WP-turn FF tapers in (centideg) +#define NAV_FW_FF_HEADING_DEADBAND_CD 500.0f // below this heading error: no WP-turn FF +#define NAV_FW_FF_HEADING_FULL_CD 3000.0f // heading error for full WP-turn FF +#define NAV_FW_LOITER_CAPTURE_BAND 0.15f // fraction of R: engage the loiter circle controller inside this radial band +#define NAV_FW_LOITER_CAPTURE_ALIGN_CD 4500 // [centideg] and only roughly tangential to the circle +#define NAV_FW_LOITER_RELEASE_BAND 0.5f // fraction of R: grossly displaced -> hand back to the carrot guidance + // If this is enabled navigation won't be applied if velocity is below 3 m/s //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -72,6 +112,17 @@ 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 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 float fwArcBankCmd = 0.0f; // direct-radius arc bank command [centideg] (Approach B), applied to roll while fwArcActive static int8_t loiterDirYaw = 1; static bool needToCalculateCircularLoiter; static bool autoSpeedIsActive = false; @@ -261,7 +312,6 @@ bool adjustFixedWingHeadingFromRCInput(void) * XY-position controller *-----------------------------------------------------------*/ static fpVector3_t virtualDesiredPosition; -static pt1Filter_t fwPosControllerCorrectionFilterState; static pt1Filter_t fwCrossTrackErrorRateFilterState; /* @@ -272,6 +322,10 @@ void resetFixedWingPositionController(void) virtualDesiredPosition.x = 0; virtualDesiredPosition.y = 0; virtualDesiredPosition.z = 0; + fwArcActive = false; + fwArcEngaged = false; + fwArcPrevLegBearing = -1; + fwFlyByCappedLatch = false; navPidReset(&posControl.pids.fw_nav); navPidReset(&posControl.pids.fw_heading); @@ -280,10 +334,17 @@ 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; + + // Reset the energy-guard bank limit; 0 = use full ceiling until the guard re-syncs on its next run + fwEffectiveBankLimit = 0.0f; + 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,7 +380,708 @@ static int8_t loiterDirection(void) { return dir; } -static void calculateVirtualPositionTarget_FW(float trackingPeriod) +// 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; +} + +// Hard roll ceiling [deg]: the global angle-mode limit (max_angle_inclination_rll) +static float getFwBankCeilingDeg(void) +{ + return (float)pidProfile()->max_angle_inclination[FD_ROLL] / 10.0f; +} + +// Output bank ceiling [deg]: hard ceiling reduced by the energy guard; nav_fw_bank_angle is only the planning target +static float getFwEffectiveBankLimit(void) +{ + const float ceiling = getFwBankCeilingDeg(); + return (fwEffectiveBankLimit > 0.0f) ? MIN(fwEffectiveBankLimit, ceiling) : ceiling; +} + +// Planning bank [deg] for sizing turn/loiter radii: the target, capped by the guard ceiling +static float getFwPlanningBankDeg(void) +{ + return MIN((float)navConfig()->fw.max_bank_angle, getFwEffectiveBankLimit()); +} + +// Roll-command bank limit [deg]: an active arc may use the reserve up to the ceiling to hold the radius against wind; everywhere else the target +static float getFwControlBankLimit(void) +{ + return fwArcActive ? getFwEffectiveBankLimit() : getFwPlanningBankDeg(); +} + +// Reduce the bank ceiling when a commanded climb stalls near the pitch/throttle limit while banked, so the turn widens and the climb recovers +static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottleValue) +{ + static timeUs_t lastUpdateUs = 0; + static timeUs_t lastTriggerUs = 0; + static bool deficitLatched = false; + static bool bankedPrev = false; + static float targetVzBaseline = 0.0f; + static pt1Filter_t deficitFilter; + static pt1Filter_t riseFilter; + + const float maxBank = getFwBankCeilingDeg(); + const float targetBank = MIN((float)navConfig()->fw.max_bank_angle, maxBank); // planning target = guard snap level + if (fwEffectiveBankLimit <= 0.0f) { + fwEffectiveBankLimit = maxBank; + } + + const timeDeltaLarge_t dtUs = currentTimeUs - lastUpdateUs; + lastUpdateUs = currentTimeUs; + // First call / gap (controller was inactive): resync, skip integration this step. + if (dtUs <= 0 || dtUs > MAX_POSITION_UPDATE_INTERVAL_US) { + targetVzBaseline = posControl.desiredState.vel.z; + pt1FilterSetCutoff(&deficitFilter, NAV_FW_GUARD_VZ_FILTER_HZ); + pt1FilterSetCutoff(&riseFilter, NAV_FW_GUARD_VZ_FILTER_HZ); + pt1FilterReset(&deficitFilter, 0.0f); + pt1FilterReset(&riseFilter, 0.0f); + deficitLatched = false; + lastTriggerUs = currentTimeUs; + return; + } + const float dtSec = US2S(dtUs); + + const float bankDeg = fabsf((float)posControl.rcAdjustment[ROLL]) / 10.0f; // rcAdjustment is decidegrees + const bool banked = bankDeg > NAV_FW_GUARD_MIN_BANK_DEG; + + // Latch target Vz at bank entry as the pre-bank reference. + if (banked && !bankedPrev) { + targetVzBaseline = posControl.desiredState.vel.z; + } + bankedPrev = banked; + + const float targetVz = posControl.desiredState.vel.z; // cm/s + const float actualVz = navGetCurrentActualPositionAndVelocity()->vel.z; // cm/s + + // Signal A: unmet climb demand. Signal B: bank-induced rise of the demand since bank entry. + const float deficit = pt1FilterApply3(&deficitFilter, targetVz - actualVz, dtSec); + const float rise = pt1FilterApply3(&riseFilter, targetVz - targetVzBaseline, dtSec); + + // Schmitt trigger + deadband on the filtered deficit (rejects fluctuations). + if (deficit > NAV_FW_GUARD_VZ_DEFICIT_ENTER) { + deficitLatched = true; + } else if (deficit < NAV_FW_GUARD_VZ_DEFICIT_EXIT) { + deficitLatched = false; + } + + const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES((float)navConfig()->fw.max_climb_angle); + const bool nearPitchLimit = (float)posControl.rcAdjustment[PITCH] >= NAV_FW_GUARD_PITCH_FRAC * maxClimbDeciDeg; + // AUTO throttle demand only: manual full throttle must not permanently arm the guard + const bool nearThrottleLimit = autoThrottleValue >= (currentBatteryProfile->nav.fw.max_throttle - NAV_FW_GUARD_THROTTLE_MARGIN); + const bool climbCommanded = targetVz > NAV_FW_GUARD_VZ_CLIMB_MIN; + + const bool trigger = banked && climbCommanded && deficitLatched && (nearPitchLimit || nearThrottleLimit); + + // Bank-induced rise (signal B) -> react faster. + const float reduceRate = (rise > NAV_FW_GUARD_VZ_DEFICIT_ENTER) ? (2.0f * NAV_FW_GUARD_REDUCE_RATE_DPS) : NAV_FW_GUARD_REDUCE_RATE_DPS; + + if (trigger) { + fwEffectiveBankLimit = MIN(fwEffectiveBankLimit, targetBank); // drop headroom at once: snap to the planning target + fwEffectiveBankLimit -= reduceRate * dtSec; // then keep easing down toward the floor + lastTriggerUs = currentTimeUs; + } else if ((currentTimeUs - lastTriggerUs) > ((timeUs_t)NAV_FW_GUARD_RECOVER_HOLDOFF_MS * 1000)) { + fwEffectiveBankLimit += NAV_FW_GUARD_RECOVER_RATE_DPS * dtSec; + } + fwEffectiveBankLimit = constrainf(fwEffectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank); + + DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwEffectiveBankLimit)); // energy-guard bank ceiling [deg] +} + +// Coordinated-turn radius R = V^2/(g*tan(phi)) [cm], clamped. Times the FLY_BY turn for any speed. +static float getFwCoordinatedTurnRadius(void) +{ + const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); // cm/s + const float bankRad = DEGREES_TO_RADIANS(getFwPlanningBankDeg()); // plan for the target bank + const float radius = (speed * speed) / (GRAVITY_CMSS * tan_approx(bankRad)); // cm + return constrainf(radius, NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); +} + +// Coordinated-turn feed-forward bank [centideg] for the active WP turn (loiter has its own circle controller). +static float getFwTurnFeedForward(int32_t navHeadingError) +{ + const uint8_t ffGain = navConfig()->fw.turn_ff_gain; + if (ffGain == 0 || posControl.actualState.velXY <= NAV_FW_TURN_MIN_SPEED + || (navConfig()->fw.wp_turn_mode == NAV_FW_WP_TURN_DIRECT && posControl.navState != NAV_STATE_FW_LANDING_APPROACH)) { + return 0.0f; // DIRECT = pure legacy PID (landing approach forces FLY_BY) + } + + float ffRadius = 0.0f; + float ffSign = 0.0f; + if (!needToCalculateCircularLoiter && isWaypointNavTrackingActive() && ABS(navHeadingError) > NAV_FW_FF_HEADING_DEADBAND_CD) { + ffRadius = getFwCoordinatedTurnRadius(); // WP turn: dynamic radius, tapered by heading error + ffSign = (navHeadingError > 0 ? 1.0f : -1.0f) * constrainf((float)ABS(navHeadingError) / NAV_FW_FF_HEADING_FULL_CD, 0.0f, 1.0f); + } + + float rollFF = 0.0f; + if (ffRadius > 0.0f) { + const float v = posControl.actualState.velXY; + const float phiFFcd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * ffRadius))); + rollFF = ffSign * phiFFcd * (ffGain / 100.0f); + } + DEBUG_SET(DEBUG_FW_TURN, 5, lrintf(rollFF)); // turn/loiter roll feed-forward [centideg] + return rollFF; +} + +// 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 +static uint32_t getFwStableLoiterRadius(uint32_t configuredRadius, float bearingFromCenterRad, bool loiterActive, timeDelta_t deltaMicros) +{ + static bool active = false; + static float commandedHold = 0.0f; + static float decayTarget = 0.0f; + static float revPeak = 0.0f; + static float netAngle = 0.0f; + static float prevBearing = 0.0f; + + const float speed = MAX(posControl.actualState.velXY, NAV_FW_TURN_MIN_SPEED); + const float required = constrainf((speed * speed) / (GRAVITY_CMSS * tan_approx(DEGREES_TO_RADIANS(getFwPlanningBankDeg()))), + NAV_FW_TURN_RADIUS_MIN, NAV_FW_TURN_RADIUS_MAX); + + if (!loiterActive) { + active = false; + commandedHold = required; // transit / WP turn: track instantaneously + } else { + if (!active) { // loiter entry: seed (no decay until the first revolution) + active = true; + commandedHold = required; + decayTarget = NAV_FW_TURN_RADIUS_MAX; + revPeak = required; + netAngle = 0.0f; + prevBearing = bearingFromCenterRad; + } + revPeak = MAX(revPeak, required); + commandedHold = MAX(commandedHold, required); // ratchet up immediately (safety) + + float dAng = bearingFromCenterRad - prevBearing; + if (dAng > M_PIf) dAng -= 2.0f * M_PIf; + if (dAng < -M_PIf) dAng += 2.0f * M_PIf; + netAngle += dAng; // signed net rotation about the centre + prevBearing = bearingFromCenterRad; + + if (fabsf(netAngle) >= 2.0f * M_PIf) { // a full revolution -> this revolution's peak is the decay target + decayTarget = revPeak; + revPeak = required; + netAngle = 0.0f; + } + + if (commandedHold > decayTarget) { // ease down gradually, never below the current need + commandedHold -= NAV_FW_LOITER_RADIUS_DECAY * US2S(deltaMicros); + commandedHold = MAX(commandedHold, MAX(decayTarget, required)); + } + } + + const uint32_t out = (uint32_t)MAX((float)configuredRadius, commandedHold); + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(out)); // active turn/loiter radius [cm] (overridden by FLY_BY/arc writers) + return out; +} + +// Roll-in/out ease time [ms] from roll rate, control_smoothness and the servo/inertia margin - +// single source of the turn's roll dynamics (the S-curve smoother is bypassed during the arc) +static float fwTurnEaseTimeMs(float phiNomDeg) +{ + const float rollRateDps = currentControlProfile->stabilized.rates[FD_ROLL] * 10.0f; + const float rollMs = (rollRateDps > 1.0f) ? (1.5f * phiNomDeg / rollRateDps * 1000.0f) : 0.0f; + const float csMs = MIN((float)navConfig()->fw.control_smoothness * NAV_FW_SMOOTH_TCONST_PER_STEP_MS, NAV_FW_SMOOTH_TCONST_MAX_MS); + return rollMs + csMs + (float)navConfig()->fw.wp_turn_control_ease; +} + +// 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) +{ + enum { ARC_RAMP_IN = 0, ARC_STEADY, ARC_CAPTURE }; + static uint8_t phase; + static float arcCx, arcCy, arcR; + static int8_t arcDir; + static int32_t arcOutBearing; + static bool arcToLegLine; // fallback capture: converge onto the leg line itself, not just its course + static float phiNomCd; // coordinated nominal bank for this turn [centideg] + static float tEaseMs; // roll-in ease time + static float rampMs; // elapsed time in the ramp-in phase + static float rampStartCd; // bank the ramp blends from (0 on entry; -phi at the FLY_INTO inflection) + + // S sequencer (FLY_INTO / FLY_OVER-tracking): first arc, roll-reversal gap, second arc + enum { FW_INTO_IDLE = 0, FW_INTO_AWAY, FW_INTO_MAIN, FW_INTO_DONE }; + static uint8_t intoStage; + static float intoEx, intoEy; // second-arc pickup point (internal-tangent touch) + static float intoO2x, intoO2y; // second-arc centre + static float intoR; + static int32_t intoBOut; + static int8_t intoDir; + + fwArcActive = false; + + // Landing approach always flies coordinated FLY_BY turns, whatever mode is configured + navFwWpTurnMode_e turnMode = navConfig()->fw.wp_turn_mode; + if (posControl.navState == NAV_STATE_FW_LANDING_APPROACH) { + turnMode = NAV_FW_WP_TURN_COORD_FLY_BY; + } + + const bool wpTracking = isWaypointNavTrackingActive() && !needToCalculateCircularLoiter; + if (turnMode == NAV_FW_WP_TURN_DIRECT || !wpTracking) { + fwArcEngaged = false; + fwArcPrevLegBearing = -1; + intoStage = FW_INTO_IDLE; + return; + } + + const int32_t legBearing = posControl.activeWaypoint.bearing; + const int32_t cog = posControl.actualState.cog; + const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; + const float v = posControl.actualState.velXY; + + if (!fwArcEngaged) { + arcToLegLine = false; + const bool legChanged = (fwArcPrevLegBearing >= 0) && (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500); + fwArcPrevLegBearing = legBearing; + if (legChanged) { + intoStage = FW_INTO_IDLE; // a new leg invalidates any staged S geometry + const bool capped = fwFlyByCappedLatch; // lead-time-capped FLY_BY: the tangent geometry no longer fits + fwFlyByCappedLatch = false; // consume the latch on any leg change + if (turnMode == NAV_FW_WP_TURN_COORD_FLY_OVER) { + // FLY_OVER: circle pinned at the overfly point. Tracking OFF: exit on the tangent + // through the next WP; tracking ON: bounded-intercept S onto the new leg itself. + const float px = posControl.activeWaypoint.pos.x; + const float py = posControl.activeWaypoint.pos.y; + const int32_t brgToWp = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - pos->y, px - pos->x))))); + const int32_t toWpErr = wrap_18000(brgToWp - cog); + if (ABS(toWpErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + const float arcRtmp = getFwCoordinatedTurnRadius(); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + const int8_t dirTmp = (toWpErr > 0) ? 1 : -1; + const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); + // Pin ahead by the roll-in drift so the ramp ends ON the circle + const float leadDist = 1.5f * v * (tTmp / 1000.0f); + const float cx = pos->x + leadDist * cos_approx(cogRad) + arcRtmp * cos_approx(cogRad + dirTmp * (M_PIf * 0.5f)); + const float cy = pos->y + leadDist * sin_approx(cogRad) + arcRtmp * sin_approx(cogRad + dirTmp * (M_PIf * 0.5f)); + if (navConfig()->fw.wp_tracking_accuracy && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + // Tracking ON: exit the main arc onto a BOUNDED intercept course (<= 45 deg to the + // leg, gamma = half the turn for shallow corners), short straight for the reverse + // roll, then a standard corner-cut arc rolls out ON the line + const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float ux = cos_approx(legRad), uy = sin_approx(legRad); + const int32_t turnCd = wrap_18000(legBearing - cog); + const float gammaRad = CENTIDEGREES_TO_RADIANS(constrainf(0.5f * (float)ABS(turnCd), 2000.0f, 4500.0f)); + const float icptRad = legRad + (float)dirTmp * gammaRad; + const float d1x = cos_approx(icptRad), d1y = sin_approx(icptRad); + const float nAng = icptRad - (float)dirTmp * (M_PIf * 0.5f); + const float p1x = cx + 2.0f * arcRtmp * cos_approx(nAng); // intercept line shifted R toward the counter side + const float p1y = cy + 2.0f * arcRtmp * sin_approx(nAng); + const float lAng = legRad - (float)dirTmp * (M_PIf * 0.5f); + const float b0x = px + arcRtmp * cos_approx(lAng); + const float b0y = py + arcRtmp * sin_approx(lAng); + const float cross = d1x * uy - d1y * ux; + if (fabsf(cross) > 0.17f) { // gamma >= 20 deg keeps the lines well separated + const float tt = ((b0x - p1x) * uy - (b0y - p1y) * ux) / cross; + const float o2x = p1x + tt * d1x; + const float o2y = p1y + tt * d1y; + const float rollAlong = (o2x - px) * ux + (o2y - py) * uy; + const float legLen = calc_length_pythagorean_2D(px - pos->x, py - pos->y); + // roll-out tangency must lie ahead of us and leave straight leg to the WP + if (rollAlong < 0.0f && -rollAlong < legLen && -rollAlong > 2.0f * arcRtmp) { + intoEx = o2x - arcRtmp * cos_approx(nAng); // pickup = tangency on the intercept line + intoEy = o2y - arcRtmp * sin_approx(nAng); + intoO2x = o2x; intoO2y = o2y; + intoR = arcRtmp; intoBOut = legBearing; intoDir = -dirTmp; + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = arcRtmp; + arcDir = dirTmp; + arcCx = cx; + arcCy = cy; + arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(icptRad)))); + phiNomCd = phiTmp; + tEaseMs = tTmp; + intoStage = FW_INTO_AWAY; // second arc staged: the shared pickup logic takes over + } + } + } + const float dCP = calc_length_pythagorean_2D(px - cx, py - cy); + if (!fwArcEngaged && dCP > 1.05f * arcRtmp) { // next WP outside the circle: a tangent exists + const float alphaCP = atan2_approx(py - cy, px - cx); + const float phiT = acos_approx(constrainf(arcRtmp / dCP, 0.0f, 1.0f)); + for (int8_t s = -1; s <= 1; s += 2) { // of the two tangent points, exit where the tangent points at the WP + const float th = alphaCP + (float)s * phiT; + const float tx = cx + arcRtmp * cos_approx(th); + const float ty = cy + arcRtmp * sin_approx(th); + if ((px - tx) * (-dirTmp * sin_approx(th)) + (py - ty) * (dirTmp * cos_approx(th)) > 0.0f) { + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = arcRtmp; + arcDir = dirTmp; + arcCx = cx; + arcCy = cy; + arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(py - ty, px - tx))))); + phiNomCd = phiTmp; + tEaseMs = tTmp; + } + } + } + } + } + const int32_t hdgErr = wrap_18000(legBearing - cog); + if (turnMode != NAV_FW_WP_TURN_COORD_FLY_OVER && ABS(hdgErr) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + const float arcRtmp = getFwCoordinatedTurnRadius(); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + const float omegaNomCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(v / arcRtmp)); // v/R == g*tan(phi)/v + const float psiTmp = 0.5f * omegaNomCds * (tTmp / 1000.0f); + if (capped || ABS(hdgErr) > NAV_FW_ARC_SHARP_TURN_CD) { + // Capped or near-reversal: no valid tangent circle - fly the bounded capture directly + fwArcEngaged = true; + phase = ARC_CAPTURE; + fwArcBankCmd = 0.0f; // fresh engagement: don't rate-limit against a stale command + rampMs = 0.0f; + rampStartCd = 0.0f; + arcDir = (hdgErr > 0) ? 1 : -1; + arcOutBearing = legBearing; + arcToLegLine = true; + phiNomCd = phiTmp; + tEaseMs = tTmp; + } else if (2.0f * psiTmp < (float)ABS(hdgErr)) { // enough turn left for a steady arc between the ease ramps + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = arcRtmp; + arcDir = (hdgErr > 0) ? 1 : -1; + arcOutBearing = legBearing; + phiNomCd = phiTmp; + tEaseMs = tTmp; + // Centre = intersection of both legs shifted R inside (inscribed circle), so the exit lands ON the out-leg + const float cogRad = CENTIDEGREES_TO_RADIANS((float)cog); + const float legRad = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float d1x = cos_approx(cogRad), d1y = sin_approx(cogRad); + const float d2x = cos_approx(legRad), d2y = sin_approx(legRad); + const float cross = d1x * d2y - d1y * d2x; + const float p1x = pos->x + arcRtmp * cos_approx(cogRad + arcDir * (M_PIf * 0.5f)); + const float p1y = pos->y + arcRtmp * sin_approx(cogRad + arcDir * (M_PIf * 0.5f)); + if (fabsf(cross) > 0.087f) { // legs not near-parallel (30..160 deg turn) + const float p2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(legRad + arcDir * (M_PIf * 0.5f)); + const float p2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(legRad + arcDir * (M_PIf * 0.5f)); + const float tt = ((p2x - p1x) * d2y - (p2y - p1y) * d2x) / cross; + arcCx = p1x + tt * d1x; + arcCy = p1y + tt * d1y; + } else { // degenerate -> tangent at the entry point + arcCx = p1x; + arcCy = p1y; + } + } + } + } + if (!fwArcEngaged && turnMode != NAV_FW_WP_TURN_COORD_FLY_BY && (navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (intoStage == FW_INTO_MAIN) { + // crossing flown: re-arm once the leg has switched (normally it already has, mid-arc) + intoStage = (ABS(wrap_18000(legBearing - intoBOut)) < 500) ? FW_INTO_IDLE : FW_INTO_DONE; + } + if (intoStage == FW_INTO_IDLE && turnMode == NAV_FW_WP_TURN_COORD_FLY_INTO) { + const int32_t nta = posControl.activeWaypoint.nextTurnAngle; + if (nta != -1 && ABS(nta) > NAV_FW_ARC_MIN_TURN_ANGLE_CD) { + const float arcRtmp = getFwCoordinatedTurnRadius(); + const float phiTmp = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRtmp))); + const float tTmp = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiTmp)); + const int8_t dirM = (nta > 0) ? 1 : -1; + const int32_t bOut = wrap_36000(legBearing + nta); + const float bOutRad = CENTIDEGREES_TO_RADIANS((float)bOut); + const float bInRad = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float ux = cos_approx(bInRad), uy = sin_approx(bInRad); + // Main circle pinned at the WP, counter circle on the inbound leg; centers + // sqrt((2R)^2+Ls^2) apart so an Ls gap gives the roll swing room + const float Ls = 2.0f * v * (tTmp / 1000.0f); + const float o2x = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bOutRad + dirM * (M_PIf * 0.5f)); + const float o2y = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bOutRad + dirM * (M_PIf * 0.5f)); + const float ax = posControl.activeWaypoint.pos.x + arcRtmp * cos_approx(bInRad - dirM * (M_PIf * 0.5f)); + const float ay = posControl.activeWaypoint.pos.y + arcRtmp * sin_approx(bInRad - dirM * (M_PIf * 0.5f)); + const float wx = o2x - ax, wy = o2y - ay; + const float wu = wx * ux + wy * uy; + const float disc = wu * wu - (wx * wx + wy * wy) + 4.0f * arcRtmp * arcRtmp + Ls * Ls; + if (disc > 0.0f) { + const float s = wu - sqrtf(disc); // signed along-leg offset of the S start from the WP + const float triggerDist = -s + 1.5f * v * (tTmp / 1000.0f); + if (s < 0.0f && posControl.wpDistance < triggerDist) { + const float o1x = ax + s * ux; + const float o1y = ay + s * uy; + const float cAng = atan2_approx(o2y - o1y, o2x - o1x); + const float beta = atan2_approx(Ls, 2.0f * arcRtmp); + const float nAng = cAng + (float)dirM * beta; + intoEx = o2x - arcRtmp * cos_approx(nAng); // main-arc pickup = internal-tangent touch on the main circle + intoEy = o2y - arcRtmp * sin_approx(nAng); + intoO2x = o2x; intoO2y = o2y; + intoR = arcRtmp; intoBOut = bOut; intoDir = dirM; + fwArcEngaged = true; + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = 0.0f; + arcR = arcRtmp; + arcDir = -dirM; // counter-arc first + arcCx = o1x; + arcCy = o1y; + arcOutBearing = wrap_36000(lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES( + cAng - (float)dirM * (M_PIf * 0.5f - beta))))); // internal-tangent course: the away arc rolls out onto it + phiNomCd = phiTmp; + tEaseMs = tTmp; + intoStage = FW_INTO_AWAY; + } + } + } + } else if (intoStage == FW_INTO_AWAY) { + intoStage = FW_INTO_IDLE; // only reachable via a controller reset mid-S: geometry is stale + } + } + if (!fwArcEngaged) { + DEBUG_SET(DEBUG_FW_TURN, 1, intoStage); + return; + } + } else { + // Mission advanced mid-arc: retarget the bounded capture onto the new leg instead of the stale out-bearing + if (ABS(wrap_18000(legBearing - fwArcPrevLegBearing)) > 500) { + fwFlyByCappedLatch = false; + arcOutBearing = legBearing; + arcToLegLine = true; + phase = ARC_CAPTURE; + if (intoStage == FW_INTO_AWAY) { + intoStage = FW_INTO_DONE; // staged S is stale: release the hand-back block + } + } + fwArcPrevLegBearing = legBearing; + + // Pick up the second arc at its tangency point. Along-track distance so a lateral residual + // cannot miss it; the heading gate blocks the trigger early in the first arc, where the + // pickup point still lies behind the exit course. + const float outRadE = CENTIDEGREES_TO_RADIANS((float)arcOutBearing); + if (intoStage == FW_INTO_AWAY + && ABS(wrap_18000(arcOutBearing - cog)) < 4500 + && ((intoEx - pos->x) * cos_approx(outRadE) + (intoEy - pos->y) * sin_approx(outRadE)) <= 1.5f * v * (tEaseMs / 1000.0f)) { + phase = ARC_RAMP_IN; + rampMs = 0.0f; + rampStartCd = fwArcBankCmd; // blend from the current bank + arcDir = intoDir; + // Radius from CURRENT groundspeed (the arming value may be unflyable downwind), and the + // circle re-anchored along the leg line through the actual position: wind drift becomes + // an along-track shift instead of a parallel roll-out offset + const float r2 = getFwCoordinatedTurnRadius(); + const float legR2 = CENTIDEGREES_TO_RADIANS((float)intoBOut); + const float u2x = cos_approx(legR2), u2y = sin_approx(legR2); + const float b0x = posControl.activeWaypoint.pos.x + r2 * cos_approx(legR2 + (float)intoDir * (M_PIf * 0.5f)); + const float b0y = posControl.activeWaypoint.pos.y + r2 * sin_approx(legR2 + (float)intoDir * (M_PIf * 0.5f)); + const float w2x = pos->x - b0x, w2y = pos->y - b0y; + const float w2u = w2x * u2x + w2y * u2y; + const float disc2 = w2u * w2u - (w2x * w2x + w2y * w2y) + r2 * r2; + if (disc2 > 0.0f) { + const float t2 = w2u + sqrtf(disc2); + arcR = r2; + arcCx = b0x + t2 * u2x; + arcCy = b0y + t2 * u2y; + phiNomCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * r2))); + tEaseMs = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiNomCd)); + } else { // drifted beyond the line: keep the planned circle + arcR = intoR; + arcCx = intoO2x; + arcCy = intoO2y; + } + arcOutBearing = intoBOut; + intoStage = FW_INTO_MAIN; + } + } + + // Leg-line capture (path tracking on): steer onto the track itself, not merely parallel to it - + // a reversal fallback otherwise ends a turn-diameter off the leg. Intercept angle tapers with + // the cross-track offset (1 cd/cm), capped at the tracker's own convergence limit. + if (arcToLegLine && navConfig()->fw.wp_tracking_accuracy) { + const float legRadT = CENTIDEGREES_TO_RADIANS((float)legBearing); + const float offLeg = (pos->x - posControl.activeWaypoint.pos.x) * (-sin_approx(legRadT)) + + (pos->y - posControl.activeWaypoint.pos.y) * cos_approx(legRadT); + const float gammaCd = constrainf(fabsf(offLeg), 0.0f, DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle)); + arcOutBearing = wrap_36000(legBearing - lrintf(SIGN(offLeg) * gammaCd)); + } + + rampMs += US2S(deltaMicros) * 1000.0f; + const int32_t hdgErrOut = wrap_18000(arcOutBearing - cog); + + // Roll-out lead: heading consumed by the shaped down-ramp plus the angle-P tail and servo delay + const float bankNowRad = CENTIDEGREES_TO_RADIANS((float)ABS(attitude.values.roll) * 10.0f); + const float omegaCds = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(GRAVITY_CMSS * tan_approx(bankNowRad) / MAX(v, NAV_FW_TURN_MIN_SPEED))); + const float levelGain = pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; // [1/s] + const float rollOutS = ((levelGain > 0.1f) ? (1.0f / levelGain) : 1.0f) + (float)navConfig()->fw.wp_turn_control_ease * 0.001f; + const float psiLeadCd = omegaCds * rollOutS + 0.5f * omegaCds * (tEaseMs / 1000.0f); + + switch (phase) { + case ARC_RAMP_IN: { + // Rate-consistent: a swing spanning 2*phi takes twice the standard ease time + const float rampSpanCd = fabsf((float)arcDir * phiNomCd - rampStartCd); + const float rampDurMs = MAX(tEaseMs * rampSpanCd / MAX(phiNomCd, 1.0f), 0.5f * tEaseMs); + const float p = (rampDurMs > 1.0f) ? constrainf(rampMs / rampDurMs, 0.0f, 1.0f) : 1.0f; + const float s = p * p * (3.0f - 2.0f * p); // smoothstep up + fwArcBankCmd = rampStartCd + ((float)arcDir * phiNomCd - rampStartCd) * s; + if (p >= 1.0f) { // roll-in done -> track the pre-placed tangent circle + phase = ARC_STEADY; + } + break; + } + case ARC_STEADY: { + const float dx = pos->x - arcCx; + const float dy = pos->y - arcCy; + const float eR = calc_length_pythagorean_2D(dx, dy) - arcR; // [cm], + = outside the arc + const float alpha = atan2_approx(dy, dx); // azimuth on the arc + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(arcDir * cos_approx(alpha), -arcDir * sin_approx(alpha))))); + const int32_t eH = wrap_18000(tangentBearing - cog); // [centideg] heading error to the arc tangent + // FF from current groundspeed: wind changes v along the arc + const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcR))); + fwArcBankCmd = arcDir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; + if (NAV_FW_ARC_EXIT_GAIN * (float)ABS(hdgErrOut) <= ABS(fwArcBankCmd) + || (float)ABS(hdgErrOut) <= psiLeadCd) { // remaining heading fits the shaped roll-out -> start it + phase = ARC_CAPTURE; + } + break; + } + case ARC_CAPTURE: + default: { + // No-overshoot envelope; the collapse is rate-limited to the ramp rate so the level-off is eased + int32_t captureErr = hdgErrOut; + if (ABS(captureErr) > 17000) { + captureErr = arcDir * ABS(captureErr); // ambiguous reversal: hold the engagement direction + } + const float errLeadCd = MAX((float)ABS(captureErr) - psiLeadCd, 0.0f); + float cmd = constrainf(NAV_FW_ARC_EXIT_GAIN * ((captureErr > 0) ? errLeadCd : -errLeadCd), -phiNomCd, phiNomCd); + const float maxStepCd = phiNomCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEaseMs, 1.0f); + if (fwArcBankCmd > 0.0f) { + cmd = MAX(cmd, fwArcBankCmd - maxStepCd); + } else if (fwArcBankCmd < 0.0f) { + cmd = MIN(cmd, fwArcBankCmd + maxStepCd); + } + fwArcBankCmd = cmd; + // Mid-S the gap between the arcs stays engaged: handing back there would give the PID and + // path tracking a moment of control while we sit a full turn diameter off the leg. + if (ABS(hdgErrOut) <= NAV_FW_ARC_EXIT_HANDOFF_CD && fabsf(fwArcBankCmd) <= phiNomCd * 0.1f + && intoStage != FW_INTO_AWAY) { // aligned and nearly level -> hand back + fwArcEngaged = false; + return; + } + break; + } + } + + // Clamp to the flyable ceiling: rate limits, handoff checks and the smoother seed must not + // run on a command the airframe cannot reach (wind can drive eR arbitrarily large) + const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); + fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(arcR)); // active arc radius [cm] + DEBUG_SET(DEBUG_FW_TURN, 1, (phase + 1) * 10 + intoStage); // coordinator state: (arc phase + 1)*10 + S stage + DEBUG_SET(DEBUG_FW_TURN, 2, arcOutBearing); // exit course [centideg] + DEBUG_SET(DEBUG_FW_TURN, 3, hdgErrOut); // remaining heading to the exit course [centideg] + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); // arc bank command [centideg] + DEBUG_SET(DEBUG_FW_TURN, 7, lrintf(tEaseMs)); // roll ease time [ms] -> sizes the turn leads + fwArcActive = true; +} + +// 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 +static void updateFwLoiterArc(timeDelta_t deltaMicros) +{ + static bool established = false; + + if (!needToCalculateCircularLoiter || fwArcEngaged) { + established = false; + return; + } + + const fpVector3_t *pos = &navGetCurrentActualPositionAndVelocity()->pos; + const float dcx = pos->x - posControl.desiredState.pos.x; + const float dcy = pos->y - posControl.desiredState.pos.y; + const float dist = calc_length_pythagorean_2D(dcx, dcy); + const float arcRadius = MAX(fwActiveLoiterRadius, (float)NAV_FW_TURN_RADIUS_MIN); + const int8_t dir = loiterDirection(); + const float alpha = atan2_approx(dcy, dcx); + const int32_t tangentBearing = lrintf(DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(dir * cos_approx(alpha), -dir * sin_approx(alpha))))); + const int32_t eH = wrap_18000(tangentBearing - posControl.actualState.cog); + const float eR = dist - arcRadius; + + if (!established) { + if (fabsf(eR) < NAV_FW_LOITER_CAPTURE_BAND * arcRadius && ABS(eH) < NAV_FW_LOITER_CAPTURE_ALIGN_CD) { + established = true; + fwArcBankCmd = fwLastNavRollCmdCd; // blend from the current command: no engage step + } else { + DEBUG_SET(DEBUG_FW_TURN, 1, 4); // loiter approach, carrot guidance + return; + } + } else if (fabsf(eR) > NAV_FW_LOITER_RELEASE_BAND * arcRadius) { + established = false; // grossly displaced: hand back to the carrot + return; + } + + const float v = posControl.actualState.velXY; + const float phiLiveCd = DEGREES_TO_CENTIDEGREES(RADIANS_TO_DEGREES(atan2_approx(v * v, GRAVITY_CMSS * arcRadius))); + const float targetCd = dir * (phiLiveCd + NAV_FW_ARC_RADIAL_GAIN * eR) + NAV_FW_ARC_HEADING_GAIN * (float)eH; + const float tEase = fwTurnEaseTimeMs(CENTIDEGREES_TO_DEGREES(phiLiveCd)); + const float maxStepCd = phiLiveCd * (US2S(deltaMicros) * 1000.0f) / MAX(tEase, 1.0f); + fwArcBankCmd += constrainf(targetCd - fwArcBankCmd, -maxStepCd, maxStepCd); + const float cmdLimitCd = DEGREES_TO_CENTIDEGREES(getFwEffectiveBankLimit()); + fwArcBankCmd = constrainf(fwArcBankCmd, -cmdLimitCd, cmdLimitCd); + + DEBUG_SET(DEBUG_FW_TURN, 1, 40); // loiter circle controller engaged + DEBUG_SET(DEBUG_FW_TURN, 4, lrintf(fwArcBankCmd)); + fwArcActive = true; +} + +static void calculateVirtualPositionTarget_FW(float trackingPeriod, timeDelta_t deltaMicros) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE) { return; @@ -334,6 +1096,14 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); + + /* Loiter-radius floor with per-revolution peak hold (see getFwStableLoiterRadius): keep the circle + * stable for the fixed-radius loiter tracker instead of chasing the wind-varying instantaneous value. */ + const bool inLoiter = (navGetCurrentStateFlags() & NAV_CTL_HOLD); + const float bearingFromCenter = atan2_approx(-posErrorY, -posErrorX); + navLoiterRadius = getFwStableLoiterRadius(navLoiterRadius, bearingFromCenter, inLoiter, deltaMicros); + fwActiveLoiterRadius = (float)navLoiterRadius; // expose to the turn feed-forward + fpVector3_t loiterCenterPos = posControl.desiredState.pos; int8_t loiterTurnDirection = loiterDirection(); @@ -351,38 +1121,26 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) needToCalculateCircularLoiter = false; } - /* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP - * Works for turns > 30 degs and < 160 degs. - * Option 1 switches to loiter path around waypoint using navLoiterRadius. - * Loiter centered on point inside turn at required distance from waypoint and - * on a bearing midway between current and next waypoint course bearings. - * Option 2 simply uses a normal turn once the turn initiation point is reached */ + /* 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); posControl.flags.wpTurnSmoothingActive = false; - if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // turnStartFactor adjusts start of loiter based on turn angle - float turnStartFactor; - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP - turnStartFactor = waypointTurnAngle / 6000.0f; - } else { // // cut inside turn missing WP - turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f); - } - // velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time - if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) { - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { - int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.bearing + 18000); - loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - - posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; - posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - - // turn direction to next waypoint - loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - - needToCalculateCircularLoiter = true; - } + 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) { + const float turnRadius = getFwCoordinatedTurnRadius(); + const float halfAngleTan = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 0.0f, NAV_FW_TURN_LEAD_TAN_MAX); + // Roll-in lead: the ramp is back-loaded and the ground track lags the bank (k calibrated in flight) + const float easeLeadDistance = posControl.actualState.velXY * (fwTurnEaseTimeMs(getFwPlanningBankDeg()) / 1000.0f) * 1.5f; + float turnStartDistance = easeLeadDistance + turnRadius * halfAngleTan; + // Cap how early the turn may begin (nav_fw_wp_turn_max_lead_time): never more than N ms of flight before the WP. + const float maxLeadDistance = posControl.actualState.velXY * (float)navConfig()->fw.wp_turn_max_lead_time * 0.001f; + const bool turnCapped = turnStartDistance > maxLeadDistance; + turnStartDistance = MIN(turnStartDistance, maxLeadDistance); + DEBUG_SET(DEBUG_FW_TURN, 0, lrintf(turnRadius)); // FLY_BY planning radius while approaching + if (posControl.wpDistance < turnStartDistance) { posControl.flags.wpTurnSmoothingActive = true; + fwFlyByCappedLatch = turnCapped; // capped corner cut -> the arc coordinator flies it direct instead } } @@ -398,6 +1156,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY); } + // Arc turn coordinator: manages the turn state and commands the roll bank directly + updateFwTurnArc(deltaMicros); + updateFwLoiterArc(deltaMicros); + // Calculate virtual waypoint virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget); virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget); @@ -484,8 +1246,9 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ - if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) { + /* If waypoint tracking enabled force craft toward and closely track along waypoint course line. + * Suppressed while the arc coordinator drives a turn (it tracks the arc, not the straight leg). */ + if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter && !fwArcActive) { if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { @@ -513,12 +1276,18 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } } else { - /* Keep state synced to the current error while not steering, so the - * controller re-engages cleanly on the next leg (no stale-data kick). */ + /* Keep state synced to the current error while not steering, and seed the convergence + * estimate from the geometric closing speed: re-engaging with a zero rate reads as + * "not converging" and commands the full correction in one step (arc hand-back kick). */ previousCrossTrackError = navCrossTrackError; previousCrossTrackErrorUpdateTime = currentTimeUs; - crossTrackErrorRate = 0.0f; - pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f); + const fpVector3_t *trackPos = &navGetCurrentActualPositionAndVelocity()->pos; + const float legRad = CENTIDEGREES_TO_RADIANS((float)posControl.activeWaypoint.bearing); + const float offLeg = (trackPos->x - virtualCoursePoint.x) * (-sin_approx(legRad)) + + (trackPos->y - virtualCoursePoint.y) * cos_approx(legRad); + const float cogOffRad = CENTIDEGREES_TO_RADIANS((float)wrap_18000(posControl.actualState.cog - posControl.activeWaypoint.bearing)); + crossTrackErrorRate = -SIGN(offLeg) * posControl.actualState.velXY * sin_approx(cogOffRad); + pt1FilterReset(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate); } } /* @@ -551,20 +1320,37 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta previousTimeMonitoringUpdate = currentTimeUs; } - // Only allow PID integrator to shrink if error is decreasing over time - const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); + // Only allow PID integrator to shrink if error is decreasing over time. + // Freeze the integrator while the arc drives the turn - the carrot error keeps one sign and winds it up + const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR + | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0) + | (fwArcActive ? PID_FREEZE_INTEGRATOR : 0); // Input error in (deg*100), output roll angle (deg*100) + const float navBankLimit = getFwControlBankLimit(); // planning target on WP turns, guard ceiling in loiter float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros), - -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), - DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), + -DEGREES_TO_CENTIDEGREES(navBankLimit), + DEGREES_TO_CENTIDEGREES(navBankLimit), pidFlags); - // Apply low-pass filter to prevent rapid correction - rollAdjustment = pt1FilterApply3(&fwPosControllerCorrectionFilterState, rollAdjustment, US2S(deltaMicros)); + // Arc bank overrides the PID; its ramps are already shaped, so the S-curve smoother is bypassed + // and re-seeded at handback to avoid a stale-state step + if (fwArcActive) { + rollAdjustment = fwArcBankCmd; + fwRollSmoothSeedCd = fwArcBankCmd; // else the handback re-seeds from a stale reset value (brief roll twitch) + fwRollSmoothReseed = true; + } else { + // Coordinated-turn feed-forward: command the bank for the active turn radius so the PID only trims. + rollAdjustment += getFwTurnFeedForward(navHeadingError); + rollAdjustment = applyFwRollInSmoothing(rollAdjustment, deltaMicros, fwRollSmoothReseed); + fwRollSmoothReseed = false; + } + rollAdjustment = constrainf(rollAdjustment, -DEGREES_TO_CENTIDEGREES(navBankLimit), DEGREES_TO_CENTIDEGREES(navBankLimit)); // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees) posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment); + fwLastNavRollCmdCd = rollAdjustment; + fwLastNavRollCmdTimeUs = currentTimeUs; /* * Yaw adjustment @@ -593,7 +1379,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) // Account for pilot's roll input (move position target left/right at max of max_manual_speed) // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs // FIXME: verify the above - calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2); + calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2, deltaMicrosPositionUpdate); updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate); needToCalculateCircularLoiter = false; } @@ -682,8 +1468,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { // ROLL >0 right, <0 left - const uint8_t maxBankAngle = navConfig()->fw.max_bank_angle; - int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(maxBankAngle), DEGREES_TO_DECIDEGREES(maxBankAngle)); + const int16_t navBankLimitDeciDeg = (int16_t)lrintf(DEGREES_TO_DECIDEGREES(getFwControlBankLimit())); + int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -navBankLimitDeciDeg, navBankLimitDeciDeg); rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]); } @@ -718,6 +1504,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } uint16_t correctedThrottleValue = constrain(cruiseThrottle + throttleCorrection, minThrottle, maxThrottle); + const uint16_t autoThrottleValue = correctedThrottleValue; // auto demand before manual increase, for the energy guard // Manual throttle increase if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { @@ -732,6 +1519,9 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); + + // Update the energy guard now that this cycle's pitch + throttle commands are known. + updateFwEnergyBankGuard(currentTimeUs, autoThrottleValue); } }