Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
48,926 Commits 71 Branches 508 Tags 635 MiB
Commit Graph

601 Commits

Author SHA1 Message Date
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 eaa8474563 AP_NavEKF3: cope with INS_MAX_INSTANCES below 3 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 1bdf6173ec AP_NavEKF3: removed have_ekf_logging 2020-11-10 16:15:45 +11:00
Andrew Tridgell 21dfdc2192 AP_NavEKF3: use DAL for body frame odometry 2020-11-10 16:15:45 +11:00
Andrew Tridgell a0de6c620f AP_NavEKF3: use selected GPS for gps_yaw_deg() 
more than one GPS could provide yaw
 2020-11-10 16:15:45 +11:00
Andrew Tridgell f4c3d7286c AP_NavEKF3: support replay with wheel odomotry 2020-11-10 16:15:45 +11:00
Andrew Tridgell e87f98066f AP_NavEKF3: support replay with external navigation data 2020-11-10 16:15:45 +11:00
Andrew Tridgell 7bbbbd314c AP_NavEKF3: added optflow support for AP_DAL 2020-11-10 16:15:45 +11:00
Andrew Tridgell 31091d6307 AP_NavEKF3: use ekf_low_time_remaining for core scheduling 2020-11-10 16:15:45 +11:00
Andrew Tridgell f3c3778169 prevent duplicate EKF logging 
vehicle code sometimes tries to log the same data twice
 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 2681d3afe2 AP_NavEKF3: simplify alignYawAngle 2020-11-03 11:00:01 +11:00
Peter Barker fc49b55b85 AP_NavEKF3: make coreBetterScore const 2020-11-03 10:39:35 +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 7b898ee001 AP_NavEKF3: minor comment and format fixes 2020-10-28 12:17:40 +09:00
Paul Riseborough f2af2b3458 AP_NavEKF3: Initialise tiltErrorVariance class variable 2020-10-26 11:03:50 +11:00
Paul Riseborough 43386a8eea AP_NAvEKF3: Use a const reference 2020-10-26 11:03:50 +11:00
Paul Riseborough 64a3d8fe4e AP_NavEKF3: Update documentation 2020-10-26 11:03:50 +11:00
Paul Riseborough 6c7820dd8d AP_NavEKF3: Add verification of tiltErrorVariance() method in SITL 2020-10-26 11:03:50 +11:00
Paul Riseborough 90e928c32a AP_NavEKF3: Re-tune tilt alignment check 
New tilt error variance estimate is more accurate and larger than before.
 2020-10-26 11:03:50 +11:00
Paul Riseborough 16ae75a681 AP_NavEKF3: Consolidate and log tilt error variance calculation 2020-10-26 11:03:50 +11:00
Paul Riseborough 8a88dd23a2 AP_NavEKF3: Add missing generated code file for yaw fusion 2020-10-26 11:03:50 +11:00
Paul Riseborough 69632336e1 AP_NavEKF3: Remove singularity in yaw fusion at +-90deg yaw 
Uses sympy derivation
 2020-10-26 11:03:50 +11:00
Paul Riseborough 4e41e9f5f1 AP_NavEKF3: Ensure consistent use of 312 or 321 rotation order 
Prevents possibility of a differnt sequence being used to calculate the yaw and application of the yaw to the quaternions
 2020-10-26 11:03:50 +11:00
Paul Riseborough fa10d114e4 AP_NavEKF3: Fix yaw drift after yaw reset at +-90 deg pitch 
Use existing covariance prediction code to set quaternion state covariances.
Assumes tilt error is 3 deg 1-sigma.
 2020-10-26 11:03:50 +11:00
Paul Riseborough 73d5ca5ad3 AP_NavEKF3: Fix yaw drift after yaw reset at +-90 deg pitch 
Use existing covariance prediction code to set quaternion state covariances.
Assumes tilt error is 3 deg 1-sigma.
TODO derive and add function that calculates tilt error variance.
 2020-10-26 11:03:50 +11:00
Paul Riseborough a97d030e4b AP_NavEKF3: Reset earth field variances when starting mag field estimation 2020-10-26 11:03:50 +11:00
Paul Riseborough 9168d0cd19 AP_NavEKF3: Initialise mag field variances to non zero values 2020-10-26 11:03:50 +11:00
Paul Riseborough 0b96f046c2 AP_NavEKF3: Unblock initial yaw alignment message 2020-10-26 11:03:50 +11:00
Paul Riseborough f81abd73d6 AP_NavEKF3: Update covariance prediction to yse SymPy generated equations 
Saves flash and variable space.
 2020-10-26 11:03:50 +11:00
Andrew Tridgell a4716fb9bf AP_NavEKF3: fixed multi-core yaw reset 
thanks to Alex Burka for finding this
 2020-10-24 19:35:30 +11:00
Andrew Tridgell fcccdcc936 AP_NavEKF3: fixed bug in external yaw for fixed wing 
when we are on the ground we should not chang to AID_NONE if we have
an external yaw source

this fixes an EKF3 error loop on the ground found by Michael
 2020-10-24 13:50:33 +11:00
Randy Mackay c3fd82b6d7 AP_NavEKF3: minor spelling fix 2020-10-24 12:35:58 +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
Peter Barker 0496ecc4f6 AP_NavEKF2: remove unused state variables 2020-10-22 08:47:08 +11:00
Peter Barker a1e7c91f8e AP_NaveEKF3: reduce code duplication in setup_core 
Less code.

Saves 40 bytes on PixRacer
 2020-10-22 08:47:08 +11: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
Peter Barker ef505df78a AP_NavEKF3: remove unused quat2Tbn method 2020-10-20 11:40:54 +11:00
Peter Barker 0eafe4b662 AP_NavEKF3: remove dangling method declarations 2020-10-20 11:40:54 +11:00
Peter Barker 83d784c22d AP_NavEKF3: fix includes 2020-10-20 11:40:54 +11:00
Peter Barker 3dc67f09fd AP_NavEKF3: remove unused innovationIncrement and lastInnovation 2020-10-20 11:24:34 +11:00
Randy Mackay eeb1a1846c AP_NavEKF3: replace prearm_healthy with pre_arm_check 2020-10-07 11:53:07 +11:00
Randy Mackay f5a04fd1a3 AP_NavEKF3: shorten buffer size send_text message length 2020-09-29 08:57:52 +09:00
Randy Mackay 7398529f6b AP_NavEKF3: minor format fix 2020-09-28 20:57:39 +09:00
Andrew Tridgell 8a4b0f858a AP_NavEKF3: reset body mag variances at key points 
we need to reset the body mag variances if we change sensors or if we
are starting 3D fusion. When not doing 3D fusion we zero the
variances, so they must be initialised again when we restart
fusion. This fixes a bug in handling the variances on a 2nd flight
 2020-09-20 09:25:57 +10:00
Andrew Tridgell e9185f526b AP_NavEKF3: add comments on state indexes 
make it easier to check indexes
 2020-09-20 09:25:57 +10:00
Andrew Tridgell 1f21d72c79 AP_NavEKF3: fixed init of earth state mag variances 
we were starting with zero variance for X and Y earth states, it
should start with sq(magNoise)
 2020-09-20 09:25:57 +10:00
Andrew Tridgell 2bc7d17e7a AP_NavEKF3: improved core comparison to check alignment 
this adds coreBetterScore() which takes account of tilt and yaw
alignment when comparing cores for lane switching

this ensures we don't switch to a lane that is unaligned due to the
zero error score for unaligned lanes
 2020-09-14 20:29:04 +10:00
Andrew Tridgell c03c43e52c AP_NavEKF3: use same mag switch pattern as EKF2 
based on Pauls suggested change
 2020-09-08 11:01:14 +10:00
Andrew Tridgell 3836b59041 AP_NavEKF3: fixed handling of failed compass 
when a compass goes unhealthy due to sensor failure we should try
another compass after 10s if another compass is available
 2020-09-08 11:01:14 +10:00
Andrew Tridgell 458ade86f5 AP_NavEKF3: fixed segfault when IMU mask covers more IMUs than GSF mask 2020-08-28 10:17:00 +10:00
Harshit Kumar Sankhla edc3709653 AP_NavEKF3: implement sensor affinity using EK3_AFFINITY parameter 
this allows the EKF core index to be used to select a GPS/baro/mag
instance. This is an alternative to GPS blending that allows EKF lane
switching to be used to select the right combination of GPS and IMU
add logging to XKFS message
 2020-08-27 20:20:51 +10:00
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
Paul Riseborough f68f355852 AP_NavEKF3: Fix timestamp wrapping 2020-04-11 21:14:31 +10:00
Paul Riseborough 33ab1a7b15 AP_NavEKF3: Ensure yaw fusion commences when starting from rest 2020-04-11 21:14:31 +10:00
Paul Riseborough a3725e2581 AP_NavEKF3: Fix bug in calculation of rotation order 2020-04-11 21:14:31 +10:00
Paul Riseborough eeac0a05b9 AP_NavEKF3: Improvements to on ground movement check 
Reduce sensitivity and log test ratios.
Reduce base logging rate to 5Hz and log when status changes.
 2020-04-11 21:14:31 +10:00
Paul Riseborough ebb8bb4f6f AP_NavEKF3: Enable use of yaw fusion before external yaw sensor starts 2020-04-11 21:14:31 +10:00
Andrew Tridgell 3ae0b0d446 AP_NavEKF3: added using_external_yaw() method 
needed by AHRS for attitudes_consistent() check
 2020-04-11 21:14:31 +10:00
Andrew Tridgell 6f04fae4a0 AP_NavEKF3: added fallback to mag option with external yaw 
this allows for a magnetometer to be used as a fallback yaw source in
flight when using an external yaw source such as a GPS. The
magnetometer bias is learned while the GPS yaw is available and
fallback is only done if the mag yaw and GPS yaw are consistent when
fallback is enabled

This also learns the Z gyro bias until first yaw alignment when
MAG_CAL is EXTERNAL_YAW_FALLBACK. This prevents large gyro bias
building while waiting for GPS lock
 2020-04-11 21:14:31 +10:00
Andrew Tridgell a3100251a8 AP_NavEKF3: use enum class for EK3_MAG_CAL values 
and use effective_magCal() everywhere for consistency
 2020-04-11 21:14:31 +10:00
Andrew Tridgell be674fc36c AP_NavEKF3: make mag variance reporting consistent 
logged scaled variance should match the value used in MAVLink
EKF_STATUS_REPORT
 2020-04-06 17:30:29 +10:00
Peter Barker 609b66a053 AP_NavEKF3: rename struct log_NKF2a to log_XKF2 2020-03-31 10:25:59 +11:00
Daniel Magree b3ff81648b Nav_EKF3: varInnovBodyVel assigned the innovation covariance, rather than inverse 2020-03-25 21:52:49 +11:00
Peter Barker 4c19eb4bab AP_NavEKF3: use compass reference rather than repeatedly asking AHRS for it 2020-03-13 08:26:18 +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
Siddharth Purohit 545331024f AP_NavEKF3: Primary compass is always at serial number 0 2020-02-19 13:59:53 +09:00
Peter Hall 47e3fda8e9 AP_NavEKF3: add initalized flag and change to uint32_t 2020-02-19 06:45:07 +11:00
Peter Barker e289020332 AP_NavEKF3: stop taking ahrs in constructor 2020-01-21 11:53:18 +11:00
Peter Hall dd75609599 AP_NavEKF3: get_time_flying in vehicle 2020-01-14 22:34:37 -07:00
Peter Barker 03cca68c5a AP_NavEKF3: use instance number when logging rather than multiple message IDs 2019-12-17 12:22:07 +11:00
Peter Barker 9ed37e4486 AP_NavEKF3: make send_status_report const 2019-12-11 07:54:41 +09:00
Peter Barker 4431d01230 AP_NavEKF3: stop relying on the presence of a RangeFinder 2019-12-11 08:09:01 +11:00
Peter Barker b055eb8889 AP_NavEKF3: use instance # when logging EKF core timings 2019-12-06 12:52:03 +09:00
Andrew Tridgell c6aad9b1ec AP_NavEKF3: don't use WMM tables unless we have a compass scale factor set 2019-12-03 20:48:23 +11:00
Peter Barker fe8599d6d7 AP_NavEKF3: adjust for renaming of RangeFinder files 2019-11-12 07:14:37 +08:00
Peter Barker 11b372a986 AP_NavEKF3: use enum-class for RangeFinder Status 2019-11-09 08:01:53 +11:00
Randy Mackay f24afbcf4b AP_NavEKF3: initialise pre-arm failure message 2019-10-31 09:51:27 +08:00
Mark Whitehorn 3e21ef1389 AP_NavEKF3: add prearm failure message for null core pointer 2019-10-29 11:59:29 +08:00
Randy Mackay b4a8691a9e AP_NavEKF3: remove wheel encoder update limit 2019-10-22 07:15:48 +08:00
Randy Mackay 63309c6925 AP_NavEKF3: suppress ekf fail-to-set-origin if core disabled 2019-10-22 07:15:48 +08:00
Jaaaky 54f8e1137e AP_NavEKF3: added EK3_MAG_EF_LIM parameter 
Adapted from EKF2 implementation as of commits
3835d2613, e9ed3540f and df4fc0fff
this sets a limit on the difference between the earth field from the
WMM tables and the learned earth field inside the EKF. Setting it to
zero disables the feature. A positive value sets the limit in mGauss.
 2019-10-20 07:34:35 +11:00
Randy Mackay 5baed38266 AP_NavEKF3: reduce EK3_HRT_FILT max to 30 2019-10-18 11:15:25 +09:00
Paul Riseborough 5b1d9ed868 AP_NavEKF3: Add missing vertical position derivative calculation 2019-10-18 11:15:25 +09:00
Andrew Tridgell c2067d5bc2 AP_NavEKF3: set a min yaw accuracy from GPS of 5 degrees 
GPS modules tend to be rather optimistic about their yaw accuracy. By
setting a min or 5 degrees we prevent the user constantly getting
warnings about yaw innovations
 2019-10-15 11:06:11 +11:00
Andrew Tridgell 3995db49a4 AP_NavEKF3: use parent class for intermediate static variables 
this makes the code faster as well as using less memory when both EK2
and EK3 are enabled
 2019-10-01 15:39:03 +10:00
Andrew Tridgell 0e3a27f74f AP_NavEKF3: change optimisation from -O3 to -O2 2019-09-28 08:57:26 +10:00
Andrew Tridgell 96c6544997 AP_NavEKF3: moved intermediate variables to common memory 
this moves intermediate variables from being per-core to being common
between cores. This saves memory on systems with more than one core by
avoiding allocating this memory on every core.

This is an alternative to #11717 which moves memory onto the stack. It
doesn't save as much memory as #11717, but avoids creating large stack
frames
 2019-09-24 12:51:54 +10:00
Andrew Tridgell 05d62c62bb AP_NavEKF3: fixed use of uninitialised variable in EKF3 mag fusion 
not all elements of H_MAG are initialised, but they are used, as found
by valgrind
 2019-09-22 21:06:36 +10:00
Andrew Tridgell c6f43e9700 AP_NavEKF3: fixed use of old irqsave() API 2019-09-21 15:36:47 +10:00
Peter Barker 1c187d8feb AP_NavEKF3: correct return of empty prearm string 2019-09-19 08:47:47 +09:00
Andrew Tridgell fae2e44eac AP_NavEKF3: reset inhibitMagStates on in-flight yaw reset 
this is needed to prevent the next ConstrainVariances() from zeroing
the variances, which leads to very slow learning of mag states
 2019-09-11 14:22:18 +10:00
Randy Mackay 0e1a2efb7c EKF3: RNG_USE_HGT param only used when ALT_SOURCE = rangefinder 2019-09-10 18:44:52 +10:00
Michael du Breuil 9a7d64e8fc AP_NavEKF3: Allow the GPS to be responsible for logging 2019-09-10 09:09:17 +10:00
Peter Barker 7ea449076f AP_NavEKF3: use timing logging from AP_NavEKF 2019-09-03 09:28:37 +10:00
Peter Barker 61ed6e07eb AP_NavEKF3: shorten EKF3 initialisation send-text string 
Now:
pbarker@bluebottle:~/rc/ardupilot(master)$ echo -n "EKF3 IMU1 buffers IMU=11 OBS=4 OF=10, dt=0.0120" | wc
      0       7      47
pbarker@bluebottle:~/rc/ardupilot(master)$

Closes #11952
 2019-08-05 19:50:32 +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 04944fa6ce AP_NavEKF3: review fixes 
thanks Paul!
 2019-07-29 10:08:22 +10:00
Andrew Tridgell 451fe840ee AP_NavEKF3: ensure that EKF origin stays in sync on all cores 
this prevents the EKF origin on different cores from being initialised
to different values. A common value is stored in the frontend and used
by a core if it doesn't have an origin
 2019-07-29 10:08:22 +10:00
Andrew Tridgell 0fcb2037e1 AP_NavEKF3: use origin lat for earth rates 
home may not yet be set when this code is run, so using home may be
invalid
 2019-07-29 10:08:22 +10:00
Andrew Tridgell a3411efdb5 AP_NavEKF3: set origin on all cores 
when EKF origin is set, make sure it is set on all cores
 2019-07-29 10:08:22 +10:00
Andrew Tridgell 3818d8ecda AP_NavEKF3: restrict setting of EKF origin 
only allow EKF origin to be set if EKx_GPS_TYPE is set to 3, which is
used for indoor operation
 2019-07-29 10:08:22 +10:00
Siddharth Purohit db9f7c3dd5 AP_NavEKF3: do logging for 3rd EKF core 2019-07-23 12:34:38 +10:00
priseborough 209a32b8b9 AP_NavEKF3: Fix bug in use of external 321 yaw to align 2019-07-13 11:05:57 +10:00
priseborough 122f214416 AP_NavEKF3: Don't allow mag to interfere with use of external yaw sensor 2019-07-13 11:05:57 +10:00
Andrew Tridgell b56914b879 AP_NavEKF3: use GPS yaw if available and enabled 
this allows a suitable GPS to be used as an external yaw source
 2019-07-13 11:05:57 +10:00
priseborough 0c4f92d4c1 AP_NavEKF3: Add control logic to use external yaw sensor 2019-07-13 11:05:57 +10:00
priseborough 71d358803a AP_NavEKF3: Fix yaw sensor alignment status reporting 2019-07-13 11:05:57 +10:00
priseborough 977a7b68ed AP_NavEKF3: Add push to buffer for external yaw sensor data 2019-07-13 11:05:57 +10:00
priseborough 0a971c5181 AP_NavEKF3: Add function to reset yaw to external measurement 2019-07-13 11:05:57 +10:00
Andrew Tridgell 62575a194c AP_NavEKF3: Add data buffers for yaw sensor 2019-07-13 11:05:57 +10:00
priseborough 87c7649d09 AP_NavEKF3: Add interface for yaw angle measurements 2019-07-13 11:05:57 +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
Andrew Tridgell f3fc910abe AP_NavEKF3: continuously update gpsGoodToAlign 
use it to determine how to handle a height datum reset
 2019-07-07 19:20:32 +10:00
Andrew Tridgell b9a5794671 AP_NavEKF3: ensure that both AMSL and relative alt are correct after a reset 
this forces the EKF origin to the GPS alt on a height datum reset if
we have GPS lock. If we don't do this then the reported AMSL alt will
drift over time away from the GPS alt when we reset while on the
ground
 2019-07-07 19:20:32 +10:00
Andrew Tridgell 2208689893 AP_NavEKF3: don't use disabled gyros in opticalflow takeoff detection 2019-07-07 19:03:01 +10:00
Andrew Tridgell 209bca162c AP_NavEKF3: added inactive bias learning 
this allows for biases on inactive IMUs to be learned so that an IMU
switch within an EKF lane does not cause a jump in the IMU data
 2019-07-07 19:03:01 +10:00
Peter Barker ce45baa034 AP_NavEKF3: factor out and logging functions, reuse for 2nd core 2019-07-06 19:05:51 +10:00
Peter Barker f3f364de26 AP_NavEKF3: move logging in from AP_Logger 2019-07-06 19:05:51 +10:00
Andrew Tridgell 5151416080 AP_NavEKF3: force first EKF lane when disarmed 
this ensures we consistently fly with EKF lane1 if it is healthy at
the point we arm. Otherwise the choice of lane will be a lottery.

This is important as many systems have quite different filtering and
vibration characteristics on their different IMUs. We by default
enable fast sampling only on the first IMU for example, which means
the 2nd and 3rd IMUs are more vulnerable to high freq causing
aliasing.
 2019-07-04 06:48:23 +10:00
Peter Barker ef2589738d AP_NavEKF3: use beacon singleton 2019-07-01 07:20:58 +09:00
Michael du Breuil b624f6f008 AP_NavEKF3: Reduce scope of AP_Baro.h 2019-06-27 14:56:21 +10:00
Michael du Breuil be74b631fe AP_NavEKF3: Fix AP_GPS.h include 2019-06-18 10:02:05 +10:00
Andrew Tridgell 60831c2878 AP_NavEKF3: added checkLaneSwitch() 
this allows the vehicle code to ask the EKF to change lanes if it is
about to do an EKF failsafe
 2019-06-12 09:52:20 +10:00
Peter Barker dce472745e AP_NavEKF3: take EAS2TAS from AHRS rather than airspeed 2019-06-06 12:44:36 +10:00
Jaaaky f21e4d833c AP_NavEKF3: Fix GPS < 3D empty PreArm: msg-as EKF2 2019-05-20 16:57:57 +10:00
Randy Mackay 0a5d6a430a AP_NavEKF3: accept optical flow data at up to 50hz 2019-05-11 16:23:57 +09:00
Andrew Tridgell e4f6ffe719 AP_NavEKF3: fixed bug in EKF lane selection 
this fixes an issue in selecting the best lane to change to when we
have 3 or more EKF cores. The bug is that if the current lane is
unhealthy it would always choose the last healthy lane instead of
choosing the lane with the lowest score
 2019-05-07 06:59:50 +10:00
Michael du Breuil fe3b29a088 AP_NavEKF3: Allow checking all cores for health 2019-04-09 10:53:25 +10:00
Pierre Kancir b3a1c9c90c AP_NavEKF3: use get_distance_NE instead of location_diff 2019-04-08 08:00:52 -07:00
Paul Riseborough 9aaa0cd2e0 AP_NavEKF3: Fix typo in comment 2019-04-02 10:51:12 +11:00
Paul Riseborough b7e0a2345c AP_NavEKF3: Fix indexing error 
Caused by copy and paste from AP_NavEKF2 which useds index 5 for vertical velocity. EKF3  should use index 6.
 2019-04-02 10:51:12 +11:00
Paul Riseborough 0d3e1a7d23 AP_NavEKF3: flow use parameter improvements 2019-04-02 10:51:12 +11:00
Paul Riseborough d3e9281846 AP_NavEKF3: Update default plane optical flow param values 
Reduce time required to form estimate of terrain offset
 2019-04-02 10:51:12 +11:00
Paul Riseborough 35c82ef67f AP_NavEKF3: Improve optical flow height estimation 2019-04-02 10:51:12 +11:00
Paul Riseborough b848e231c7 AP_NavEKF3: Add parameter to control how optical flow data is used 2019-04-02 10:51:12 +11:00
Mark Whitehorn 3c432cf25c AP_NavEKF3: add getQuaternionBodyToNED 2019-03-29 10:58:00 +11:00
Tom Pittenger 17ee20af3f AP_NavEKF3: rename dataflash to logger 2019-03-28 16:40:57 +11:00
Dr.-Ing. Amilcar do Carmo Lucas 390e0fa601 AP_NavEKF3: replace location_offset() and get_distance() function calls with Location object member function calls 
This allows removing duplicated code
 2019-02-28 11:44:09 +11:00