Commit Graph

291 Commits

Author SHA1 Message Date
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