Commit Graph

43 Commits

Author SHA1 Message Date
Stephen Dade abcacec25f AP_NavEKF3: Add support for fixed height optical flow 2022-12-01 08:03:47 +09:00
Randy Mackay e0023a99bc AP_NavEKF3: replace AP_HAL::millis() with dal.millis() 2022-05-18 17:59:57 +10:00
Randy Mackay 0c7ad9511f AP_NavEKF3: FuseOptFlow only calculates range and velocity once
was unnecessarily recalculating these values before fusion
2022-02-09 08:42:29 +09:00
Randy Mackay cfcdc1e78f AP_NavEKF3: optflow terrain reset after 5sec 2022-02-02 18:04:43 +11:00
Randy Mackay e16a64383f AP_NavEKF3: getOptFlowSample returns latest correct flow data for use in calibration 2022-01-29 08:26:12 +09:00
Randy Mackay cff3794d25 AP_NavEKF3: minor spelling fixes 2022-01-17 11:30:47 +09:00
Paul Riseborough c0d88e2673 AP_NavEKF3: Fix vertical velocity reset
Fixes bug that prevents the vertical velocity being reset to the GPS if the position aiding has already timed out and improves sensitivity of the bad IMU data check.
2021-09-23 18:55:28 +10:00
Randy Mackay d1f2acd813 AP_NavEKF3: rename varInnovOptFlow to flowVarInnov
also renamed innovOptFlow to flowInnov
2021-08-24 16:49:04 +09:00
Randy Mackay 11847cfcf5 AP_NavEKF3: always calculate optical flow innovations and variances 2021-08-24 16:49:04 +09:00
Randy Mackay 914a8dca49 AP_NavEKF3: for loops use uint8_t instead of unsigned 2021-08-24 16:49:04 +09:00
Randy Mackay f9fd63e01e AP_NavEKF3: remove unused optflow calcs 2021-08-24 16:49:04 +09:00
Paul Riseborough ed61287410 AP_NavEKF3: Don't update accel bias states if vibration affected 2021-07-22 18:20:45 +10:00
Andrew Tridgell 885e518741 AP_NavEKF3: allow for double EKF build 2021-07-10 07:20:41 +10:00
Peter Barker 15658362b5 AP_NavEKF3: make time-horizon OF data a local variable
Saves 40 bytes of RAM per core on stm32
2021-06-01 09:56:19 +10:00
Paul Riseborough ac87cab6bc AP_NavEKF3: constify array index variable 2021-03-23 10:05:59 +11:00
Paul Riseborough a07427fd30 AP_NavEKF3: Don't learn poorly observed IMU dvel bias states before flight 2021-03-23 10:05:59 +11:00
Randy Mackay 33b6212cce AP_NavEKF3: rename _sources to sources 2020-11-20 16:43:44 +09:00
Randy Mackay 6daaa06317 AP_NavEKF3: integrate AP_NavEKF_Source::useVelXYSource
also integrate useVelZSource
2020-11-20 16:43:44 +09:00
Randy Mackay 8a56ce49be AP_NavEKF3: optical flow fusion checks source 2020-11-20 16:43:44 +09:00
Randy Mackay 8931e50166 AP_NavEKF3: integrate Source for alt 2020-11-20 16:43:44 +09:00
Andrew Tridgell f5275fd0ad AP_NavEKF3: fixed use of pointers in ringbuffers
these don't work with AP_DAL
2020-11-10 16:15:45 +11:00
Andrew Tridgell 096aab9388 AP_NavEKF3: 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
Randy Mackay c4b7a1c41a AP_NavEKF3: flowDataToFuse moved to local variable
This variable is never used outside the SelectFlowFusion() method
This variable is always updated at the top of the function meaning subsequent calls to the function will always overwrite its previous value
2020-08-21 13:12:10 +09:00
Randy Mackay 41acf555bf AP_NavEKF3: fix skipping of optflow fusion if mag fusion performed 2020-08-21 13:12:10 +09:00
Peter Barker 6b701ae3b3 AP_NavEKF3: correct includes 2020-08-07 19:20:07 +10:00
Paul Riseborough 9aaa0cd2e0 AP_NavEKF3: Fix typo in comment 2019-04-02 10:51:12 +11:00
Paul Riseborough b7e0a2345c AP_NavEKF3: Fix indexing error
Caused by copy and paste from AP_NavEKF2 which useds index 5 for vertical velocity. EKF3  should use index 6.
2019-04-02 10:51:12 +11:00
Paul Riseborough 0d3e1a7d23 AP_NavEKF3: flow use parameter improvements 2019-04-02 10:51:12 +11:00
Paul Riseborough 35c82ef67f AP_NavEKF3: Improve optical flow height estimation 2019-04-02 10:51:12 +11:00
Paul Riseborough b848e231c7 AP_NavEKF3: Add parameter to control how optical flow data is used 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 93e09c51d3 AP_NavEKF3: eliminate GCS_MAVLINK::send_statustext_all 2017-07-11 23:53:53 +01:00
priseborough 9d0fa09d39 AP_NavEKF3: Fix calculation of predicted LOS rate in terrain estimator 2017-06-26 14:58:06 +01:00
priseborough 06d40a7901 AP_NavEKF3: Fix terrain estimator innovation consistency check 2017-06-20 02:08:56 +01:00
priseborough 284fc0f3c5 AP_NavEKF3: Fix bug in flow fusion for terrain estimation
Also remove unused class variables
2017-06-20 02:08:56 +01:00
priseborough a1c117360c AP_NavEKF3: Ensure Kalman gain calculatons respect deactivated states
All Kalman gain calculations now explicity set gains for deactivated states to zero.
Previous use of loops to set gains to zero have been replaced with more efficient memset operations.
2017-06-14 12:44:41 +01:00
priseborough 1301b9797a AP_NavEKF3: 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
priseborough fb7104f4e3 AP_NavEKF3: add fusion method for body frame odometry data 2017-04-19 11:04:40 +09:00
priseborough 8250fe4943 AP_NavEKF3: fix documentation derivation references 2017-04-10 15:47:35 +09:00
priseborough 2e5ac40ef6 AP_NavEKF3: remove unnecessary stdio includes 2017-01-27 16:57:42 +09:00
priseborough ed5039823f AP_NavEKF3: Adapt sensor buffer lengths
Adapt the lengths of the IMU and observations buffers on startup to the specified time delays and update rates.
This does require the EKF to be re-started if time delays are changed.
2017-01-27 16:57:42 +09:00
priseborough 39969e7d8e AP_NavEKF3: added EKF3 for EKF experimentation
AP_NavEKF3: Implement same maths as PX4/ecl EKF

Replace attitude vector states with quaternions
Remove gyro scale factor states
Add XY accel delta velocity bias estimation
Initial tuning
Add GPS body frame offset compensation

AP_NavEKF3: 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.

AP_NavEKF3: prevent multiple fusion mode changes per filter update

AP_NavEKF3: Update tuning defaults

AP_NavEKF3: Fix bug causing switching in and out of aiding
If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding.
This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed.
An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS.

AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers
This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.

AP_NavEKF3: Improve switch-over to backup magnetometer
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.

AP_NavEKF3: enable automatic use of range finder height

AP_NavEKF3: Fix bug in handling of invalid range data

AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3:

AP_NavEKF3: Handle yaw jumps due to core switches

AP_NavEKF3: Enable simultaneous GPS and optical flow use

AP_NavEKF3: fix console status reporting

AP_NavEKF3: send messages to mavlink instead of console

This allows the GCS to better handle the display of messages to the user.

AP_NavEKF3: replace deprecated function call

AP_NavEKF3: Compensate for sensor body frame offsets

AP_NavEKF3: Fix bug in median filter code

AP_NavEKF3: save some memory in the position offsets in EKF3

We don't need to copy that vector3f for every sample. A uint8_t does the job

AP_NavEKF3: Add fusion of range beacon data

AP_NavEKF3: Bring up to date with EKF2

AP_NavEKF3: Misc range beacon updates

AP_NavEKF3: Add mising accessors

AP_NavEKF3: remove duplicate include

AP_NavEKF3: Prevent NaN's when accessing range beacon debug data

AP_NavEKF3: Update range beacon naming

AP_NavEKF3: updates

AP_NavEKF3: miscellaneous changes

AP_NavEKF3: misc updates

AP_NavEKF3: misc range beacons updates

AP_NavEKF3: add missing rover default param
2016-12-19 08:06:47 +11:00