Commit Graph

80 Commits

Author SHA1 Message Date
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
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
8ff6780323 AP_NavEKF3: Clarify distinct use cases for 'takeoff expected' 2020-07-07 15:33:58 +10:00
Randy Mackay
ec5f84c926 AP_NavEKF3: remove unused terrainHgtStableSet_ms 2020-06-24 21:16:50 +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
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
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
Andrew Tridgell
5350ea5c58 AP_NavEKF3: avoid build warnings with g++ 9 2020-05-10 15:11:22 +10: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
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
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
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
Paul Riseborough
33ab1a7b15 AP_NavEKF3: Ensure yaw fusion commences when starting from rest 2020-04-11 21:14:31 +10:00
Siddharth Purohit
545331024f AP_NavEKF3: Primary compass is always at serial number 0 2020-02-19 13:59:53 +09:00
Randy Mackay
f24afbcf4b AP_NavEKF3: initialise pre-arm failure message 2019-10-31 09:51:27 +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
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
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
c6f43e9700 AP_NavEKF3: fixed use of old irqsave() API 2019-09-21 15:36:47 +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
priseborough
0c4f92d4c1 AP_NavEKF3: Add control logic to use external yaw sensor 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
Andrew Tridgell
62575a194c AP_NavEKF3: Add data buffers for yaw sensor 2019-07-13 11:05:57 +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
Michael du Breuil
be74b631fe AP_NavEKF3: Fix AP_GPS.h include 2019-06-18 10:02:05 +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
Paul Riseborough
35c82ef67f AP_NavEKF3: Improve optical flow height estimation 2019-04-02 10:51:12 +11:00
Arjun Vinod
f382a657bd AP_NavEKF3: fixed typos 2019-02-26 08:33:39 +11:00
Tom Pittenger
55377b234f AP_NavEKF3: remove HAL_CPU_CLASS_150 check, 150 is already a minimum requirement 2019-02-17 13:00:00 -08:00
Peter Barker
0d4475f443 AP_NavEKF3: adjust for Location_Class and Location unification 2019-01-16 11:45:29 +11:00
Pierre Kancir
340429fbbb AP_NavEKF3: pass by const reference when needed 2018-12-20 09:05:01 +09:00
Michael du Breuil
25ef5fed82 AP_NavEKF3: Suppress clearing non-trival type warning 2018-10-30 16:17:03 -07:00
Andrew Tridgell
de58fb4637 AP_NavEKF3: support in-flight compass learning 2018-10-24 07:25:42 +10:00
Peter Barker
560f17a570 AP_NavEKF3: use union to alias array and struct access to states
This avoids creating two pointers of different types to the same memory.

Having two pointers to the same memory can lead to the compiler
optimising code such that a write to one pointer is rearranged to be
either before or after a read from the other pointer depending on
which is deemed faster - not a good outcome.
2018-10-19 08:46:43 +11:00
Peter Barker
27e3a89158 AP_NavEKF3: correct compilation when running MATH_CHECK_INDEXES 2018-10-19 08:46:43 +11:00
Andrew Tridgell
553abda91c AP_NavEKF3: use single precision ceilf() 2018-05-07 11:43:23 +10:00
Pierre Kancir
44a21bc8ac EKF3: correct acos to acosf 2018-04-03 09:54:45 +09:00
Peter Barker
55b8a2288e AP_NavEKF3: use ins singleton 2018-03-16 00:37:35 -07:00
Peter Barker
5dc714bf5f AP_NavEKF3: use GPS singleton 2017-12-27 00:58:02 +00:00
Andrew Tridgell
6b30c9213a AP_NavEKF3: delay startup until IMU buffer is filled
this prevents a vulnerability where the initial IMU data is processed
26 times (where 26 is the IMU buffer length)
2017-12-12 11:39:38 +11:00
priseborough
c7f6d52065 AP_NavEKF3: Add wheel encoder odometry
Uses the existing body frame odometry observation model.
Handles each sensored wheel as a separate sensor.
2017-07-27 16:52:27 +09:00
Peter Barker
93e09c51d3 AP_NavEKF3: eliminate GCS_MAVLINK::send_statustext_all 2017-07-11 23:53:53 +01:00