Commit Graph

236 Commits

Author SHA1 Message Date
Peter Barker
c8a20726ff AP_NavEKF3: do not store prediction-enabled as state
this is only used in one place, and that place is called from the same routine setting the persistent state.  The only other place which calls readIMUData shouldn't be running the prediction step, but mmay, depending on the previous setting of the prediction step.

We are not initialising this state on filter reset, so it's possible that the state will be set when we do an InitialiseFilterBootstrap, which is probably not desired
2024-07-09 10:45:03 +10:00
Peter Barker
ce48932f4d AP_NavEKF3: remove storedRange member variable if rangefinder measurements disabled 2024-07-04 19:21:44 +10:00
Willian Galvani
cb74ebbddd EKF3: allow earth-frame fields to be estimated with an origin but no GPS 2024-07-03 16:49:27 +10:00
Peter Barker
13e7d04497 AP_NavEKF3: make AP_RANGEFINDER_ENABLED remove more code 2024-07-02 09:17:26 +10:00
Clyde McQueen
0e6543f0e4 AP_NavEKF3: sub update for yaw reset and inFlight checks
on sub, request final yaw reset after diving 0.5m.
Also update rangefinder tests for sub
2024-06-25 18:09:37 +10:00
Paul Riseborough
61874da020 AP_NavEKF3: Rework method of synthesising airspeed for dead reckoning
The previous method resulted in data incest and fusion of predicted airspeed on every EKF internal time step.
This was not apparent during flight where the vehicle was turning, but during long straight legs did not constrain along track drift.
2024-06-09 14:25:02 +10:00
Paul Riseborough
720c2da807 AP_NavEKF3: Rework GPS jamming resiliency
Make it user selectable.
Remove potential for a race condition between decisions based on latest data and the EKF fusion processing which operates on a delayed time horizon. This is achieved by preventing data entering the buffer if awaiting checks to pass ensuring that no EKF fusion time horizon processes can use data that hasn't passed checks.
Log the waitingForGpsChecks class variable
2024-06-09 14:25:02 +10:00
Paul Riseborough
6baeb1cd9d AP_NavEKF3: Fix bug causing in flight yaw align to not complete 2024-06-09 14:25:02 +10:00
Paul Riseborough
87bf8d9997 AP_NavEKF3: Treat wind as truth when deadreckoning with no airspeed sensor 2024-06-09 14:25:02 +10:00
Paul Riseborough
324d5da811 AP_NavEKF3: Use last observed wind states to enable dead reckoning
These changes enable the EKF to use the last observable wind velocity vector estimate to synthesise an airspeed measurement if operating without an airspeed sensor and when all other measurement types that can constrain velocoty drift are lost. This enables the EKF to use dead reckoning to continue after loss of GPS when there is no air speed sensor fitted and without the need to set a default airspeed value.
The logic used to fuse a default airspeed value has also been cleaned up and the call to FuseAirSpeed() from inside SelectBetaDragFusion() has been removed.

AP_NavEKF3: Fix error in default airspeed observation variance

AP_NavEKF3: Enable shadow fusion of airspeed when sensor is disabled
2024-06-09 14:25:02 +10:00
Paul Riseborough
e53416e77b AP_NavEKF3: Apply GPS quality checks following loss of 3D fix if velocity error is bounded 2024-06-09 14:25:02 +10:00
Randy Mackay
4e2dd7c399 AP_NavEKF3: accept set origin even when using GPS 2024-05-21 09:56:02 +10:00
Paul Riseborough
3c8b3be7a7 AP_NavEKF3: Change yaw source to stop compass use when calibrating 2024-04-23 15:19:56 +09:00
Randy Mackay
65b4db5ede AP_NavEKF3: define Yaw alignment min GPS speed per vehicle 2024-03-26 10:01:42 +11:00
Peter Barker
21edc6aee1 AP_NavEKF3: do not trust number of beacons to not change
if the count from the beacon library changes we may end up looking at memory we shouldn't
2024-02-28 18:37:42 +11:00
Peter Barker
2df3cb98c6 AP_NavEKF3: move initialisation of rngBcn into BeaconFusion method 2024-02-27 12:04:14 +11:00
Paul Riseborough
3677cb025d AP_NavEKF3: Add handlers for external lat lng position set 2023-06-06 15:19:12 +10:00
Peter Barker
5d68f44694 AP_NavEKF3: move beacon variables into structure
AP_NavEKF3: change beacon variable names
2023-04-18 09:28:02 +10:00
Paul Riseborough
5ab17496f6 AP_NavEKF3: Improve protection against GPS glitches during yaw alignment 2023-04-17 08:48:03 +10:00
Paul Riseborough
7063b385cf AP_NavEKF3: Reduce use of GSF yaw for planes with no compass 2023-04-17 08:48:03 +10:00
Peter Barker
35a5a73f52 AP_NavEKF3: avoid using struct Location
clang reports this could be a problem when compiling under some EABIs.  Remove it from most places as it is just noise, replace with class where we want to avoid including Location.h
2023-02-04 22:51:54 +11:00
Peter Barker
8a312d4f34 AP_NavEKF3: use classes to fix MATH_CHECK_INDEXES 2023-01-25 19:58:54 +11:00
Peter Barker
da95985dd7 AP_NavEKF3: remove mag_state state
we don't need to persist this across multiple calls as we now fuse all axes on the one step.

I've moved the defintion of these variables to where they are initialised to make it clear they're not used uninitialised.
2023-01-25 19:58:54 +11:00
Peter Barker
063a041d22 AP_NavEKF3: add trailing comma to AidingMode enum, reindent same 2023-01-25 19:50:34 +11:00
Tatsuya Yamaguchi
366a325fc0 AP_NavEKF3: do not check the second range finder if RANGEFINDER_MAX_INSTANCES is 1 2022-12-20 10:05:24 +11:00
Stephen Dade
abcacec25f AP_NavEKF3: Add support for fixed height optical flow 2022-12-01 08:03:47 +09:00
Peter Barker
5cb6906968 AP_NavEKF3: add and use AP_BEACON_ENABLED 2022-11-16 08:16:31 +11:00
Andrew Tridgell
92f25150f6 AP_NavEKF3: fixed getLLH alt for local origin height
this fixes a bug introduced in #21834

this fix in #21834 was correct for getPosD, but should not have been
applied to getLLH

this caused cruise mode in plane to descend/ascend by the difference
between the public and local origins on mode entry

fixes #21984
2022-10-23 11:23:48 +11:00
Paul Riseborough
399f30bfce AP_NavEKF3: Allow reporting of airspeed consistency for a deselected sensor 2022-09-26 14:17:53 +10:00
Paul Riseborough
eb0e9230a1 AP_NavEKF3: Add accessor function for airspeed health monitoring 2022-09-26 14:17:53 +10:00
Paul Riseborough
608b4c0713 AP_NavEKF3: Enable monitoring of unhealthy airspeed sensors
Innovations and innovation test ratios will still be calculated and reported for an unhealthy sensor, but the EKF states wnd covariance matrix will not be modified.
2022-09-26 14:17:53 +10:00
Peter Barker
0665f9c32b AP_NavEKF3: store and use lastKnownPositionD
When returning our last-known-position we were supplying lat/lng but not
altitude.

This usually really doesn't matter as this result generally isn't used,
especially for altitude.  OTOH, it may prevent a bug into the future.
2022-08-22 19:48:03 +10:00
Peter Barker
0443c8561b AP_NavEKF3: stop using GCS_MAVLINK.h in header files
... by passing through a reference to a link object instead
2022-08-16 09:45:51 +10:00
Tamas Nepusz
8959d35b91 AP_NavEKF3: add EK3_GPS_VACC_MAX as a threshold that decides whether to use GPS as altitude source depending on vAcc 2022-08-03 17:57:05 +10:00
Randy Mackay
e37bf9ad53 AP_NavEKF3: drag fusion for att and velAiding
also add dragTimeout
2022-05-25 20:05:21 +09:00
Ruffalo-sunghwan
5a4d61e3a4 AP_NavEKF3 : correct comment spelling 2022-05-24 20:27:45 +09:00
Andrew Tridgell
9dcff1a23f AP_NavEKF3: set rejecting_airspeed flag
we report as rejecting airspeed when we have not fused airspeed for 3s
and want to use airspeed
2022-02-07 14:13:49 +11:00
Randy Mackay
e16a64383f AP_NavEKF3: getOptFlowSample returns latest correct flow data for use in calibration 2022-01-29 08:26:12 +09:00
Andrew Tridgell
a33febd90a AP_NavEKF3: fixed gaps in EKF logging
timestamps should not be static as otherwise some lanes will not be
logged
2021-12-28 16:03:47 +11:00
Andrew Tridgell
282209e3e6 AP_NavEKF3: add accessor for GSF yaw estimator 2021-11-01 09:00:27 +11:00
Andy Piper
bc0101e446 AP_NavEKF3: convert APM_BUILD_COPTER_OR_HELI() to APM_BUILD_COPTER_OR_HELI 2021-10-26 11:42:12 +11:00
Paul Riseborough
c828bdfbc9 AP_NavEKF3: Reduce output observer vertical velocity error when bad IMU 2021-09-23 18:55:28 +10:00
Paul Riseborough
c0d88e2673 AP_NavEKF3: Fix vertical velocity reset
Fixes bug that prevents the vertical velocity being reset to the GPS if the position aiding has already timed out and improves sensitivity of the bad IMU data check.
2021-09-23 18:55:28 +10:00
Randy Mackay
63e579d738 AP_NavEKF3: add using_extnav_for_yaw 2021-08-31 09:20:17 +10:00
Randy Mackay
0edfd2634f AP_NavEKF3: rename using_external_yaw to using_noncompass_for_yaw 2021-08-31 09:20:17 +10:00
Randy Mackay
d1f2acd813 AP_NavEKF3: rename varInnovOptFlow to flowVarInnov
also renamed innovOptFlow to flowInnov
2021-08-24 16:49:04 +09:00
Randy Mackay
85ade10e85 AP_NavEKF3: ofDataNew made local 2021-08-24 16:49:04 +09:00
Randy Mackay
11847cfcf5 AP_NavEKF3: always calculate optical flow innovations and variances 2021-08-24 16:49:04 +09:00
Randy Mackay
5779ebd177 AP_NavEKF3: remove unused Tbn_flow 2021-08-24 16:49:04 +09:00
Randy Mackay
2a2cde68ef AP_NavEKF3: skip GSF reset count check if source actively changed
also only fail all compass on emergency reset
2021-08-10 09:44:57 +10:00