Commit Graph

47 Commits

Author SHA1 Message Date
Stephen Dade c0df999094 AP_NavEKF2: Add support for fixed height optical flow 2022-12-01 08:03:47 +09:00
Randy Mackay 6c947fc10b AP_NavEKF2: minor spelling fixes to optflow fusion 2022-02-08 17:01:23 +09:00
Randy Mackay 88a3bfeccb AP_NavEKF2: minor spelling fixes 2022-01-17 11:29:39 +09:00
Andrew Tridgell 6aca0bb08a AP_NavEKF2: allow for double EKF build 2021-07-10 07:20:41 +10:00
murata a5b6ce9220 AP_NavEKF2: Change misnomer (NFC) 2021-03-19 17:49:27 +11:00
Andrew Tridgell 16f7e51ea5 AP_NavEKF2: fixed use of pointers in ringbuffers
these don't work with AP_DAL
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 032b7c81e5 AP_NavEKF2: correct includes 2020-08-07 19:20:07 +10:00
Andy Piper 9f02b1b1a8 AP_NavEKF2: fix typo in comment
Signed-off-by: Dr.-Ing. Amilcar do Carmo Lucas <amilcar.lucas@iav.de>
2020-05-20 07:29:31 +09:00
Andrew Tridgell cbe34320b5 AP_NavEKF2: fixed build
broken by recent common variable change
2019-10-01 17:35:26 +10:00
Andrew Tridgell d40d812ea4 AP_NavEKF2: fixed build with g++ 9.1
this is a false positive, but the cost of clearing this array is low,
and it saves a much more complex fix
2019-09-24 14:59:17 +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
Paul Riseborough 8038efc152 AP_NavEKF2: Flow use parameter updates 2019-04-02 10:51:12 +11:00
Paul Riseborough aa6eee82f4 AP_NavEKF2: Rework opt flow terrain height logic 2019-04-02 10:51:12 +11:00
Paul Riseborough a40208ebd4 AP_NavEKF2: Improve optical flow height estimation
Updated derivation using sequential fusion of Y and X axis data.
2019-04-02 10:51:12 +11:00
Paul Riseborough 2b8b53d6b2 AP_NavEKF2: Add parameter to control how optical flow flow data is used 2019-04-02 10:51:12 +11:00
Arjun Vinod 78b165e36f AP_NavEKF2: fixed typos 2019-02-26 08:33:39 +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
priseborough c69c5440a0 AP_NavEKF2: Fix calculation of predicted LOS rate in terrain estimator 2017-06-26 14:58:06 +01:00
priseborough ce8e935896 AP_NavEKF2: Fix terrain estimator innovation consistency check 2017-06-20 02:08:56 +01:00
priseborough b7e6e6f95f AP_NavEKF2: Fix bug in flow fusion for terrain estimation
Also remove unused class variables
2017-06-20 02:08:56 +01:00
priseborough 082f429da3 AP_NavEKF2: Fix solution validity reporting bug
Fixes bug that causes relative position status to report as false when using optical flow and using range finder as the primary height sensor.
2017-04-28 17:19:23 +01:00
Andrew Tridgell 82365f8670 AP_NavEKF2: save some memory in the position offsets in EKF2
we don't need to copy that vector3f for every sample. A uint8_t does
the job
2016-10-27 17:09:06 +11:00
priseborough 9a0ce1d5eb AP_NavEKF2: Don't correct flow sensor for zero position offset 2016-10-27 14:54:43 +11:00
priseborough 8922861359 AP_NavEKF2: Correct optical flow data for sensor position offset
Correction requires the body rates averaged across the flow sensor sampling interval. This data has been added to the sensor buffer.
The body rate data from the flow sensor driver does not contain the Z component, so an equivalent value sampled from the navigation IMU has been used instead.
The variable omegaAcrossFlowTime has been moved out of the class and into the only function that uses it.
2016-10-27 14:54:43 +11:00
Mathieu OTHACEHE 152edf7189 Global: remove mode line from headers
Using a global .dir-locals.el file is a better alternative than
reincluding the same emacs header in every file of the project.
2016-10-24 09:42:01 -02:00
priseborough a75a383ef2 AP_NavEKF2: Update protection for out of focus flow data 2016-10-10 14:49:41 +09:00
priseborough f9018fcc1b AP_NavEKF2: Enable simultaneous optical flow and GPS use
Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained.
2016-10-10 14:49:02 +09:00
priseborough e6f36f04db AP_NavEKF2: Enable automatic use of range finder height
The EK2_RNG_USE_HGT parameter sets the height (expressed as a percentage of the maximum range of the range finder as set by the RNGFND_MAX_CM parameter) below which the range finder will be used as the primary height source when the vehicle is moving slowly.

When using a height reference other than GPS, the height datum can drift due to air pressure changes if using baro, or due to terrain height changes if using range finder as the primary height source. To ensure that a consistent height datum is available when switching between altitude sources, the WGS-84 height estimate of the EKF's local positi norigin is updated using a
single state Bayes estimator,

If rngfinder or gps height data is lost whilst being used, there will be a fall-back to baro data.
2016-08-08 10:56:44 +09:00
priseborough 9a23152ee4 AP_NavEKF2: Fix bugs and consolidate aiding switch logic
Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.
2016-07-19 12:16:50 +10:00
Paul Riseborough 65f63341d7 AP_NavEKF2: Improve numerical error protection in optical flow fusion
Abort fusion of data if variances will become negative and publish the fault
2016-05-21 15:13:51 +10:00
Lucas De Marchi 2591261af6 Global: rename min and max macros to uppercase
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.

Changes generated with:

	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
2015-12-01 16:28:09 -02:00
Paul Riseborough f9cadaf15c AP_NavEKF2: Increase resolution of innovation consistency gate parameters
Also adds protection against setting the gate to a number that would casue numerical errors.
2015-11-18 11:39:53 +11:00
Paul Riseborough c9eea98142 AP_NavEKF2: Improve terrain height estimation
The ad-hoc scaling of error growth has been replaced with a consistent method that uses the main nav filters published vertical velocity uncertainty and the terrain gradient assumption.
2015-11-18 11:18:46 +11:00
Paul Riseborough 04228e0b3b AP_NavEKF2: Rework selection of height measurements for fusion
GPS height has been added as a measurement option along with range finder and baro
Selection of the height measurement source has been moved into a separate function
Each height source is assigned its own measurement noise
If GPS or baro alt is not able to be used, it reverts to baro
When baro is not being used, an offset is continually calculated which enables a switch to baro without a height step.
2015-11-18 11:18:42 +11:00
Paul Riseborough 577670ccee AP_NavEKF2: Reduce memory required by 6KB when running at 400Hz
Down-sample the IMU and output observer state data to 100Hz for storage in the buffer.
This reduces storage requirements for Copter by 75% or 6KB
It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate.
This means that the EKF filter operations operate at a maximum rate of 100Hz.
The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy.
2015-11-10 15:51:18 +11:00
Andrew Tridgell 2ab2afc86a AP_NavEKF2: added EK2_IMU_MASK for multiple IMUs
no voting between multiple IMUs yet
2015-11-06 10:42:58 +11:00
Paul Riseborough 74da4d8e57 AP_NavEKF2: Remove GPS glitch offset logic
Correction for steps in position and velocity caused by resets following GPS glitches and other events are now handled by the control loops.
2015-10-30 12:24:04 +09:00
Paul Riseborough cd8b9c7d26 AP_NavEKF: prevent high rate mag data locking out other data fusion 2015-10-20 20:16:04 +11:00
Andrew Tridgell cb0f7cb370 NavEKF2: use new perf counter API 2015-10-20 18:16:15 +11:00
Paul Riseborough a29147d6d2 AP_NavEKF2: Apply optimisations to optical flow covariance update
These are the same optimisations that were successful with the magnetometer fusion
2015-10-20 15:21:40 +11:00
Andrew Tridgell d1dfd5fd01 AP_NavEKF2: use common header for optimisation level and irq disable 2015-10-20 15:21:40 +11:00
Paul Riseborough 8526a8ba7e AP_NavEKF2: Level processor loading between frames
Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
2015-10-20 15:21:39 +11:00
Paul Riseborough 1b8a93ef0c AP_NavEKF2: Update function header comments 2015-10-20 15:21:38 +11:00
Paul Riseborough e3013b493b AP_NavEKF2: Critical bug fix - perf counter not initialised 2015-10-15 09:48:13 +11:00
Paul Riseborough b142cc7fd2 AP_NavEKF2: Rename files and re-distribute content 2015-10-10 14:48:50 +09:00