Commit Graph

182 Commits

Author SHA1 Message Date
murata
c143fb0a5a AP_NavEKF2: Console output can be disabled 2022-05-17 09:53:06 +10:00
Josh Henderson
9a274ba565 AP_NavEKF2: initialize stateStruct.quat to unit length 2021-09-28 19:09:16 +10:00
Josh Henderson
d5f5a3024a AP_NavEKF2: use vector.xy().length() instead of norm(x,y) 2021-09-14 10:43:46 +10:00
Randy Mackay
b524094216 AP_NavEKF2: init rngOnGnd to 5cm to avoid div-by-zero 2021-08-26 12:23:32 +09:00
Andrew Tridgell
6aca0bb08a AP_NavEKF2: allow for double EKF build 2021-07-10 07:20:41 +10:00
Siddharth Purohit
124eaf1a38 AP_NavEKF2: use first usable compass index to set magSelectIndex 2021-06-02 17:10:19 +10:00
Andrew Tridgell
32b079911f AP_NavEKF2: use DAL APIs for takeoff/touchdown expected 2021-06-02 11:13:16 +09:00
Andrew Tridgell
1e75bb971e AP_NavEKF2: constrain vertical error
this prevents a floating point exception with external AHRS
2021-01-05 21:13:12 +11:00
Paul Riseborough
b372d62f35 AP_NavEKF2: Add accessor function for body frame airspeed vector 2020-12-11 15:21:41 +11:00
Peter Barker
fc7e7dcee4 AP_NavEKF2: remove unused variables 2020-12-01 10:40:09 +11:00
Peter Barker
addd57dcab AP_NavEKF2: move badMagYaw onto stack 2020-12-01 10:40:09 +11:00
Peter Barker
400ba7a9df AP_NavEKF2: move global state to be on the stack 2020-12-01 10:40:09 +11:00
Andrew Tridgell
7313e9eb0f AP_NavEKF2: removed inhibitGps and inhibitGpsVertVel options
these were unused
2020-12-01 10:33:50 +11:00
Andrew Tridgell
5f0e943f0f AP_NavEKF2: use dal reference in EKF backends
saves a bit of flash space
2020-11-10 16:15:45 +11:00
Andrew Tridgell
6be3d19e82 AP_NavEKF2: 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
Peter Barker
0496ecc4f6 AP_NavEKF2: remove unused state variables 2020-10-22 08:47:08 +11:00
Peter Barker
53305ba2a4 AP_NavEKF2: remove unused quat2Tbn method 2020-10-20 11:40:54 +11:00
Peter Barker
80f9ef7ed1 AP_NavEKF2: remove unused innovationIncrement and lastInnovation 2020-10-20 11:24:34 +11:00
Andrew Tridgell
35e95d3328 AP_NavEKF2: reset body mag variances at key points
reset on sensor switch or when changing to 3D fusion
2020-09-20 09:25:57 +10:00
Peter Barker
032b7c81e5 AP_NavEKF2: correct includes 2020-08-07 19:20:07 +10:00
Andy Piper
e39e24bd18 AP_NavEKF2: INS get_sample_rate() renamed to get_loop_rate_hz() 2020-07-10 16:45:29 +10:00
Randy Mackay
a8c2be26ba AP_NavEKF2: remove unused terrainHgtStableSet_ms 2020-06-24 21:16:50 +09:00
Randy Mackay
6c10655059 AP_NavEKF2: accept extnav at up to 50hz 2020-06-09 09:42:14 +10:00
Peter Barker
b02d9abd32 AP_NavEKF2: 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
dee095b4a4 AP_NavEKF2: support VISION_SPEED_ESTIMATE 2020-06-01 17:51:24 +09:00
Paul Riseborough
abc821ad36 AP_NavEKF2: Split GSF yaw estimator processing
Separate prediction and correction steps are required to provide an up to date yaw estimate using IMU prediction before it may be required by SelectMagFusion() whilst still doing the velocity update after GPS data haw been pulled from the buffer by SelectVelPosFusion()
2020-04-24 09:43:23 +10:00
Andrew Tridgell
c83e2d7c0e AP_NavEKF2: 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
de0040ad69 AP_NavEKF2: Enable use of EKF-GSF yaw estimate
Enables the yaw to be reset in flight to a value estimated from a specialised yaw estimator. This allows faster recovery if taking off with a bad magnetometer and also allows yaw alignment and GPS use to commence in-air when operating without any yaw sensing.

AP_NavEKF2: Add missing accesor functions for default airspeed
2020-04-24 09:43:23 +10:00
Siddharth Purohit
4cca1c307c AP_NavEKF2: Primary compass is always at serial number 0 2020-02-19 13:59:53 +09:00
Randy Mackay
cc5facf6d2 AP_NavEKF2: initialise pre-arm failure message 2019-10-31 09:51:27 +08:00
Randy Mackay
e2d3afaaa7 AP_NavEKF2: reduce EK2_HRT_FILT max to 30 2019-10-18 11:15:25 +09:00
Paul Riseborough
c26c6fd078 AP_NavEKF2: Add missing vertical position derivative calculation
Use a third order order complementary filter to estimate the rate of change of vertical position output.
2019-10-18 11:15:25 +09:00
Andrew Tridgell
f7e138bddd AP_NavEKF2: 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
8dd2119b14 AP_NavEKF2: make it easy to do EK2 timing tests
for future benchmarking
2019-09-28 08:57:26 +10:00
Andrew Tridgell
255981c60c AP_NavEKF2: moved some of the intermediate vars to the stack
this keeps stack frames below 1k, while giving faster access to the
variables and saving more memory
2019-09-24 12:51:54 +10:00
Andrew Tridgell
0041874826 AP_NavEKF2: 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
1d47f9186e AP_NavEKF2: fixed use of old irqsave() API 2019-09-21 15:36:47 +10:00
Andrew Tridgell
9372073f63 AP_NavEKF2: continuously update gpsGoodToAlign
use it to determine how to handle a height datum reset
2019-07-07 19:20:32 +10:00
Andrew Tridgell
1c4c5a6850 AP_NavEKF2: learn gyro biases for inactive gyros
this allows us to learn the gyro biases each lane would need if it had
to switch to another gyro due to a sensor failure. This prevents a
sudden change in gyro bias on IMU failure
2019-07-07 19:03:01 +10:00
Michael du Breuil
892b687c09 AP_NavEKF2: Fix AP_GPS.h include 2019-06-18 10:02:05 +10:00
Andrew Tridgell
e9ed3540f1 AP_NavEKF2: constrain field by table after fusion
this should make for faster convergence
2019-06-03 12:21:29 +10:00
Andrew Tridgell
3835d2613e AP_NavEKF2: added EK2_MAG_EF_LIM parameter
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-06-03 12:21:29 +10:00
Randy Mackay
e19d638c0f AP_NavEKF2: support higher optical flow updates rates 2019-05-11 16:23:57 +09:00
Paul Riseborough
aa6eee82f4 AP_NavEKF2: Rework opt flow terrain height logic 2019-04-02 10:51:12 +11:00
Arjun Vinod
a0e7c37f8c AP_NavEKF2: fixed typos 2019-02-22 10:21:35 +11:00
Tom Pittenger
b32e8a4424 AP_NavEKF2: remove HAL_CPU_CLASS_150 check, 150 is already a minimum requirement 2019-02-17 12:59:52 -08:00
Peter Barker
ebd12b30e8 AP_NavEKF2: adjust for Location_Class and Location unification 2019-01-16 11:45:29 +11:00
Pierre Kancir
49d20364cb AP_NavEKF2: pass quaternion by const reference 2018-12-20 09:05:01 +09:00
Andrew Tridgell
8ea7df3efe AP_NavEKF2: added filter reset if unhealthy for 5s and disarmed 2018-11-30 11:02:30 +11:00
Michael du Breuil
29db069c75 AP_NavEKF2: Suppress clearing non-trival type warning 2018-10-30 16:17:03 -07:00