gsf: heading from GPS velocity and the IMU, without the compass, for airplanes (ahrs_gsf_yaw) by MrScothh · Pull Request #12159 · iNavFlight/inav

MrScothh · 2026-10-18T00:34:56Z

Builds on #12142 (mag_learn), so this branch contains it and the diff shows both until #12142 is merged: the blackbox group and the settings sit after mag_learn's. The change of its own is the last three commits.

An airplane's heading comes from the compass, and with a bad compass (interference, a wrong align_mag, an old calibration) INAV has nothing that tells it so: the GPS course helps only in straight flight without wind. This adds a heading that doesn't use the compass at all, the Gaussian sum filter of Paul Riseborough's EKF-GSF yaw estimator (ArduPilot EKFGSF_yaw, PX4 yaw_estimator), and lets the AHRS use it if asked.

How it works:

Five 3-state EKFs (velocity north, east and yaw), each started from a different yaw, fed with the horizontal acceleration and the nose's heading rate, and corrected by the GPS velocity. Their weights follow how well each one predicts the GPS; the estimate is their weighted mean, with its spread as sigma. The covariance and Kalman equations are Riseborough's. Unlike ArduPilot and PX4, each model takes roll and pitch from INAV's AHRS instead of running its own: ArduPilot's own tilt took the takeoff run's acceleration for nose up and came out 130-180 degrees wrong after liftoff in my replays of HITL flights.
It runs only on airplanes, in flight (from the first detected flight to landing or disarm, not just while the throttle is above cruise), at 50 Hz, with the IMU accumulated in imuUpdateAttitude() and each GPS sample fused when the filter reaches its time.
The GPS delay starts at one receiver interval plus 20 ms and is then measured in flight, from the turns: the delay that best lines up the IMU's and the GPS's velocity changes over 400 ms, kept once its sigma is under 25 ms.
"Usable" needs 25 m/s of velocity change from manoeuvres (about a 90 degree turn at 16 m/s, counted in 1 s blocks above 1 m/s so vibration doesn't count) and sigma under 15 degrees for 3 s. A filtered normalised innovation over 6 (chi-square, 2 dof, 95 %) flags it as inconsistent and takes "usable" away; 10 s inconsistent restarts it.
Blackbox group GSF, off by default: gsfYaw, gsfYawStd, gsfExcitation, gsfNis, gsfFlags, gsfGpsDelay, gsfMagErr (compass heading minus the estimate).

With ahrs_gsf_yaw = ON (default OFF) the AHRS uses it:

the estimate replaces the GPS course as the AHRS's GPS heading, weighted 1 at a sigma of 5 degrees down to 0 at 15, and shared with the compass as ahrs_gps_yaw_weight already sets;
the compass is left out after 3 s more than max(25 degrees, 3 sigma) away from it, and comes back after 10 s under half that;
an AHRS heading more than max(45 degrees, 3 sigma) away for 1 s, with no valid compass to hold it, is reset to the estimate, at most twice a flight.

Off, the estimator still runs and the blackbox logs it, so a flight shows what it would have done.

USE_GSF_YAW is on with GPS on targets with more than 512 KB of flash.

Size against #12142:

Target	Flash	Fast RAM (DTCM/CCM/TCM)	RAM
TBS_LUCID_H7_WING	+8708 B	+1904 B	+192 B, ITCM +32 B
KAKUTEF7 / KAKUTEF7HDV	+9620 / +9652 B	+1936 B	+88 / +96 B, ITCM +104 B (99.71 %)
SPEEDYBEEF745AIO	+8864 B	+1936 B	+104 B, ITCM +168 B (94.19 %)
MATEKF765	+9144 B	+1940 B	+76 B, ITCM -32 B
MATEKF405	+8196 B (79.98 %)	+1936 B	+92 B
NEUTRONRCF435WING	+8856 B	+1888 B	+48 B (89.44 %)
MATEKF722	+8 B: not built on 512 KB

On KAKUTEF7 and KAKUTEF7HDV the ITCM grows by 104 B, to 16336 of 16384: none of it is new code. nm shows the same FAST_CODE functions a little longer (gyroFilter +36, pidController +20, dTermProcess +16, mixTable +16, applyRateDynamics +8, pidApplyMulticopterRateController +8), because LTO addresses the globals differently once the program grows; moving the new globals out of FASTRAM, or into one struct, didn't change it.

imuConfig_t grows from 16 to 18 bytes with gsf_yaw appended at offset 16, after the end of the stored record, with a zero default, so the PG version stays and a stored config keeps its values; pg_struct_sizes.reference.db is updated so that validate-pg-for-release.sh agrees.

Tested:

9 unit tests: convergence from any heading after a turn, no claimed heading without manoeuvres, a noisy GPS, the wrap across south, the GPS delay measured within 5 ms at 0-260 ms (10 Hz) and 220 ms (5 Hz), also with a noisy GPS and gyro drift, none without manoeuvres, and the nose heading rate under roll.
SITL with a fake X-Plane and wind (10 m/s from north and 5 from east): sigma about 107 degrees until the first turn, then 2-3 degrees error within 5 s, usable after 40 s; with a compass 50-180 degrees wrong and ahrs_gsf_yaw = ON, the compass left out, one reset, then the AHRS within 4 degrees for the rest of the flight.
HITL with X-Plane 11 on a TBS Lucid H7, a 1.2 m trainer, four flights: compass turned 60 degrees with ON (AHRS +25 degrees until usable at about 95 s, then the compass left out and within 6 degrees) and with OFF (AHRS between +11 and +47 the whole flight, the estimator flagging it); compass turned 180 degrees with the GPS 150 ms late (compass left out, AHRS within 8 degrees) and 300 ms late with ahrs_gps_yaw_weight = 0 (AHRS at +178 until a reset to +10 at 98 s, then within 7 degrees; the delay measured at 118-146 ms, the X-Plane plugin's accelerometer itself being about 170 ms late).

Configurator: iNavFlight/inav-configurator#2844. Blackbox Explorer: iNavFlight/blackbox-log-viewer#127.

The header declared motor[0..7], while the I and P frames write getMotorCount() values, up to MAX_SUPPORTED_MOTORS (12). With 9 to 12 motors and motor logging on, every frame carried more values than the header described, and no frame of the log decoded. The header now declares motor[8..11] behind four more conditions. The condition cache was a uint64_t already full on dual-gyro targets, so it becomes an array of 32-bit words sized for every condition. The MSP enum reference gets the new values (that enum's section only).

With mag_learn ON, the compass keeps one raw reading per field direction during each flight (12 headings x 6 equal-area elevation bands), refits the offsets with the sphere fit the ground calibration already uses and saves magzero at disarm when the flight turned through at least 270 degrees and the fit agrees with the stored calibration: radius spread within 5%, radius within 30% of what maggain implies, no axis moving more than 30% of the radius. maggain is kept. The fit runs on samples scaled by maggain, so axes of different sensitivity weigh alike, and a ridge toward the stored offset keeps a direction the flight never swept (the vertical in level flight, the normal of a circle flown at one bank) where it was. A calibration started while armed, or mag_learn switched off, drops what the flight had collected. The save waits out the 5 s in which INAV accepts an emergency rearm after a disarm, so a rearm finds the same calibration and no flash write, and then for the aircraft to be still; if it still moves a minute after the disarm, nothing is saved. EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS moves to fc_core.h for that. The estimate is refitted about once a second in flight and logged in the blackbox slow frame (magBias, magBiasFlags, magBiasSectors, magBiasSpread; header mag_learn and mag_zero) under its own field group, MAG_LEARN (on by default in a new configuration, logged only while mag_learn is ON), and MSP2_INAV_MAG_LEARN reports it for the Configurator. Built on iNavFlight#12127 for the wider condition cache and on the slow-frame conditions. USE_MAG_LEARN on USE_MAG targets with more than 512 KB of flash. The setting is appended to compassConfig_t with a zero default, so the PG version stays: on ARM (short enums) the struct had no tail padding and grows from 24 to 26 bytes; on SITL the new byte lands in former tail padding, which the reset template writes as zero.

A Gaussian sum of five 3-state EKFs (velocity north and east, yaw), each started from a different yaw, as in Paul Riseborough's EKF-GSF yaw estimator. Each model is handed the horizontal acceleration and the yaw rate instead of running its own attitude filter, so roll and pitch can come from INAV's AHRS. Core only, with unit tests on simulated flights.

Every AHRS update adds the horizontal acceleration and the yaw rate, both from rMat's roll and pitch only, so the compass never reaches the estimate. A 50 Hz task steps the filter on 20 ms windows held back by the GPS delay and fuses each GPS velocity at the time it was taken, with a 3D fix, 6 satellites and 5 m accuracy, and not while the aircraft rotates faster than 90 deg/s. It learns from INAV's first flight detection to its landing detection, since isFlightDetected alone drops whenever the throttle is under cruise or hover. The estimate counts as usable once it has seen 25 m/s of velocity change and its sigma has stayed under 15 deg for 3 s. Blackbox group GSF: yaw, sigma, velocity change seen, filtered NIS of the leading model, flags. Targets with more than 512 KB of flash.

…sf_yaw) The GPS delay starts at one receiver interval plus 20 ms (120 ms at 10 Hz, 220 ms at 5 Hz, ArduPilot's u-blox lags) and is then measured in flight: for each delay from 0 to 300 ms, how well the 400 ms velocity change of the IMU matches the GPS one after the best rotation, the rotation standing for the unknown heading. A measurement is kept once its least-squares sigma is under 25 ms. Manoeuvres count per second and only past 1 m/s, so vibration and tilt errors do not add up in straight flight. A filtered NIS over 6 flags the estimate inconsistent and not usable; 10 s of it start it over and give the compass its trust back. The yaw rate fed to the models is the rate of the nose's heading (the Euler yaw rate from roll and pitch), not the rate about the vertical: a roll about the nose at pitch leaves the heading alone. With ahrs_gsf_yaw on (off by default), a usable estimate replaces the GPS course as the AHRS's GPS heading, the compass is left out after it has stayed 25 deg or 3 sigma off it for 3 s (back after 10 s within half that, and whenever disarmed), and a heading 45 deg off with no compass to hold it is reset to it, at most twice a flight; the navigation heading target moves with the reset so the aircraft does not turn. The compass still reaches the estimate through the tilt, since the GPS inertia compensation uses the full orientation. Timers are 64-bit, so a flight past 35 min keeps its state. The IMU accumulation runs only while the task does. A tailsitter transition restarts the estimator. Blackbox: gsfGpsDelay, gsfMagErr, more flags, header ahrs_gsf_yaw; the log is where the estimator is read, there is no MSP message. imuConfig_t grows by the appended zero-default field without a version bump (pgLoad copies the stored bytes over the defaults): the PG size database says 18 bytes at version 2.

qodo-code-review · 2026-10-18T00:35:02Z

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

qodo-free-for-open-source-projects · 2026-10-18T00:36:43Z

PR Summary by Qodo

Add compass-independent GSF heading for airplanes

✨ Enhancement 🐞 Bug fix 🧪 Tests 📝 Documentation 🕐 40+ Minutes

AI Description

• Estimate airplane heading from GPS velocity and IMU motion, without relying on the compass.
• Optionally use settled estimates in AHRS to reject a disagreeing compass and recover heading.
• Include in-flight compass offset learning, Blackbox diagnostics, and a 12-motor log-header fix.

Diagram

graph TD
  GPS["GPS Velocity"] --> GSF["GSF Estimator"] --> AHRS["AHRS Heading"]
  Compass["Compass Samples"] --> MagLearn["Offset Learner"] --> Config[("Stored Offsets")]
  IMU["IMU Attitude"] --> GSF
  Config --> Compass
  Compass --> AHRS
  GSF --> Log["Blackbox Log"]
  MagLearn --> Log

High-Level Assessment

The following are alternative approaches to this PR:

1. Give each GSF model its own tilt estimator

➕ Reduces dependence on AHRS roll and pitch.
➖ Adds computation and memory pressure.
➖ Takeoff acceleration can corrupt independent tilt, as reported in flight replays.

2. Keep GSF diagnostic-only

➕ Avoids changing flight-critical AHRS heading selection initially.
➖ Cannot recover heading when the compass is persistently wrong.

Recommendation: Reusing AHRS tilt and gating AHRS adoption on manoeuvre evidence, uncertainty, and consistency is the better fit for these targets. Review the inherited compass-learning and Blackbox changes as a distinct dependency where possible, so the heading safety path remains easy to assess.

Files changed (32) +2526 / -161

Enhancement (14) +1583 / -52

blackbox.cLog estimator status and declare all motor fields +156/-49

Log estimator status and declare all motor fields

• Adds conditional GSF main-frame and compass-learning slow-frame fields. Expands motor headers to 12, grows the condition cache, and records relevant settings in log headers.

src/main/blackbox/blackbox.c

blackbox.hDefine GSF and MAG_LEARN field groups +2/-0

Define GSF and MAG_LEARN field groups
• Adds selectable Blackbox feature bits for the two diagnostic groups.
src/main/blackbox/blackbox.h

cli.cExpose estimator Blackbox groups in CLI +2/-0

Expose estimator Blackbox groups in CLI
• Adds MAG_LEARN and GSF to selectable Blackbox field names.
src/main/fc/cli.c

fc_msp.cReport compass-learning status over MSP +13/-0

Report compass-learning status over MSP
• Serializes flags, sector counts, spread, and estimated offset changes in a new MSP reply.
src/main/fc/fc_msp.c

fc_tasks.cSchedule the GSF estimator at 50 Hz +13/-0

Schedule the GSF estimator at 50 Hz
• Enables a medium-priority GSF task when GPS and an accelerometer are available.
src/main/fc/fc_tasks.c

gsf_yaw.cImplement GPS-and-IMU GSF yaw estimation +878/-0

Implement GPS-and-IMU GSF yaw estimation

• Implements five weighted velocity/yaw EKFs, GPS-delay estimation, and flight-only fusion. Qualifies heading use, detects compass disagreement, requests bounded AHRS resets, and exposes status.

src/main/flight/gsf_yaw.c

gsf_yaw.hDefine GSF state and integration interfaces +115/-0

Define GSF state and integration interfaces
• Declares model, history, delay, status, and AHRS input types and estimator entry points.
src/main/flight/gsf_yaw.h

imu.cIntegrate qualified GSF heading into AHRS +98/-3

Integrate qualified GSF heading into AHRS

• Accumulates IMU motion for GSF and optionally substitutes its heading for GPS course. It can omit a disagreeing compass and apply requested heading resets.

src/main/flight/imu.c

msp_protocol_v2_inav.hAssign a compass-learning MSP command +2/-0

Assign a compass-learning MSP command
• Defines the command identifier for reading current or last-flight learning status.
src/main/msp/msp_protocol_v2_inav.h

navigation_pos_estimator.cForward GPS updates to GSF +5/-0

Forward GPS updates to GSF
• Passes incoming GPS velocity observations to the GSF estimator.
src/main/navigation/navigation_pos_estimator.c

scheduler.hRegister the GSF scheduler task +3/-0

Register the GSF scheduler task
• Adds the feature-gated GSF task identifier.
src/main/scheduler/scheduler.h

compass.cCollect and conditionally save learned compass offsets +74/-0

Collect and conditionally save learned compass offsets
• Collects readings while flying, periodically evaluates a fit, and saves accepted offsets only after disarm, the rearm window, and a stillness check.
src/main/sensors/compass.c

compass_learn.cFit guarded in-flight compass offsets +153/-0

Fit guarded in-flight compass offsets

• Bins magnetic samples by direction and refits offsets using gain-scaled readings and a prior toward stored calibration. Rejects insufficient coverage, poor sphere fits, scale mismatch, and excessive corrections.

src/main/sensors/compass_learn.c

compass_learn.hDefine compass-learning limits and status +69/-0

Define compass-learning limits and status
• Declares sample-bin dimensions, acceptance limits, status flags, and the learning interface.
src/main/sensors/compass_learn.h

Bug fix (1) +7 / -0

blackbox_fielddefs.hExtend Blackbox field conditions +7/-0

Extend Blackbox field conditions
• Defines conditions for motors nine through twelve and both estimator groups.
src/main/blackbox/blackbox_fielddefs.h

Refactor (2) +2 / -1

fc_core.cShare the emergency rearm interval +0/-1

Share the emergency rearm interval
• Removes the local definition now exposed through fc_core.h for deferred calibration saving.
src/main/fc/fc_core.c

fc_core.hExpose the emergency rearm interval +2/-0

Expose the emergency rearm interval
• Makes the five-second in-flight rearm window available to compass-learning save logic.
src/main/fc/fc_core.h

Tests (3) +618 / -0

CMakeLists.txtBuild estimator unit tests +6/-0

Build estimator unit tests
• Registers compass-learning and GSF yaw tests with their required sources and definitions.
src/test/unit/CMakeLists.txt

compass_learn_unittest.ccTest compass-learning fits and rejection gates +310/-0

Test compass-learning fits and rejection gates
• Simulates flights to check convergence, limited vertical observability, repeated-flight learning, and rejection of inadequate or corrupted calibration data.
src/test/unit/compass_learn_unittest.cc

gsf_yaw_unittest.ccTest GSF convergence and GPS-delay measurement +302/-0

Test GSF convergence and GPS-delay measurement
• Simulates turns, noise, heading wrap, and manoeuvre-free flight. Checks delay estimation across receiver rates and nose-heading rate under roll.
src/test/unit/gsf_yaw_unittest.cc

Documentation (7) +280 / -108

Blackbox.mdDocument the MAG_LEARN Blackbox group +1/-0

Document the MAG_LEARN Blackbox group
• Lists the optional slow-frame group for in-flight compass learning.
docs/Blackbox.md

Sensors.mdExplain in-flight compass offset learning +10/-0

Explain in-flight compass offset learning
• Describes collection, fit acceptance, deferred saving, limitations, and diagnostic fields.
docs/Sensors.md

Settings.mdDocument GSF and compass-learning settings +20/-0

Document GSF and compass-learning settings
• Adds generated setting descriptions and defaults for ahrs_gsf_yaw and mag_learn.
docs/Settings.md

README.mdDocument the compass-learning MSP reply +17/-0

Document the compass-learning MSP reply
• Specifies the new status command, payload, flags, and availability.
docs/development/msp/README.md

inav_enums.jsonRegister logging and estimator enums +90/-54

Register logging and estimator enums
• Adds Blackbox groups, expanded motor conditions, and GSF and compass-learning status flags.
docs/development/msp/inav_enums.json

inav_enums_ref.mdPublish updated enum reference +97/-53

Publish updated enum reference
• Reflects expanded Blackbox conditions and the new estimator status flags.
docs/development/msp/inav_enums_ref.md

msp_messages.jsonSpecify MSP2_INAV_MAG_LEARN +45/-1

Specify MSP2_INAV_MAG_LEARN
• Adds the compass-learning status reply and increments the message-specification patch version.
docs/development/msp/msp_messages.json

Other (5) +36 / -0

CMakeLists.txtInclude both estimator modules in the build +4/-0

Include both estimator modules in the build
• Registers the GSF yaw and compass-learning source and header files.
src/main/CMakeLists.txt

settings.yamlAdd opt-in heading and compass-learning settings +12/-0

Add opt-in heading and compass-learning settings
• Defines default-off ahrs_gsf_yaw and mag_learn settings behind their build features.
src/main/fc/settings.yaml

imu.hAdd AHRS GSF configuration and compass-heading access +7/-0

Add AHRS GSF configuration and compass-heading access
• Appends the optional gsf_yaw setting and declares a compass-heading helper for comparison.
src/main/flight/imu.h

compass.hAdd the compass-learning setting +3/-0

Add the compass-learning setting
• Appends a feature-gated magLearn field to compass configuration.
src/main/sensors/compass.h

common_post.hGate estimators by target capabilities +10/-0

Gate estimators by target capabilities
• Enables compass learning on compass targets and GSF yaw on GPS targets with more than 512 KB of flash.
src/main/target/common_post.h

qodo-free-for-open-source-projects · 2026-10-18T00:38:44Z

Code Review by Qodo

🐞 Bugs (0) 📘 Rule violations (0) 📎 Requirement gaps (0) 🎨 UX issues (0) 🔗 Cross-repo conflicts (0) 📜 Skill insights (0)

1. ~~A lost GPS fix leaves stale yaw in use~~ ✓ Resolved

Description

taskGsfYaw() computes usable from fusion state, excitation and covariance without checking
whether GPS velocity is still current. After a fix or receiver timeout stops new samples, the AHRS
can continue accepting the last GPS-corrected heading while the filter predicts forward from the
gyro.

Code

src/main/flight/gsf_yaw.c[R830-832]

+    const bool settled = gsfState.fusing && !inconsistent && gsfStatus.excitation >= GSF_USABLE_EXCITATION &&
+        gsfStatus.yawSigma < GSF_USABLE_SIGMA;
+    const bool usable = heldFor(&gsfSettledSince, settled, currentTimeUs, GSF_USABLE_SETTLE_US);

Evidence
The GPS callback rejects updates without a fix or valid horizontal velocity, but the task's
usability test has no freshness condition. The receiver timeout clears the GPS fix independently of
the GSF state.
src/main/flight/gsf_yaw.c[572-598]
src/main/flight/gsf_yaw.c[772-774]
src/main/flight/gsf_yaw.c[829-832]
src/main/io/gps.c[564-570]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
A previously usable GSF heading remains usable after GPS velocity updates stop, allowing the AHRS to follow an estimate no longer corrected by GPS.
## Fix Focus Areas
- src/main/flight/gsf_yaw.c[572-598]
- src/main/flight/gsf_yaw.c[829-832]
## Recommended Fix
Track the time and validity of accepted GPS velocity updates. Require a recent valid update to retain GSF usability, and test fix loss and receiver timeout after convergence.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

2. ~~GSF state starts as RAM garbage on AT32 boards~~ ✓ Resolved

Description

Every piece of GSF state (gsfState, gsfWindows, gsfWindowHead/gsfWindowCount, gsfHistory,
gsfDelay, gsfStatus, gsfGpsIntervalUs, gsfActive, gsfMagDisagrees, …) is declared
STATIC_FASTRAM, and the code assumes it starts at zero (taskGsfYaw resets only when
model[0].weight == 0.0f, and the ring indices and counters are never initialised). The AT32F43x
startup clears only .bss, not .fastram_bss, so on AT32 targets that build USE_GSF_YAW (e.g.
NEUTRONRCF435WING) closeWindow/gsfHistoryAdd/gsfDelayAddGps index their arrays with garbage
uint8_t heads (up to 255, writing past 16/40/8-entry arrays into other FASTRAM data), a NaN
gsfGpsIntervalUs never recovers, and garbage weights or gsfMagDisagrees reach the AHRS and the
blackbox.

Code

src/main/flight/gsf_yaw.c[R504-509]

+STATIC_FASTRAM gsfState_t gsfState;
+STATIC_FASTRAM gsfWindow_t gsfAccumulating;
+STATIC_FASTRAM gsfWindow_t gsfWindows[GSF_WINDOW_COUNT];
+STATIC_FASTRAM uint8_t gsfWindowHead;      // where the next window goes
+STATIC_FASTRAM uint8_t gsfWindowCount;
+STATIC_FASTRAM gsfImuHistory_t gsfHistory;

Evidence

The repo's own guide says AT32 startup does not zero .fastram_bss and that FASTRAM variables need
explicit initialisation, including to zero. The AT32 startup only zero-fills _sbss.._ebss, and
the AT32 linker script puts .fastram_bss in a separate NOLOAD section. This PR's own
compass_learn.c even notes 'AT32 startup leaves FASTRAM uncleared'. gsf_yaw.c only guards gsfState
through the weight test and uses gsfWindowHead, gsfHistory.head and gsfDelay.gpsHead as array
indices with no bounds check, so garbage values there mean out-of-bounds writes.

docs/development/ram-and-flash-optimization.md[112-121]
src/main/startup/startup_at32f435_437.s[77-87]
src/main/target/link/at32_flash_f4_split.ld[206-215]
src/main/flight/gsf_yaw.c[631-638]
src/main/flight/gsf_yaw.c[733-739]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
The GSF yaw module keeps all its state in `STATIC_FASTRAM` and relies on it starting at zero. AT32F43x startup does not clear `.fastram_bss`, so ring indices, counters, floats and bools start as garbage. That causes out-of-bounds array writes and NaN state.
## Fix Focus Areas
- src/main/flight/gsf_yaw.c[504-536]
- src/main/flight/gsf_yaw.c[733-739]
- src/main/fc/fc_tasks.c[408-410]
## Recommended Fix
Add a `gsfYawInit()` function that memsets every GSF static to zero (or group them in a single struct and memset that struct), then calls `gsfReset(&gsfState)` and `gsfReset` on the delay estimator and history. Call it once at boot, e.g. from `fcTasksInit` or `imuInit`, before the task or `gsfYawAccumulateImu` can run. Remove the `model[0].weight == 0.0f` lazy-reset check. Another option is to move these variables out of FASTRAM into plain static `.bss`.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

3. ~~A delay change mis-times one GPS sample~~ ✓ Resolved

Description

gsfYawOnNewGpsData() fixes gsfGpsTakenAt using the current delay, but taskGsfYaw() can change
that delay from the same pending sample before advancing the IMU state. When the measured delay
first replaces the receiver-rate estimate, the pending velocity can be fused against the wrong IMU
window or skipped because its recorded time no longer matches the filter's delay.

Code

src/main/flight/gsf_yaw.c[R792-795]

+    // The latest GPS sample, fused once the filter has reached the time it was taken
+    if (gsfGpsPending && last && cmpTimeUs(last->end, gsfGpsTakenAt) >= -GSF_WINDOW_US) {
+        gsfGpsPending = false;
+        const bool spaced = !gsfState.fusing || gsfGpsTakenAt - gsfLastFusion >= GSF_FUSION_MIN_US;

Evidence
The callback timestamps the pending sample before the task runs; the task may then update its delay
and advance windows using the new value, while the fusion check still uses the old timestamp.
src/main/flight/gsf_yaw.c[592-597]
src/main/flight/gsf_yaw.c[664-686]
src/main/flight/gsf_yaw.c[780-806]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
A pending GPS sample retains its old delayed timestamp while the filter can advance according to a newly measured delay before fusion.
## Fix Focus Areas
- src/main/flight/gsf_yaw.c[592-597]
- src/main/flight/gsf_yaw.c[664-686]
- src/main/flight/gsf_yaw.c[780-806]
## Recommended Fix
Apply the same delay to the pending sample timestamp and the IMU prediction target, or defer a newly measured delay until after the pending sample is processed. Test both increases and decreases from the initial delay.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

4. ~~Compass stays excluded once the estimate stops being usable~~ ✓ Resolved

Description

gsfYawGetAhrsInput sets ignoreMag from the latched gsfMagDisagrees even when the GSF estimate
is unusable and its AHRS input weight is zero; checkCompass clears only the timers when usability
is lost. After the compass has been excluded, loss of GSF usability due to lost GPS, landing
detection, or growing sigma makes imuGsfHeadingInput suppress the compass without supplying a GSF
reference, leaving the AHRS to rely on gyro heading if GPS course is also invalid until the latch
clears or the craft disarms.

Code

src/main/flight/gsf_yaw.c[R870-873]

+    if (enabled && (gsfStatus.flags & GSF_YAW_USABLE)) {
+        in->weight = constrainf(scaleRangef(gsfStatus.yawSigma, GSF_AHRS_FULL_SIGMA, GSF_USABLE_SIGMA, 1.0f, 0.0f), 0.0f, 1.0f);
+    }
+    in->ignoreMag = enabled && gsfMagDisagrees;

Evidence
checkCompass returns early on !usable and resets the since-timers without clearing
gsfMagDisagrees. gsfYawGetAhrsInput can therefore set ignoreMag while the GSF weight is zero,
and imuGsfHeadingInput applies that suppression before returning without a GSF reference when
in.weight <= 0; GPS course is available only when isGPSHeadingValid().
src/main/flight/gsf_yaw.c[691-711]
src/main/flight/gsf_yaw.c[865-876]
src/main/flight/imu.c[839-865]
src/main/flight/gsf_yaw.c[691-708]
src/main/flight/gsf_yaw.c[864-875]
src/main/flight/imu.c[981-999]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
A latched compass disagreement can keep the compass excluded after GSF yaw becomes unusable, even though the AHRS receives no GSF heading. If GPS course is also invalid, it has no absolute heading reference.
## Fix Focus Areas
- src/main/flight/gsf_yaw.c[691-711]
- src/main/flight/gsf_yaw.c[865-876]
- src/main/flight/imu.c[839-856]
## Recommended Fix
In `gsfYawGetAhrsInput`, set `in->ignoreMag = enabled && gsfMagDisagrees && (gsfStatus.flags & GSF_YAW_USABLE)` so an unusable GSF estimate cannot suppress the compass. Alternatively, clear `gsfMagDisagrees` in `checkCompass` after usability has been lost for a while. Test a converged flight in which the compass has been excluded and GSF subsequently loses usability; confirm the compass becomes available again when no usable GSF heading is supplied.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Tip of the day

💡 Did you know, you can import rules and skills only from the repositories your team follows

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗

github-actions · 2026-10-18T01:29:41Z

RAM / Flash usage vs. base commit 13ebed9 — commit b5343a5

Target	Flash Δ	RAM Δ
MATEKF405 ⚠️	+11168 B (+1.54%)	CCM: +2368 B (+7.27%) RAM: +148 B (+0.14%)
MATEKF722	+120 B (+0.02%)	ITCM_RAM: ±0 B (±0.00%) RAM: +8 B (+0.01%) TCM: ±0 B (±0.00%)
MATEKF765 ⚠️	+11312 B (+1.50%)	DTCM_RAM: +2376 B (+8.30%) SRAM1: +144 B (+0.11%)
MATEKH743 ⚠️	+11372 B (+1.42%)	D2_RAM: ±0 B (±0.00%) DTCM_RAM: +2336 B (+18.04%) ITCM_RAM: +32 B (+0.21%) RAM: +168 B (+0.12%)

See RAM/flash optimization guide for techniques to reduce usage.

github-actions · 2026-10-18T01:30:09Z

Test firmware build ready — commit b5343a5

Download firmware for PR #12159

251 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

With correlated north and east errors the 5 sigma clip judged spikes along the correlation as ordinary and clipped ordinary updates across it. A unit test fuses a 6.7 sigma innovation of opposite signs.

… the compass - Usable needs a GPS velocity in the last second: without one the heading is the gyro's alone. - The compass is left out only while the estimate is usable, so a lost GPS gives it back at once. - The pending GPS sample is fused with the delay it was stamped and stepped to with; a new delay applies from the next one, and a sample the filter is already well past (the delay grew) is dropped. - gsfYawInit() zeroes the state at boot from imuInit(): AT32's startup clears .bss but not .fastram_bss.

MrScothh · 2026-10-19T15:42:35Z

Expected: the GSF heading estimator, about 11.2-11.4 KB of flash where it is built, and its state, about 2.3 KB in FASTRAM (CCM on F405, DTCM on F7 and H7). MATEKF722 builds without the estimator (+120 B there). The +32 B of ITCM is not GSF code: on KAKUTEF7, which shows the same +32 B, the only function that grows in ITCM is gyroFilter() (+36 B, one more instruction), where LTO addresses the gyro's variables from different base registers once the new static data has moved them.

MrScothh · 2026-10-20T09:06:38Z

The flash and the fast RAM are the GSF heading (USE_GSF_YAW, on with GPS on targets with more than 512 KB of flash, so MATEKF722 shows only +120 B) plus #12142 (mag_learn), which this PR builds on. Against #12142 this PR adds +8.2 KB of flash and +1.9 KB of fast RAM on MATEKF405, which ends at 79.98 % (table in the description).

MrScothh · 2026-10-21T08:31:19Z

Flight data from four flights on 2026-10-17 and 18 on a fixed wing (1.2 m T-28, TBS Lucid H7 Wing, u-blox GPS at
10 Hz), ahrs_gsf_yaw = ON, 21 minutes in the air. This flew as the local branch this PR was made from, without the
two fixes of 2026-10-18 (63d26cf, the sign of the off-diagonal term, and b5343a5).

flight	usable after	usable	heading sigma (median)	GPS delay measured	NIS (median)	in the AHRS	compass disagrees
1	10 s	97 %	1.9 deg	95 ms	0.24	97 %	75 %
2	10 s	96 %	1.9 deg	118 ms	0.21	96 %	56 %
3	8 s	98 %	2.0 deg	105 ms	0.16	98 %	61 %
4	8 s	96 %	1.5 deg	91 ms	0.24	96 %	93 %

Never inconsistent, no heading reset. The compass field followed the motor current:
against the GSF heading it read -9 to +1 deg in the median, between -39 and +28 deg at the 10th and 90th percentiles.
The AHRS ran on the GSF heading and left the compass out for 56-93 % of each flight, as intended.

MrScothh added 6 commits October 8, 2026 02:14

blackbox: slow-frame fields can carry a condition

36b7aa6

This was referenced Oct 18, 2026

Calibration and Blackbox tabs: heading from GPS (GSF) iNavFlight/inav-configurator#2844

Open

Fields of INAV's GSF heading, the heading from GPS velocity without the compass iNavFlight/blackbox-log-viewer#127

Open

MrScothh mentioned this pull request Oct 18, 2026

compass: mag_learn fits the offsets against the attitude while the GSF heading is usable, and learns mag_yaw_trim #12160

Open

qodo-free-for-open-source-projects Bot reviewed Oct 18, 2026

View reviewed changes

Comment thread src/main/flight/gsf_yaw.c Outdated

Comment thread src/main/flight/gsf_yaw.c Outdated

Comment thread src/main/flight/gsf_yaw.c

Comment thread src/main/flight/gsf_yaw.c Outdated

MrScothh added 2 commits October 18, 2026 17:47

gsf: the inverse innovation covariance takes minus its off-diagonal term

63d26cf

With correlated north and east errors the 5 sigma clip judged spikes along the correlation as ordinary and clipped ordinary updates across it. A unit test fuses a 6.7 sigma innovation of opposite signs.

Navigation Menu

Repository navigation

gsf: heading from GPS velocity and the IMU, without the compass, for airplanes (ahrs_gsf_yaw) - #12159

gsf: heading from GPS velocity and the IMU, without the compass, for airplanes (ahrs_gsf_yaw)#12159
MrScothh wants to merge 8 commits into
iNavFlight:maintenance-10.xfrom
MrScothh:feature/gsf-heading

MrScothh commented Oct 18, 2026 •

edited

Loading

Uh oh!

qodo-code-review Bot commented Oct 18, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Oct 18, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Oct 18, 2026 •

edited

Loading

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Oct 18, 2026 •

edited

Loading

Uh oh!

github-actions Bot commented Oct 18, 2026 •

edited

Loading

Uh oh!

MrScothh commented Oct 19, 2026

Uh oh!

MrScothh commented Oct 20, 2026

Uh oh!

MrScothh commented Oct 21, 2026

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

Conversation

MrScothh commented Oct 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

qodo-code-review Bot commented Oct 18, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Oct 18, 2026

PR Summary by Qodo

Uh oh!

qodo-free-for-open-source-projects Bot commented Oct 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Code Review by Qodo

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Oct 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

github-actions Bot commented Oct 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

MrScothh commented Oct 19, 2026

Uh oh!

MrScothh commented Oct 20, 2026

Uh oh!

MrScothh commented Oct 21, 2026

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

MrScothh commented Oct 18, 2026 •

edited

Loading

qodo-free-for-open-source-projects Bot commented Oct 18, 2026 •

edited

Loading

github-actions Bot commented Oct 18, 2026 •

edited

Loading

github-actions Bot commented Oct 18, 2026 •

edited

Loading