Commit Graph

389 Commits

Author SHA1 Message Date
Harshit Kumar Sankhla
d7edc946b6 AP_NavEKF3: Relative Error based Lane-Switching
Improvments to the lane selection logic, we accumulate error for each
EKF lane relative to the primary for a more robust core selection
2020-08-27 20:20:51 +10:00
Randy Mackay
46a7e96c48 AP_NavEKF3: readRngBcnData avoids array overflow from beacon count 2020-08-25 09:38:10 +09:00
Randy Mackay
b314195f6e AP_NavEKF3: minor spelling fix 2020-08-25 09:38:10 +09:00
Randy Mackay
b86e16e927 AP_NavEKF3: readRngBcnData gets minor formatting fix 2020-08-25 09:38:10 +09:00
Randy Mackay
384029d278 AP_NavEKF3: rngBcnDataNew made local 2020-08-25 09:38:10 +09:00
Randy Mackay
8c877f02af AP_NavEKF3: shrink beacon arrays and add assert 2020-08-25 09:38:10 +09:00
Randy Mackay
d292ba028a AP_NavEKF3: increase beacon buffer to imu + 1 2020-08-25 09:38:10 +09:00
Randy Mackay
886a7d44a7 AP_NavEKF3: allow switch to ExtNav from optflow 2020-08-24 16:52:34 +09:00
Randy Mackay
6bf2057712 AP_NavEKF3: minor formatting fixes 2020-08-21 13:18:58 +09:00
Randy Mackay
4c40d2ac3f AP_NavEKF3: readyToUseBodyOdm uses delayed imu and wheel encoder timestamps 2020-08-21 13:18:58 +09:00
Randy Mackay
056a2de260 AP_NavEKF3: wheelOdmDataNew member made local
this variable is never used outside the writeWheelOdom method
2020-08-21 13:18:58 +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
Randy Mackay
c4b7a1c41a AP_NavEKF3: flowDataToFuse moved to local variable
This variable is never used outside the SelectFlowFusion() method
This variable is always updated at the top of the function meaning subsequent calls to the function will always overwrite its previous value
2020-08-21 13:12:10 +09:00
Randy Mackay
41acf555bf AP_NavEKF3: fix skipping of optflow fusion if mag fusion performed 2020-08-21 13:12:10 +09:00
Andrew Tridgell
44d5a923cc AP_NavEKF3: apply min GPS accuracy at measurement point
this fixes an issue a RTK GPS gives 1cm horizontal and vertical
accuracy and that causes the variances to get too small
2020-08-21 07:40:13 +10:00
chobits
c722367c6c AP_NavEKF3: move extNavVelNew to local variable 2020-08-12 15:40:38 +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
4b8be2e535 AP_NavEKF3: fix comments 2020-08-04 18:16:18 +10:00
Paul Riseborough
c8bbbd2720 AP_NavEKF3: Fix bug preventing setting of mag field states 2020-07-25 20:46:04 +10:00
Randy Mackay
d8f2d0cf8d AP_NavEKF3: max delay includes visual odometry 2020-07-21 09:31:52 +10:00
chobits
564244ce9a AP_NavEKF3: fix extnav vel reset 2020-07-17 14:13:06 +09:00
Andy Piper
e35458cedf AP_NavEKF3: INS get_sample_rate() renamed to get_loop_rate_hz() 2020-07-10 16:45:29 +10:00
Paul Riseborough
24fccd5d87 AP_NavEKF3: Don't delay GPS use unnecessarily
Fixed wing should not wait for bias state convergence after in-flight yaw alignment
2020-07-07 15:33:58 +10:00
Paul Riseborough
9a4108f55e AP_NavEKF3: Simplify setting EK3_MAG_CAL
Don't require user to separately set EK3_MAG_CAL to fly without a magnetomer
2020-07-07 15:33:58 +10:00
Paul Riseborough
0dc598a6e1 AP_NavEKF3: Fix param description and comment 2020-07-07 15:33:58 +10:00
Paul Riseborough
21ea5d5039 AP_NavEKF3: Require GSF yaw history for reset when not using a yaw sensor 2020-07-07 15:33:58 +10:00
Paul Riseborough
f86c2e1db6 AP_NavEKF3: Don't fuse EKF-GSF yaw for FW flight 2020-07-07 15:33:58 +10:00
Paul Riseborough
354b551ef0 AP_NavEKF3: Handle repeated FW flight without magnetometer
The EKF can build up large yaw errors on ground so it is safer to stop using GPS and re-align after launch as per first launch.
2020-07-07 15:33:58 +10: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
b469a80633 AP_NavEKF3: Reset all co-variances when performing a yaw reset 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
Paul Riseborough
f7c8ee807c AP_NavEKF3: Reset yaw estimator bias states at start of flight
Enables yaw bias to be learned when sitting stationary on ground.
2020-07-07 15:33:58 +10:00
Paul Riseborough
bcd23e1063 AP_NavEKF3: Fix one frame delay in processing yaw estimator velocity data 2020-07-07 15:33:58 +10:00
Peter Barker
2e55584417 AP_NavEKF3: correct comments on getFilterFaults and getFilterTimeouts methods 2020-07-07 10:50:41 +10:00
Randy Mackay
ec5f84c926 AP_NavEKF3: remove unused terrainHgtStableSet_ms 2020-06-24 21:16:50 +09:00
Randy Mackay
dccc3e22d9 AP_NavEKF3: minor comment fix re RNG_USE_HGT 2020-06-24 21:16:50 +09:00
Paul Riseborough
ed5b695c94 AP_NavEKF3: Clarify intended use of setTerrainHgtStable accessor 2020-06-24 21:16:50 +09:00
Dr.-Ing. Amilcar do Carmo Lucas
fca8d33c50 AP_NavEKF3: fix vertical flyaways when rangefinder stops providing data 2020-06-24 21:16:50 +09: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
Pierre Kancir
431468452c AP_NavEKF3: add --debug-ekf to debug the EKF 2020-06-20 19:59:05 +10:00
Randy Mackay
ca0ae57b56 AP_NavEKF3: protect against position-delta sensor data being NaN 2020-06-19 13:46:52 +09:00
Randy Mackay
331f2f5fe7 AP_NavEKF3: writeExtNavData stores pos to buffer first
This is a non-functional change
2020-06-19 13:46:52 +09:00
Randy Mackay
578800dfbb AP_NavEKF3: protect against ExtNav being NaN 2020-06-19 13:46:52 +09:00
chobits
9b448b83c0 AP_NavEKF3: use err info from ext nav interface 2020-06-09 13:31:21 +09:00
Randy Mackay
345029ec67 AP_NavEKF3: print extNav buff size 2020-06-09 09:42:14 +10:00
Randy Mackay
0f212ca727 AP_NavEKF3: accept extnav at up to 50hz 2020-06-09 09:42:14 +10:00
Peter Barker
86687d240d AP_NavEKF3: avoid compiler warnings from -Wclass-memaccess
../../libraries/AP_NavEKF2/AP_NavEKF2_core.cpp: In member function ‘void NavEKF2_core::InitialiseVariables()’:
../../libraries/AP_NavEKF2/AP_NavEKF2_core.cpp:343:50: warning: ‘void* memset(void*, int, size_t)’ clearing an object of non-trivial type ‘struct NavEKF2_core::ext_nav_vel_elements’; use assignment or value-initialization instead [-Wclass-memaccess]
  343 |     memset(&extNavVelNew, 0, sizeof(extNavVelNew));
      |                                                  ^
In file included from ../../libraries/AP_NavEKF2/AP_NavEKF2_core.cpp:4:
../../libraries/AP_NavEKF2/AP_NavEKF2_core.h:518:12: note: ‘struct NavEKF2_core::ext_nav_vel_elements’ declared here
  518 |     struct ext_nav_vel_elements {
      |            ^~~~~~~~~~~~~~~~~~~~
../../libraries/AP_NavEKF2/AP_NavEKF2_core.cpp:344:58: warning: ‘void* memset(void*, int, size_t)’ clearing an object of non-trivial type ‘struct NavEKF2_core::ext_nav_vel_elements’; use assignment or value-initialization instead [-Wclass-memaccess]
  344 |     memset(&extNavVelDelayed, 0, sizeof(extNavVelDelayed));
      |                                                          ^
In file included from ../../libraries/AP_NavEKF2/AP_NavEKF2_core.cpp:4:
../../libraries/AP_NavEKF2/AP_NavEKF2_core.h:518:12: note: ‘struct NavEKF2_core::ext_nav_vel_elements’ declared here
  518 |     struct ext_nav_vel_elements {
      |            ^~~~~~~~~~~~~~~~~~~~
2020-06-05 08:42:45 +10: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
Peter Barker
bf3cb977fd AP_NavEKF3: clarify yaw reset error message
The current wording can be taken to indicate that the mag sensors
stopped, causing this issue.
2020-05-26 10:05:35 +10:00
Paul Riseborough
939331ae8d AP_NavEKF3: Fix double iteration of axes in SelectMagFusion 2020-05-18 09:38:45 +10: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
ef02942459 AP_NavEKF3: writeBodyFrameOdom accepts delay 2020-05-15 10:07:52 +09:00
Randy Mackay
c28fd27b02 AP_NavEKF3: writeExtNavData accepts delay 2020-05-15 10:07:52 +09:00
Paul Riseborough
318811210f AP_NavEKF3: Don't perform emergency yaw reset unless commanded externally
This limits the use of the reset to situations where it is a last ditch resort before a lane switch and failsafe.
This will limit false positives for general deployment, but still provide protection from fly-aways at the cost of some increase in reaction time.
2020-05-14 10:40:48 +10:00
Randy Mackay
45e6896d95 AP_NavEKF3: ensure extnav angle error is at least 5deg 2020-05-14 08:02:13 +10:00
Randy Mackay
67903a29e2 AP_NavEKF3: fix getLLH when no GPS 2020-05-10 15:35:55 +10:00
Andrew Tridgell
5350ea5c58 AP_NavEKF3: avoid build warnings with g++ 9 2020-05-10 15:11:22 +10:00
Henry Wurzburg
8b3f07416c AP_NAVEKF3: change plane check_scaler value to match EKF2 value 2020-05-10 07:59:27 +10:00
Andrew Tridgell
cd5aae20c1 AP_NavEKF3: fixed scary message for non-compass planes 2020-05-09 12:33:33 +10:00
Andrew Tridgell
0c1ba48212 AP_NavEKF3: fixed external yaw EK3_MAG_CAL=5 or 6
don't use GSF
2020-05-04 15:05:56 +10: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
cbb25311bb AP_NavEKF3: setAidingMode uses lastExtNavPassTime_ms 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
7e010ef50a AP_NavEKF3: use set_enable() to set enable flag 2020-04-29 07:36:55 +10:00
Paul Riseborough
ad5ae1813d AP_NavEKF3: Don't use geomag data when user specifies declination 2020-04-28 11:42:31 +09:00
Randy Mackay
91a79543c9 AP_NavEKF3: getLLH fix when no GPS available 2020-04-24 20:29:18 +09:00
Peter Barker
3440a64b66 AP_NavEKF3: correct description on XKY0 2020-04-24 18:08:14 +10:00
Peter Barker
aa3165b71c AP_NavEKF3: remove incorrect units from yaw esimtator LoggerMessage 2020-04-24 18:08:14 +10:00
Andrew Tridgell
43ef8a192b AP_NavEKF3: added missing log documentation for field C 2020-04-24 09:43:23 +10:00
Paul Riseborough
1720c8de0f AP_NavEKF3: Add documentation for GSF logging 2020-04-24 09:43:23 +10:00
Paul Riseborough
9bdb89474a AP_NavEKF3: Use unique names for EKF2 logging 2020-04-24 09:43:23 +10:00
Andrew Tridgell
080f555b16 AP_NavEKF3: remove unused isDeltaYaw param from resetQuatStateYawOnly 2020-04-24 09:43:23 +10:00
Andrew Tridgell
197d31e9cc AP_NavEKF3: fixed in-flight yaw reset 2020-04-24 09:43:23 +10:00
Randy Mackay
86d221708c AP_NavEKF3: more white space fixes 2020-04-24 09:43:23 +10:00
Randy Mackay
c1ef894a30 AP_NavEKF3: getDataEKFGSF and getYawData pass by reference 2020-04-24 09:43:23 +10:00
Randy Mackay
8a8271c2c7 AP_NavEKF3: minor format fixes 2020-04-24 09:43:23 +10:00
Randy Mackay
ed3d072318 AP_NavEKF3: rename GSF_RUN_MASK, GSF_USE_MASK, GSF_RST_MAX 2020-04-24 09:43:23 +10:00
Paul Riseborough
7ba39c844c AP_NavEKF3: Split GSF yaw estimator processing
Separate prediction and correction steps are required to provide an up to
2020-04-24 09:43:23 +10: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
9f5e037015 AP_NavEKF3: Restore bug fix lost during rebase 2020-04-24 09:43:23 +10:00
Paul Riseborough
53df8a0238 AP_NavEKF3: Fix timer wrapping bug 2020-04-24 09:43:23 +10:00
Paul Riseborough
12d639fc17 AP_NavEKF3: Misc changes arising from review
AP_NavEKF3: Fix typo in comment

AP_NavEKF3: Use uint32_t data type for msec timers

AP_NavEKF3: Misc msec timer fixes

Fix wrapping bug.
Use common timestamp.

AP_NavEKF3: Format fixes - remove tabs

AP_NavEKF3: Fix typing and wrap issue on time comparison
2020-04-24 09:43:23 +10:00
Paul Riseborough
4b3952bd15 AP_NavEKF3: Eliminate unnecessary function call 2020-04-24 09:43:23 +10:00
Paul Riseborough
6ec69e6ed1 AP_NavEKF3: Update EK3_MAG_CAL parameter advice 2020-04-24 09:43:23 +10:00
Paul Riseborough
863f989130 AP_NavEKF3: Fix integration of GPS yaw options
AP_NavEKF3: fix failure to arm with EK3_MAG_CAL = 7

AP_NavEKF3: Fix failure to arm when not using magnetometer

AP_NavEKF3: Reduce yaw drift when EK3_MAG_CAL = 7
2020-04-24 09:43:22 +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
Peter Barker
87108b86c8 AP_NavEKF3: add documentation for XKFM message 2020-04-21 09:48:43 +10:00
Randy Mackay
22c9081c83 AP_NavEKF3: minor comment fix 2020-04-15 19:35:30 +09:00
Peter Barker
d19f8db06f AP_NavEKF3: rename APMrover2 to Rover 2020-04-14 09:50:34 +09:00
Andrew Tridgell
1f8cd830ea AP_NavEKF3: fixed use of tabs 2020-04-11 21:14:31 +10:00