Commit Graph

767 Commits

Author SHA1 Message Date
Peter Barker ce04d69d53 AP_NavEKF3: correct compilation in SITL when logging is disabled
this code is gated on HAL_BOARD_SITL already, and usually we have logging in SITL.  But not always
2024-03-12 09:25:17 +11:00
Marek S. Lukasiewicz 07681416f5 AP_NavEKF3: Match handling of MAV_CMD_EXTERNAL_POSITION_ESTIMATE to common MAVLink dialect 2024-03-07 18:13:51 +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 a517d5fed1 AP_NavEKF3: correct initialisation of BeaconFusion data structure
the fusionReport object is not being cleared when the filter undergoes a forced reset.

So delete and recreate the object.  Will also make this sensitive to the number of beacons changing.

Also don't attempt to allocate zero bytes.
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
Peter Barker e9d065c1cc AP_NavEKF3: tidy population of fusion reports
simply take a refefence and use it
2024-02-26 20:55:41 +11:00
Simon Hancock 07eecd3b67 AP_NavEKF3: Updates to log message units and help text
Set quaternion component units to no-unit from UNKNOWN
2024-01-21 14:26:54 +11:00
Peter Barker 0afed2f8c2 AP_NavEKF3: allow compilation with HAL_LOGGING_ENABLED false 2024-01-17 18:25:55 +11:00
Peter Barker 10c8af3409 AP_NavEKF3: do not use fmaxF on floating point values
it returns double, which will not fit into this float
2023-09-12 09:47:17 +10:00
Paul Riseborough 5aa7bd0b7a AP_NavEKF3: Allow operation with EK3_SRCx_POSZ = 0 (NONE) 2023-08-23 18:25:26 +10:00
Andy Piper c57efa1b20 AP_NavEKF3: allow high values of EK3_ALT_M_NSE for boards without baros 2023-08-23 18:25:26 +10:00
Paul Riseborough e3c0175bb4 AP_NavEKF3: Update EK3_GLITCH_RADIUS metadata 2023-08-16 17:56:43 +10:00
Paul Riseborough 842882355f AP_NavEKF3: increase innovation variance instead of clipping innovations 2023-08-16 17:56:43 +10:00
Paul Riseborough 573bd7c7f3 AP_NavEKF3: Provide option to clip velocity and position innovations 2023-08-16 17:56:43 +10:00
Andrew Tridgell 484312df93 AP_NavEKF3: fixed race condition in updateFilterStatus()
filter status was initially set to zero then updated. This interacts
with the IMU filtering code which checks filter status from a
different thread to determine active_EKF_type(). When the race
condition is hit then the IMU we are running notch filters on changes
for a single sample, causing a notch filter glitch
2023-08-12 17:47:56 +10:00
Andy Piper 2f97a9855a AP_NavEKF3: fix docs on ABIAS_P_NSE_DEFAULT 2023-07-14 08:33:05 +09:00
Andrew Tridgell d11d4fa0bf AP_NavEKF3: fixed velocity reset on AID_NONE
The ResetVelocity() function is only supposed to reset XY states, not
Z state. Resetting the Z state for velocity results in a large
velocity glitch if a vehicle is descending or ascending when aiding
switches to AID_NONE

this fixes #19386
2023-06-26 18:09:31 +10:00
Paul Riseborough 4d03327470 AP_NavEKF3: Fix sign error in position fix delay compensation 2023-06-08 16:31:22 +10:00
Paul Riseborough 3677cb025d AP_NavEKF3: Add handlers for external lat lng position set 2023-06-06 15:19:12 +10:00
Paul Riseborough c91909032f AP_NavEKF3: Increase delta velocity bias state process noise
Required due to state variance collapse on ground with some systems using RTK GPS.
2023-05-24 10:31:26 +10:00
Paul Riseborough 1053daaf90 AP_NavEKF3: Strengthen recovery from bad delta velocity bias learning 2023-05-24 10:31:26 +10:00
Paul Riseborough b1111e79fe AP_NavEKF3: Increase default value of EK3_ABIAS_P_NSE
This is required because some hardware setups with RTK GPS have experienced a collapse of the delta velocity state variances.
2023-05-24 10:31:26 +10:00
Paul Riseborough ef76890f9b AP_NavEKF3: Retune and fix delta velocity bias state variance protection 2023-05-24 10:31:26 +10:00
Andrew Tridgell 52cb59e61d AP_NavEKF3: handle core setup failure
ensure num_cores is left as zero so that calls such as Log_Write don't
de-reference nullptr
2023-05-08 16:28:08 +10:00
Peter Barker ce8afaf57b AP_NavEKF3: use quat.to_euler(Vector3f&) 2023-04-19 14:24:45 +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
Paul Riseborough a25aa8d2ac AP_NavEKF3: Lock in wind state estimates when using srag to dead reckon 2023-04-04 09:22:23 +10:00
bugobliterator deeeb4134c AP_NavEKF3: use INS_MAX_INSTANCES instead of MAX_EKF_CORES for IMU mask 2023-03-21 10:04:16 +11:00
Peter Barker 2af6a05fa2 AP_NavEKF3: include writeWheelOdom symbol even if no body-odom
like the method above it, we need this symbol or we won't compile
2023-02-11 10:36:33 +11: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
Iampete1 16b1bd8b9a AP_NavEKF3: pre-arm return failure message for correct core 2023-01-31 13:36:56 +11:00
Peter Barker 8a312d4f34 AP_NavEKF3: use classes to fix MATH_CHECK_INDEXES 2023-01-25 19:58:54 +11:00
Peter Barker 7f4c5a9a84 AP_NavEKF3: tidy variable declarations 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
Peter Barker b716793a0e AP_NavEKF3: remove duplicated lines in parameter metadata 2022-12-28 13:21:18 +11:00
Andrew Tridgell 8bf8d4889a AP_NavEKF3: added get_enable() 2022-12-20 10:34:22 +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
Andy Piper e4b8d8a9b6 AP_NavEKF3: ensure wind estimation from airspeed can be used on its own 2022-12-14 19:19:58 +11:00
Stephen Dade abcacec25f AP_NavEKF3: Add support for fixed height optical flow 2022-12-01 08:03:47 +09:00
Paul Riseborough 33349ed92a AP_NavEKF3: Prevent on ground range to ground being used in flight 2022-11-22 11:23:44 +11:00
Paul Riseborough 2228937536 AP_NavEKF3: Don't allow range finder use to start if terrain state is stale 2022-11-22 11:23:44 +11:00
Peter Barker 5cb6906968 AP_NavEKF3: add and use AP_BEACON_ENABLED 2022-11-16 08:16:31 +11:00
Peter Barker 806b2708c1 AP_NavEKF3: change namespace of MultiCopter and FixedWing params
this stops the libraries knowing anything about AP_Vehicle
2022-11-09 19:04:37 +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
jackhong12 4a4f361a17 all: use CLASS_NO_COPY() macro 2022-10-04 11:23:04 +11:00
Andrew Tridgell 17d9018a91 AP_NavEKF3: fixed EKF3 origin alt inconsistency
always use common origin, and adjust output posD for difference
between public and local lane origin heights
2022-09-30 07:44:42 +10:00
Paul Riseborough 481bec5ff5 AP_NavEKF3: Allow wind states to recover faster when airspeed sensor failed 2022-09-26 14:17:53 +10: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
nrt 0c65cd1b47 AP_NavEKF3: added a getter function to infer the source index used by ekf3 2022-08-18 02:05:27 -04:00
Peter Barker 1b13dd053b AP_NavEKF3: use send_mesage for sending status report 2022-08-16 09:45:51 +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
Henry Wurzburg b7f49db1f7 AP_NavEKF3: Add streaming log verbosity level parameter 2022-08-02 10:21:39 +10:00
Iampete1 cf14e741ee AP_NavEKF3: only include log struture if enabled 2022-07-13 18:14:12 +10:00
Andrew Tridgell e2a83ba428 AP_NavEKF3: fixed undefined behaviour in logging 2022-07-12 11:00:08 +10:00
Pierre Kancir 21890000f7 AP_NavEKF3: fix python files exec permissions 2022-06-08 08:16:42 +09:00
Andrew Tridgell d5670ace3b AP_NavEKF3: fixed use of configured() vs configured_in_storage() 2022-06-06 13:11:50 +10:00
Andrew Tridgell 74c4f3c9a9 AP_NavEKF3: log set source events 2022-05-31 09:17:37 +10:00
Randy Mackay eafb118b47 AP_NavEKF3: set filter status dead_reckoning bit 2022-05-25 20:05:21 +09:00
Randy Mackay e37bf9ad53 AP_NavEKF3: drag fusion for att and velAiding
also add dragTimeout
2022-05-25 20:05:21 +09:00
Randy Mackay 61c4643aed AP_NavEKF3: minor comment fix 2022-05-25 20:05:21 +09:00
Ruffalo-sunghwan 5a4d61e3a4 AP_NavEKF3 : correct comment spelling 2022-05-24 20:27:45 +09:00
Randy Mackay e0023a99bc AP_NavEKF3: replace AP_HAL::millis() with dal.millis() 2022-05-18 17:59:57 +10:00
Randy Mackay 37e9ce3fb7 AP_NavEKF3: correct wind estimate param descriptions
EK3_MCOEF becomes EK3_DRAG_MCOEF
EK3_BCOEF_X/Y becomes EK3_DRAG_BCOEF_X/Y
2022-04-13 07:57:35 +09:00
Peter Barker 0b16c4063e AP_NavEKF3: stop libraries including AP_Logger.h in .h files
AP_Logger.h is a nexus of includes; while this is being improved over
time, there's no reason for the library headers to include AP_Logger.h
as the logger itself is access by singleton and the structures are in
LogStructure.h

This necessitated moving The PID_Info structure out of AP_Logger's
namespace.  This cleans up a pretty nasty bit - that structure is
definitely not simply used for logging, but also used to pass pid
information around to controllers!

There are a lot of patches in here because AP_Logger.h, acting as a
nexus, was providing transitive header file inclusion in many (some
unlikely!) places.
2022-04-08 19:18:38 +10:00
Peter Barker adf9c21d48 AP_NavEKF3: remove instance id from EK3 external interface
Removes passing of instance id in interfaces where -1 was the only value
ever passed in
2022-04-06 17:36:05 +10:00
Peter Barker df4911cbcb AP_NavEKF3: avoid direct use of Location alt field 2022-03-22 10:33:37 +11:00
Andrew Tridgell a4da65ea05 AP_NavEKF3: fixed constrain indexing bug
fixes #20180

thanks to @liyue75 for spotting this!
2022-03-01 17:06:44 +09:00
Henry Wurzburg 0f05cb06fa AP_NavEKF3: update and correct GSF parameter documentation 2022-02-15 10:56:35 +11:00
Randy Mackay 0c7ad9511f AP_NavEKF3: FuseOptFlow only calculates range and velocity once
was unnecessarily recalculating these values before fusion
2022-02-09 08:42:29 +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 cfcdc1e78f AP_NavEKF3: optflow terrain reset after 5sec 2022-02-02 18:04:43 +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
Randy Mackay cff3794d25 AP_NavEKF3: minor spelling fixes 2022-01-17 11:30:47 +09:00
VMsunghwan db9c1bbfaf AP_NavEKF3 : correct typo
receverPosDownMax -> receiverPosDownMax
receverPosDownMin -> receiverPosDownMin
calcuate -> calculate
2022-01-17 11:25:56 +09:00
Hwurzburg 970e5f829b AP_NavEKF3: remove user unintelligible GCS message 2022-01-12 08:11:18 +11: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
Paul Riseborough 5178f4a7d0 AP_NavEKF3: Add missing covariance row reset 2021-12-20 09:28:37 +11:00
Peter Barker 0cb4425eb9 AP_NavEKF3: fix type, 'willbe' -> 'will be' 2021-12-17 09:44:57 +09:00
Peter Barker 15ec2c9c40 AP_NavEKF3: allow boards to override NavEKF3 features 2021-12-14 11:12:50 +11:00
Andrew Tridgell 468444bef9 AP_NavEKF3: revert compass parameter changes 2021-12-04 16:51:53 +11:00
Peter Barker 308f4e99b4 AP_NavEKF3: correct structure used for logging
XKF4 and XKF5 are clones of NKF4 and NKF5, which is why this worked
2021-12-03 15:34:21 +09:00
Josh Henderson a89f58a775 AP_NavEKF3: allow define for IMU_MASK_DEFAULT 2021-11-30 10:20:54 +11:00
Iampete1 e818decc39 AP_NavEKF3: update compass param discription 2021-11-30 08:14:43 +09:00
Andrew Tridgell 86c481b4b0 AP_NavEKF3: fixed switch to non-zero primary on disarm
if EK3_PRIMARY is not zero then we were not switching to it when
disarmed
2021-11-07 10:11:52 +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
Peter Barker 670663a741 AP_NavEKF3: allow hwdef to override IMU default 2021-10-16 10:26:29 +11:00
Gone4Dirt 0abfde0a7d AP_NavEKF3: Add APM_BUILD_Heli 2021-09-29 19:55:48 +10:00
Josh Henderson 67eb6d17eb AP_NavEKF3: initialize stateStruct.quat to unit length 2021-09-28 19:09:16 +10: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 8429c66860 AP_NavEKF3: Make subsequent bad IMU event detections faster 2021-09-23 18:55:28 +10:00
Paul Riseborough 458c67639d AP_NavEKF3: Force velocity state to follow GPS when IMU data is bad 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