Commit Graph

224 Commits

Author SHA1 Message Date
Jonathan Challinger 6938e3d57b AP_NavEKF: check mag instance id when returning mag offsets 2016-04-21 09:51:41 +10:00
Lucas De Marchi bb0d96cedd AP_NavEKF: replace header guard with pragma once 2016-03-16 18:40:42 +11:00
Peter Barker 11760c33f6 AP_NavEKF: avoid use of undefined #defines
Ensure EKF_DISABLE_INTERRUPTS is defined

Do not define MATH_CHECK_INDEXES, assume it is defined
2016-02-19 12:34:24 -02:00
Paul Riseborough 624f169b9b AP_NavEKF: Publish enabled status 2015-11-08 15:37:27 +11:00
Andrew Tridgell 56909ce973 AP_NavEKF: split EKF1 into frontend and backend
this allows the primary EKF to be allocated dynamically, allowing for
experiments with EKF2 with multi-IMU support
2015-11-06 10:42:57 +11:00
Paul Riseborough 0dd5a7c4fa AP_NavEKF: Use correct blended delta time for all vel and pos integration
dtIMUactual has been spit into a separate dtDelAng and dtDelVel and dtDelVel1 and dtDelVel2 delta time in recognition of the amount of timing jitter and different update rates for the IMU's
2015-11-02 17:47:00 +09:00
Paul Riseborough 42214ec303 AP_NavEKF: Fix failure to start mag cal due to gyro noise
Vibration in the 400Hz delta angles could cause the angular rate condition check for in-flight magnetic field alignment to fail.
The symptons were failure to start magnetic field learning as expected when EK2_MAG_CAL=3 was set.
The calculation of a delta rotation between consecutive magnetometer samples has been introduced instead of the most recent IMU delta angle as this is less affected by noise and give an upper bound on the angular error.
the check has been moved into the magnetometer fusion control function so that any reset will be performed using fresh magnetometer data
2015-11-02 17:46:59 +09:00
Paul Riseborough 778b1c3e18 AP_NavEKF: Remove dead code for constant velocity mode 2015-11-02 17:46:54 +09:00
Paul Riseborough 59df23bc0f AP_NavEKF: Reduce EKF health pre-arm check false positives 2015-11-02 17:46:52 +09:00
Randy Mackay 3f29365cae AP_NavEKF: constify getLastPosNorthEastReset 2015-10-30 12:24:15 +09:00
Randy Mackay 758c5a7d7f AP_NavEKF: constify getLastYawResetAngle 2015-10-30 12:24:11 +09:00
Paul Riseborough 7c40448bab AP_NavEKF: Remove GPS glitch offset logic
The correction for offsets due to position and velocity resets will now be handled in the control loops.
2015-10-30 12:24:01 +09:00
Paul Riseborough 5489ff9118 AP_NavEKF: publish position and velocity state reset data
Used by the control loops to compensate for steps in velocity and/or position resulting from state resets.
2015-10-30 12:24:00 +09:00
Andrew Tridgell f66966ccae NavEKF: use new perf counter API 2015-10-20 18:16:15 +11:00
Paul Riseborough 240ea92947 AP_NavEKF: Re-center gyro bias limits on arming
Allows the gyro bias to vary by up to +-10 deg/second before and after arming.
2015-10-20 15:21:39 +11:00
Paul Riseborough 6b3e114cd6 AP_NavEKF: Enable EKF1 to be disabled to reduce frame over-runs 2015-10-20 15:21:39 +11:00
Paul Riseborough 840f307d58 AP_NavEKF: Make EKF2 PosDownDerivative interface follow coding conventions
Updates arising from peer review.
2015-10-20 15:21:37 +11:00
Paul Riseborough dab658f6ed AP_NavEKF: Add methods to output and calculate vertical position derivative 2015-10-20 15:21:36 +11:00
Paul Riseborough 3479a0e209 AP_NavEKF: Prevent blocking of synthetic position fusion
Fixes a potential error where changes to timing and arrival rate of magnetometer and baro data could block the fusion of synthetic position and velocity measurements, allowing unrestrained tilt errors during operation without GPS or optical flow.
Fusion of synthetic position or velocity measurements is now timed to coincide with fusion of barometer observations.
If a new barometer observation has not arrived after 200 msec then the synthetic position or velocity is fused anyway so that fusion of synthetic position or velocity observations cannot occur any slower than 5 Hz
2015-10-13 14:05:36 +09:00
Paul Riseborough f451a81ef9 AP_NavEKF: Add missing GPs check report 2015-10-10 21:22:49 +09:00
Paul Riseborough ef5049862a AP_NavEKF: Add fix status to GPS check report message 2015-10-10 14:49:05 +09:00
Paul Riseborough 5177746c00 AP_NavEKF: Add reporting of GPS check status 2015-10-10 14:49:01 +09:00
Paul Riseborough 2b0434f089 AP_NavEKF: Improved detection of GPS glitch behaviour 2015-10-08 15:09:45 +09:00
Paul Riseborough 8a9d9c04c5 AP_NavEKF: Add public function to declare GPS glitch 2015-10-08 15:09:40 +09:00
Paul Riseborough 21e4910149 AP_NavEKF: Allow user to select preflight GPS checks 2015-10-08 15:09:37 +09:00
Paul Riseborough cdae84aec1 AP_NavEKF: Additional pre-arm GPS quality checks 2015-10-08 15:09:34 +09:00
Randy Mackay b5c49e0792 AP_NavEKF: minor comment fix
No functional change
2015-09-24 16:57:36 +09:00
Jonathan Challinger 51fb13a329 AP_NavEKF: fix getLastYawResetAngle to return yaw reset system time 2015-09-24 16:57:33 +09:00
Paul Riseborough 02408861a1 AP_NavEKF: Remove unused function 2015-09-24 12:58:43 +10:00
Paul Riseborough 5c32bb8858 AP_NavEKF: Add pre-flight check for gyro quality 2015-09-21 17:06:31 +09:00
Paul Riseborough 930f730612 AP_NavEKF: Reset mag and heading states to try and pass pre-flight checks 2015-09-16 15:13:36 +09:00
Andrew Tridgell c10ce3ffbd AP_NavEKF: added prearm_failure_reason() 2015-09-08 16:07:33 +10:00
Randy Mackay 629a5fd714 NavEKF: IMUSwitchState enum 2015-08-19 16:44:31 +09:00
Paul Riseborough 559cc29635 AP_NavEKF: Add hysteresis to IMU switching logic 2015-08-19 16:44:26 +09:00
Paul Riseborough 12e884ba6b AP_NavEKF: Modify method used to check for vibration errors
This method checks for consistency between accelerometer readings and switches to the unit with the lowest vibration of the difference exceeds 0.3g
The threshold of 1.7 m/s/s corresponds to a maximum tilt error of 10 deg assuming one IMU is good, one is bad and the EKF is using the bad IMU.
2015-08-19 16:44:24 +09:00
Gustavo Jose de Sousa f7fd9e3d99 AP_NavEKF: standardize inclusion of libaries headers
This commit changes the way libraries headers are included in source files:

 - If the header is in the same directory the source belongs to, so the
 notation '#include ""' is used with the path relative to the directory
 containing the source.

 - If the header is outside the directory containing the source, then we use
 the notation '#include <>' with the path relative to libraries folder.

Some of the advantages of such approach:

 - Only one search path for libraries headers.

 - OSs like Windows may have a better lookup time.
2015-08-11 16:38:20 +10:00
Paul Riseborough 8b4b0b9576 AP_NavEKF: Add accelerometer clip monitoring and isolation
Isolates an accelerometer that is clipping  and attempts to use the one that is clipping less.
2015-07-12 16:49:19 +09:00
Andrew Tridgell 686b1137fa AP_NavEKF: added vehicle specific initial gyro bias uncertainty 2015-06-30 16:16:26 +10:00
Paul Riseborough 87e6a24154 AP_NavEKF: Add public method to reset EKF height datum and baro alt 2015-06-24 16:25:11 +09:00
Paul Riseborough e3ccb74e12 AP_NavEKF: Publish amount of in-flight yaw angle reset 2015-06-24 11:49:29 +09:00
Paul Riseborough 6ba8cd05ff AP_NavEKF: Fix bug preventing GPS vertical velocity being used
This fixes a bug that prevents GPS velocity being used permanently if there is a temporary unavailability
2015-06-18 09:01:21 +10:00
Paul Riseborough c1c5e3598a AP_NavEKF: Enforce alignment of realigned earth mag field with declination
This prevents bad inertial or GPS data combined with the post takeoff heading alignment check used by plane from resulting in earth field states that have an incorrect declination
2015-05-19 20:35:52 +10:00
Paul Riseborough a232606fc9 AP_NavEKF: Prevent EKF yaw errors due to fast yaw spins
Prolonged yaw rotations with gyro scale factor errors can cause yaw errors and gyro bias estimation errors to build up to a point where EKF health checks fail.
This patch introduces the following protections:

1) The assumed yaw gyro error is scaled using a filtered yaw rate and an assumed 3% scale factor error (MPU6000 data sheet)
2) When the filtered yaw rate magnitude is greater than 1 rad/sec, the Z gyro bias process noise is zeroed and the state variance set to zero to inhibit modification of the bias state
3) When the filtered yaw rate magnitude is greater than 1 rad/sec, the magnetometer quaternion corrections are scaled by a factor of four to maintain tighter alignment with the compass
2015-05-08 16:47:31 +09:00
Paul Riseborough be04be9b43 AP_NavEKF: Fix bug preventing home position being set by copter
The interface definition has been modified so that it returns true for a position obtained usin geither the normal inertial navigation calculation, or a raw GPS measurement.
This enables this function to be used to set a home position before flight.
2015-05-02 17:20:21 +09:00
Randy Mackay c366fbbc96 AP_NavEKF: remove unused haveDeltaAngles 2015-05-01 16:37:33 +09:00
Paul Riseborough b71b8f4bda AP_NavEKF: Always return a NED relative position if possible
If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.
2015-05-01 16:37:32 +09:00
Paul Riseborough 20ba1e6b1b AP_NavEKF: Always return a WGS-84 location if possible
If a calculated location is not available, the function will return raw GPS data if available, but the status will be set to false.
2015-05-01 16:37:31 +09:00
Paul Riseborough 2c4572eb50 AP_NavEKF: Make ground effect variable names more descriptive 2015-05-01 16:37:29 +09:00
Paul Riseborough 5dc29699ab AP_NavEKF: Publish the INS delta quaternion 2015-05-01 16:37:25 +09:00
Jonathan Challinger 2f38dd1b67 AP_NavEKF: use quaternion functions to apply IMU delta angles 2015-05-01 16:37:22 +09:00