Commit Graph

78 Commits

Author SHA1 Message Date
Randy Mackay
8931e50166 AP_NavEKF3: integrate Source for alt 2020-11-20 16:43:44 +09:00
Randy Mackay
c21d58ebea AP_NavEKF3: integrate Source for position 2020-11-20 16:43:44 +09:00
Andrew Tridgell
f5275fd0ad AP_NavEKF3: fixed use of pointers in ringbuffers
these don't work with AP_DAL
2020-11-10 16:15:45 +11:00
Andrew Tridgell
9b81c5a1e0 AP_NavEKF3: use dal reference in EKF backends
saves a bit of flash space
2020-11-10 16:15:45 +11:00
Andrew Tridgell
096aab9388 AP_NavEKF3: convert to use AP_DAL for new replay structure
Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
2020-11-10 16:15:45 +11:00
Randy Mackay
b726630ef4 AP_NavEKF3: pos timeout or glitch does not reset vel if fusing successfully
vel is not reset if GPS velocity, optical flow, visual odometry or wheel
encoders are successfully being fused
2020-10-28 19:06:53 +11:00
Randy Mackay
3b38aa6d7b AP_NavEKF3: GPS change pos reset uses ResetPositonNE and ResetPositionD
Using these existing functions removes some code duplicate
non-functional change
2020-10-28 19:03:34 +11:00
Randy Mackay
2f1ae29a9c AP_NavEKF3: extnav data gets corrected boolean
CorrectExtNavForSensorOffset and CorrectExtNavVelForSensorOffset use
corrected boolean to avoid correcting twice
2020-10-24 12:35:58 +11:00
Randy Mackay
a16de76f57 AP_NavEKF3: correct extnav position as it is recalled from the buffer
This makes extnav position correction consistent with the velocity correction
2020-10-24 12:35:58 +11:00
Randy Mackay
71487e9c10 AP_NavEKF3: gps_elements gets corrected flag
CorrectGPSForAntennaOffset uses this flag to ensure position and velocity are only corrected once
2020-10-24 08:49:33 +09:00
Randy Mackay
3d4e1cd5c5 AP_NavEKF3: CorrectGPSForAntennaOffset always corrects vel 2020-10-24 08:49:33 +09:00
Randy Mackay
e25579cc00 AP_NavEKF3: correct GPS for position as it is recalled from buffer 2020-10-24 08:49:33 +09:00
Randy Mackay
e4056086e0 AP_NavEKF3: minor format and spelling fixes 2020-10-21 13:54:22 +09:00
Randy Mackay
231a958e51 AP_NavEKF3: minor comment fix 2020-10-21 11:40:35 +09:00
Randy Mackay
e4ccd21dd7 AP_NavEKF3: consolidate extNav height source timeouts 2020-10-20 17:11:26 +09:00
Randy Mackay
6989a28bfc AP_NavEKF3: allow alt from other sources when using ExtNav
also allows using ExtNav alt when not using it for XY position
removes redundant and incorrect setting of activeHgtSource from ExtNav
which was always being overwritten by the correct value later in selectHeightForFusion
2020-10-20 17:11:26 +09:00
Randy Mackay
0cebde0632 AP_NavEKF3: minor restructure of how fusePosData and fuseVelData are set
this non-functional change slightly reduces the number of places we set these variables
2020-10-20 17:11:26 +09:00
Randy Mackay
3d5161f364 AP_NavEKF3: remove unused usingWheelSensors variable
This variable was set but never consumed
2020-08-21 13:18:58 +09:00
chobits
7ad9e3548f AP_NavEKF3: make reset source variable local 2020-08-10 09:43:28 +09:00
Peter Barker
6b701ae3b3 AP_NavEKF3: correct includes 2020-08-07 19:20:07 +10:00
chobits
564244ce9a AP_NavEKF3: fix extnav vel reset 2020-07-17 14:13:06 +09:00
Paul Riseborough
8ff6780323 AP_NavEKF3: Clarify distinct use cases for 'takeoff expected' 2020-07-07 15:33:58 +10:00
Paul Riseborough
d1a0c2eb30 AP_NavEKF3: Start EKF-GSF yaw estimator before fixed wing takeoff 2020-07-07 15:33:58 +10:00
Paul Riseborough
d8c2096ad8 AP_NavEKF3: improve angle accuracy during no mag launch 2020-07-07 15:33:58 +10:00
Dr.-Ing. Amilcar do Carmo Lucas
a3d140b949 AP_NavEKF3: reduce duplicated code. 2020-06-24 21:16:50 +09:00
Paul Riseborough
344d621a08 AP_NavEKF3: Add missing fallback to baro for range beacon use case 2020-06-24 21:16:50 +09:00
Paul Riseborough
906731222d AP_NavEKF3: Fix range aid switching logic
Clarifies use of EK3_RANGE_USE_HGT and EK3_ALT_SOURCE parameters to control range finder use.
Setting EK3_RNG_USE_HGT to a positive number to enable automatic switching between the primary height source (baro or GPS) and range finder should be disabled if any primary height source other than Baro or GPS is selected via the EK3_ALT_SOURCE parameter.
2020-06-24 21:16:50 +09:00
Randy Mackay
93d8458d2a AP_NavEKF3: FuseVelPosNED uses ext nav vel err in obs data check 2020-06-02 08:33:42 +09:00
Randy Mackay
d37eec5fd8 AP_NavEKF3: ext nav vel corrected for sensor position when recalled from buffer 2020-06-02 08:33:42 +09:00
chobits
c7817eaca1 AP_NavEKF3: support VISION_SPEED_ESTIMATE
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
2020-06-02 08:33:42 +09:00
Randy Mackay
6c20264d9c AP_NavEKF3: minor comment fix 2020-05-15 11:23:07 +09:00
Randy Mackay
5ad3611142 AP_NavEKF3: improve ext nav glitch handling
replaces extNavTimeout with posTimeout
replaces lastExtNavPassTime_ms with lastPosPassTime_ms
2020-05-15 11:23:07 +09:00
Randy Mackay
183ce50138 AP_NavEKF3: check GPS used before resetting pos due to GPS receiver change 2020-04-30 15:55:59 +09:00
Randy Mackay
f79d5d2d2c AP_NavEKF3: handle external nav position reset 2020-04-30 15:55:59 +09:00
Randy Mackay
d763597065 AP_NavEKF3: compensate for visual odometry sensor position 2020-04-30 15:55:59 +09:00
priseborough
c5e465aec9 AP_NavEKF3: add external nav system support
includes decoupling height source from use of external nav data
2020-04-30 15:55:59 +09:00
Andrew Tridgell
216935c1f2 AP_NavEKF3: fixed loss of GPS fusion
we must not do a storedGPS.recall unless we will be using the data,
otherwise we will lose GPS samples and risk stopping GPS fusion
2020-04-24 09:43:23 +10:00
Paul Riseborough
410b5825fb AP_NavEKF3: Enable use of EKF-GSF yaw estimate
AP_NavEKF3: Add emergency yaw reset using a Gussian Sum Filter estimate

AP_NavEKF3: Reduce default minimum GPS velocity noise for Copters

Enables fail-safe to be set with more sensitivity and improves tracking accuracy.
Origin values were set using typical GPS performance for receivers on sale 6 years ago. Receiver performance has improved since then.

AP_NavEKF3: Prevent constant mag anomaly yaw resets

Prevents constant magnetic anomaly induced resets that can be triggered when flying with vehicle generated magnetic interference.
Allows for two resets per takeoff. Allowance for two resets is required, becasue a large ground based magnetic yaw anomaly can cause a sufficiently large yaw innovation that two resets in close succession are required.

AP_NavEKF3: Add option to fly without magnetometer

AP_NavEKF3: Rework emergency yaw reset logic

Use a separate external accessor function to request the yaw reset.
Allow reset requests to remain active for a defined period of time.
Tidy up reset function to split out accuracy check.

AP_NavEKF3: Fix vulnerability to lane switch race condition

Prevents the situation where a lane switch results in a lane being selected that does not have the correct yaw. This can occur if the primary lane becomes unhealthy before the external failsafe monitor has time to react.

AP_NavEKF3: Fix EKF_MAG_CAL = 6 behaviours

Fix bug causing the yaw alignment to be performed at startup before the GSF had a valid estimate.
Fix bug causing emergency yaw message to be output for a normal reset.
Fix vulnerability to reported negative yaw variance.
Remove duplicate timer checks.

AP_NavEKF3: Update EK3_MAG_CAL documentation

AP_NavEKF3: Relax yaw gyro bias convergence check when not using mag

AP_NavEKF3: Reduce yaw drift in hover with no yaw sensor

Uses the GSF yaw estimate if available which is better than the EKF's own yaw when no yaw sensor is available.
2020-04-24 09:43:22 +10:00
Randy Mackay
7470bc9994 AP_NavEKF3: CorrectGPSForAntennaOffset made const
also reduce scope of call to slightly reduce CPU load
2020-04-21 10:21:23 +10:00
Randy Mackay
22c9081c83 AP_NavEKF3: minor comment fix 2020-04-15 19:35:30 +09:00
Daniel Magree
b3ff81648b Nav_EKF3: varInnovBodyVel assigned the innovation covariance, rather than inverse 2020-03-25 21:52:49 +11:00
Andrew Tridgell
772438f3ed AP_NavEKF3: fixed use of antenna position when switching GPS primary
when GPS primary switches we were using a position which had not been
corrected for antenna offset. This was used for calculating the reset
for sensor change.

This fixes that (trivial fix) and also fixes a similar issue on
position reset
2020-02-19 18:12:54 +11:00
Peter Barker
4431d01230 AP_NavEKF3: stop relying on the presence of a RangeFinder 2019-12-11 08:09:01 +11:00
Peter Barker
fe8599d6d7 AP_NavEKF3: adjust for renaming of RangeFinder files 2019-11-12 07:14:37 +08:00
Paul Riseborough
5b1d9ed868 AP_NavEKF3: Add missing vertical position derivative calculation 2019-10-18 11:15:25 +09:00
Randy Mackay
0e1a2efb7c EKF3: RNG_USE_HGT param only used when ALT_SOURCE = rangefinder 2019-09-10 18:44:52 +10:00
Andrew Tridgell
fc65e04ef2 AP_NavEKF3: fixed baro innovation gate when on ground with AIDING_NONE
when on the ground without a position source we would disable the
innovation gate for the barometer. This meant that a single (or small
number of) really bad baro readings would be fused into the EKF,
causing it to destabilise

Fixes #11903
2019-07-30 08:23:21 +10:00
Andrew Tridgell
634db441f8 AP_NavEKF3: origin handling fixes from Francisco 2019-07-11 06:40:55 +10:00
Andrew Tridgell
92783bccfa AP_NavEKF3: don't allow height datum reset when not on ground 2019-07-07 19:20:32 +10:00
Andrew Tridgell
3f9e48951b AP_NavEKF3: set terrainState to zero on height datum reset 2019-07-07 19:20:32 +10:00