Commit Graph

656 Commits

Author SHA1 Message Date
Paul Riseborough 778b1c3e18 AP_NavEKF: Remove dead code for constant velocity mode 2015-11-02 17:46:54 +09:00
Paul Riseborough fb8e01a73c AP_NavEKF: Do not automatically accept DCM alignment data after 30 seconds
A looser check is applied at 30 seconds rather than no check.
2015-11-02 17:46:53 +09:00
Paul Riseborough be11e12f81 AP_NavEKF: Improve height reset during climbs and descents
Resets the vertical velocity to the GPS when the height is reset is the GPS is healthy
2015-11-02 17:46:53 +09:00
Paul Riseborough 59df23bc0f AP_NavEKF: Reduce EKF health pre-arm check false positives 2015-11-02 17:46:52 +09:00
Paul Riseborough a1e32d71ec AP_NavEKF: Prevent large rotation rates corrupting the in-flight mag reset 2015-10-30 15:34:14 +09:00
Lucas De Marchi 831d8acca5 Remove use of PROGMEM
Now variables don't have to be declared with PROGMEM anymore, so remove
them. This was automated with:

    git grep -l -z PROGMEM | xargs -0 sed -i 's/ PROGMEM / /g'
    git grep -l -z PROGMEM | xargs -0 sed -i 's/PROGMEM//g'

The 2 commands were done so we don't leave behind spurious spaces.

AVR-specific places were not changed.
2015-10-30 14:35:16 +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
Paul Riseborough 44f42fe72d AP_NavEK: Reduce settling time after start-up disturbance
Now that we are using  a consistent 50Hz minimum update rate for the covariance prediction we do not need a different initial gyro bias uncertainty for plane and copter to maintain filter stability margins.
The default value of 0.1 rad/s was too high and gave excessive settling time of the filter attitude after startup.
The initial attitude uncertainty has been increased to allow for some movement during startup.
2015-10-22 16:23:14 +11:00
Paul Riseborough 6c017ffcb5 AP_NavEKF: Predict covariance more often to prevent instability on startup
If the baro data and magnetometer data are interleaved (arriving every 100 msec and offset by 50 msec), then the filter will go unstable during startup and fail to complete checks.
2015-10-22 09:57:38 +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 8515dda727 AP_NavEKF: Fix bug in vertical position derivative calculation 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
Andrew Tridgell f5b5a3b0f8 AP_NavEKF: added EKF_DISABLE_INTERRUPTS
this helps with performance tuning
2015-10-20 14:37:41 +11:00
Andrew Tridgell 10fddf4c7e AP_NavEKF: enable optimisation level 3 for the EKF 2015-10-20 14:36:56 +11:00
Andrew Tridgell 6f36267a66 AP_NavEKF: fixed EKF error message during 10 second warmup 2015-10-16 14:58:46 +11:00
Gustavo Jose de Sousa afccf615d5 AP_NavEKF: use compass get_{field,offsets}() functions
Both functions are equivalent, so we're going to simply use
get_{field,offsets}() instead of get_{field,offsets}_milligauss().
2015-10-15 19:56:03 +09: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 a895b16fa6 AP_NavEKF: Prevent GPS glitch activating EKF failsafe 2015-10-12 16:29:21 +09:00
Paul Riseborough f451a81ef9 AP_NavEKF: Add missing GPs check report 2015-10-10 21:22:49 +09:00
Paul Riseborough a3a1dabb94 AP_NavEKF: Update GPS check description and default setting
Previous check default only checked the number of satellites and horizontal position accuracy.
Updated default value also checks HDoP and speed accuracy.
2015-10-10 21:21:25 +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 d0080b66cd AP_NavEKF: Define a structure for reporting of GPS checks 2015-10-10 14:48:56 +09:00
Paul Riseborough 2b0434f089 AP_NavEKF: Improved detection of GPS glitch behaviour 2015-10-08 15:09:45 +09:00
Paul Riseborough d48d4ac950 AP_NavEKF: Add GPS glitching monitor to EKF status report 2015-10-08 15:09:43 +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
Paul Riseborough 8a66c9c2ed AP_NavEKF: Critical Bugfix 2015-10-08 10:54: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
Tom Pittenger e2fde36ec6 AP_NavEKF: index out of range due to incorrect assignment 2015-09-22 11:41:51 +09:00
Jonathan Challinger d2b103b323 AP_SmallEKF: replace incorrect quaternion rotations with library call 2015-09-22 11:15:05 +10:00
Paul Riseborough 5c32bb8858 AP_NavEKF: Add pre-flight check for gyro quality 2015-09-21 17:06:31 +09:00
Paul Riseborough 8cfde42e15 AP_NavEKF: Ensure bad mag data cannot cause the heading to reset too often 2015-09-16 15:13:37 +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
Staroselskii Georgii b5b6d767bd AP_NavEKF: make EKF use milligauss 2015-09-09 10:38:16 +10:00
Andrew Tridgell 0ca534bfab AP_NavEKF: only call calcGpsGoodToAlign if we need to
avoid calling it once we have an origin. This avoids some calculations
and string operations
2015-09-08 16:07:33 +10:00
Andrew Tridgell c10ce3ffbd AP_NavEKF: added prearm_failure_reason() 2015-09-08 16:07:33 +10:00
Paul Riseborough 21f873662c AP_NavEKF: Prevent false triggering of optical flow takeoff detection
Now that we have a pre-arm check in place to detect bad lidar, the motion check is unnecessary and can false trigger for copters with flexible undercarriages or on uneven ground.
2015-08-27 20:57:38 +09:00
Randy Mackay ec2bc5e41d NavEKF: use maxf when retrieving vibration levels
This reduces the performance hit that was caused by multiple calls to get_vibration_levels
2015-08-19 16:44:36 +09:00
Randy Mackay 629a5fd714 NavEKF: IMUSwitchState enum 2015-08-19 16:44:31 +09:00
Randy Mackay c179ed5253 NavEKF: IMU ratio set to primary accel when neither IMU is used
This forces the EKF IMU ratio to indicate which IMU is used except that it will be "0" in the unlikely case that the third IMU is used
2015-08-19 16:44:28 +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
Randy Mackay 0b981d38e8 NavEKF: incorporate use_gyro and use_accel 2015-08-19 16:44:17 +09:00
Randy Mackay 8b17d32994 NavEKF: check baro health before consuming 2015-08-12 16:32:48 -07: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
Randy Mackay 0acfcbfa81 NavEKF: use primary compass instead of first instance
Thanks very much to staroselskii for finding this bug
2015-07-17 10:02:42 +09:00
Paul Riseborough 610595bfb9 AP_NavEKF: Prevent position drift between arming and takeoff when using OF 2015-07-13 10:54:36 +09: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
Paul Riseborough 4fb7beba4b AP_NAvEKF: Prevent failure due to magnetometer fusion numerical errors
If a badly conditioned covariance matrix causes negative innovation variances, then the filter will diverge. The previous approach of increasing process noise was not effective in some cases, so a hard reset of the covariance matrix has been adopted to guarantee recovery.
This fixes a numerical error observed using the replay on flight log which had significant periods of compass rejection.
2015-07-02 18:07:22 +10:00
Andrew Tridgell 686b1137fa AP_NavEKF: added vehicle specific initial gyro bias uncertainty 2015-06-30 16:16:26 +10:00
Randy Mackay 9f17fc17ab NavEKF: update parameter descriptions 2015-06-24 16:25:14 +09: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
Andrew Tridgell c9988d8b37 AP_NavEKF: rely on delta_velocity and delta_angles always being available 2015-06-17 13:10:26 +10:00
Andrew Tridgell a115182041 AP_NavEKF: fixed minimum deltat to be 100us 2015-06-17 13:10:26 +10:00
Andrew Tridgell c221959d5a AP_NavEKF: fixed corrected del velocity to use both accels 2015-06-17 13:10:25 +10:00
Paul Riseborough d3f83e8ec6 AP_NavEKF: Improve gyro bias learning rate for plane and rover
This patch increases initial gyro bias uncertainty and plane and rover specific process noise to improve the rate of gyro bias learning.
This reduces the likelihood of a navigation failure due to rapid temperature changes in the inertial sensors causing rapid changes in zero rate offset.

The increase in process noise cannot be applied to Copter due to different numerical stability limits arising from the faster update rate.
2015-06-03 11:46:17 +10:00
Andrew Tridgell d5da9fcca6 AP_NavEKF: fixed build warnings 2015-05-30 22:51:30 +10:00
Andrew Tridgell efd958f0b0 AP_NavEKF: rename file to remove spaces
spaces in file names break shell scripts
2015-05-23 17:14:52 +10:00
Andrew Tridgell 411a96b4b3 AP_NavEKF: take at most 30s to accept DCM attitude
velocity noise on the GPS can cause DCM to take a long time to
converge. If it takes longer than 30s then allow EKF to start
2015-05-20 15:22:15 +10:00
Andrew Tridgell 92c4c5cbcf AP_NavEKF: change using_gps threshold to 4s
this means plane will continue to use dead reckoning with GPS 3D lock
for 4s
2015-05-19 20:52:39 +10:00
Paul Riseborough 69ca654194 AP_NavEKF: Update declination when we know our location
This ensures that when we start using GPS, that the EKF will be using the correct declination for that location
If declination is not known it defaults to zero
2015-05-19 20:35:52 +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 b1d8805114 AP_NavEKF: Reduce takeoff detection sensitivity
This patch reworks the in-air transition criteria to reduce the likelihood of false positives and to ensure that there will be enough ground speed to make the heading check work reliably.
2015-05-19 20:35:52 +10:00
Andrew Tridgell 6154db5cf8 AP_NavEKF: added using_gps status bit 2015-05-17 21:24:57 +10:00
Tom Pittenger e987173ffb AP_NavEKF: float to double promotion via tan instead of tanf 2015-05-16 08:21:49 +10:00
Paul Riseborough bd91b9727f AP_NavEKF: Add pre-arm check for horizontal inertial errors
This check will declare the EKF as unhealthy if the horizontal position innovations exceed a threshold  before motors are armed.
This will help to prevent a takeoff with bad inertial data caused by bad accel or gyro offsets.
2015-05-15 10:35:25 +09:00
Paul Riseborough cb59570938 AP_NavEKF: Remove bug preventing external selection of optical flow mode 2015-05-09 18:31:18 +10:00
Tom Pittenger 2deb0e08ea AP_NavEKF: replace fabs() with fabsf() 2015-05-09 09:57:27 +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 08382373f1 AP_NavEKF: Reduce likelihood of in-air switch to GPS with a bad heading
Increases magnetometer weighting on yaw corrections when there there is no other aiding to constrain yaw drift.
Prevents switch to GPS if magnetometer data is failing innovation checks which indicates a bad yaw angle
2015-05-08 16:47:29 +09:00
Jonathan Challinger 142e018a18 AP_NavEKF: fix bug causing takeoff to fail 2015-05-08 09:15:45 +09:00
Andrew Tridgell a140a5e77f AP_NavEKF: revert AP_Math class change 2015-05-05 13:27:07 +10:00
Tom Pittenger 9b53961a7d AP_NavEKF: compiler warnings: float to double 2015-05-05 13:26:52 +10:00
Tom Pittenger 3276eec30e AP_NavEKF: compiler warnings: apply is_zero(float) or is_equal(float) 2015-05-05 13:26:52 +10:00
Andrew Tridgell c4d6f9d040 AP_NavEKF: fix for HAL_SITL rename 2015-05-05 09:45:55 +10:00
Andrew Tridgell ce0a863d01 AP_NavEKF: allow for optimisation of the EKF
disable optimisation when debugging needed
2015-05-05 09:45:53 +10: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 fb7e75594f AP_NavEKF: replace memset with quaternion.initialise 2015-05-01 16:37:35 +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 dd1e0b2f0b AP_NavEKF: Add missing function to return the NED to XYZ quaternion 2015-05-01 16:37:30 +09:00
Paul Riseborough 2c4572eb50 AP_NavEKF: Make ground effect variable names more descriptive 2015-05-01 16:37:29 +09:00
Paul Riseborough 3986851c51 AP_NavEKF: Prevent Z accel bias changing during ground effect takeoff
The inconsistent baro data during ground effect takeoff combined with the larger variances in the Z accel bias state early in flight can cause unwanted changes in bias estimate and therefore changes in height estimation error.
This patch turns of the process noise and state updates for the Z accel bias state when takeoff in ground effect is expected.
2015-05-01 16:37:28 +09:00
Paul Riseborough 2cba60c731 AP_NavEKF: Decouple takeoff ground effect compensation from arm transtion
Thsi fixes a potential bug where the vehicle could land at a lower location without disarming and re-enter ground effect takeoff mode wiht a baro height floor above the current altitude, causing unpredictable height gain
2015-05-01 16:37:27 +09:00
Paul Riseborough c4c0f819b5 AP_NavEKF: Ensure Covariance initialisation uses correct IMU time step 2015-05-01 16:37:26 +09:00
Paul Riseborough 5dc29699ab AP_NavEKF: Publish the INS delta quaternion 2015-05-01 16:37:25 +09:00
Paul Riseborough d273302ce7 AP_NavEKF: Improve rate of accel bias learning before flight 2015-05-01 16:37:24 +09:00