* EKF: centralize range finder tilt check
* Ekf-control: do not double check for terrain estimate validity
isRangeAidSuitable can only return true if the terrain estimate is valid
so there is no need for an additional check
* range_finder_checks: restructure the checks to avoid early returns
There is now only one clear path that can lead to the validity being
true.
Furthermore, if the _rng_hgt_valid is true, we can trust it and we don't
need for additional checks such as tilt.
The case where we need to provide fake measurements because the drone is
on the ground and the range finder data is bad is already handled
in "controlHeightFusion" so there is no need to hack the range finder
checks with that.
* Add Sensor and SensorRangeFinder classes
The purpose is to encapsulate the checks for each sensor in a dedicated
class with the same interface
* SensorRangeFinder: encapsulate in estimator::sensor namespace
* EKF: rename _sensor_rng to _range_sensor
* Range checks: include limits in valid range
* RangeChecks: update comment in the continuity checks
* RangeChecks: move more low-level checks in functions
Also move setTilt out of the terrain estimator, this is anyway protected internally
to not compute cos/sin if the parameter did not change.
* Sensor: remove unused virtual functions
Those are not required yet but can still be added later
* SensorRangeFinder: re-organise member variables
Also rename getRangeToEarth to getCosTilt
* SensorRangeFinder: split setSensorTilt and setCosMaxTilt functions
* SensorRangeFinder: Add a few unit tests
- good data
- tilt exceeded
- max range exceeded
* SensorRangeFinder: set hysteresis in us instead of ms
* SensorRangeFinder: Add more tests
* SensorRangeFinder: update continuity, hysteresis and stuck tests
* SensorRangeFinder: rename variables
* SensorRangeFinder: get rid of "delayed" specification
From the SensorRangeFinder class point of view, it's not relevant to
know if the data is delayed or not
* SensorRangeFinder: move time_last_valid out of stuck check
* SensorRangeFinder: rename file names to sensor_range_finder
* SensorRangeFinder: address Kamil's comments
* SensorRangeFinder: Add more tilt tests
* SensorRangeFinder: store current tilt offset
This is to avoid recomputing cos/sin functions at each loop
* EKF: Enable GPS flight without magnetometer
Enables takeoff in a non-GPS flight mode with mag fusion type set to MAG_FUSE_TYPE_NONE. After sufficient movement the EKF will reset the yaw tot he EKF-GSF estimate. After that GPS fusion will commence.
* EKF: Fix unconstrained yaw and yaw variance growth when on ground
* EKF: Ensure first yaw alignment can't be blocked
* EKF: Increase yaw variance limit allowed for alignment
Flight test data indicates that an uncertainty value of 15 deg still provides a reliable yaw estimate and enables an earlier alignment/reset if required.
* EKF: Remove unexecutable code
* EKF: Restructure heading fusion
* EKF: parameterise quarter variance check and retune default value
* EKF: Pass by reference instead of pointer
* EKF: Clarify reset logic
* EKF: Remove incorrect setting of mag field alignment flag
* EKF: Non-functional tidy up
* EKF: Fix non-use of function argument
The updateQuaternion function was using the _heading_innovation class variable instead of setting it using the innovation argument.
* EKF: Fix undefined variable
* EKF: Use single precision atan2
* EKF: remove unnecessary timer reset and unwanted execution of reset function
* EKF: Don't declare a mag fault when non-use is user selected
Doing so produces unnecessary user alerts.
* ekf_control: Inhibit mag fusion when field magnitude is large
Move mag inhibition check in separate function
* ekf_control: pull out of functionalities out of controlMagFusion
- yaw abd mag bias observability checks
- mag 3D conditions
- load mag covariances
- set and clear mag control modes
* ekf_control: refactor mag heading/3D start/stop.
Move mag declination, mag 3d and mag heading fusion out of the main function
* ekf_control: extract mag yaw reset and mag declination fusion requirements
* ekf_control: use WMM in isStronMagneticField for mag fusion inhibition
- Correct units of WMM strength table
* ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom)
Also split inAirYawReset from onGroundYawReset
* ekf_control: extract mag automatic selection
- transform if-else into switch-case for parameter fusion type selection
* ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix
flag naming in Test script
* ekf_control: do not run mag fusion if tilt is not aligned.
Reset some variables on ground even if mag fusion is not running yet. It
could be that it runs later so we need to make sure that those variables
are properly set.
* ekf_control: move controlMagFusion and related functions to mag_control.cpp
* ekf control: check for validity of mag strength from WMM and falls back
to average earth mag field with larger gate if not valid
* ekf control: remove evyaw check for mag inhibition
* ekf control: change nested ternary operator into if-else if
* Ekf: create AlphaFilter template class for simple low-pass filtering
0.1/0.9 type low-pass filters are commonly used to smooth data, this
class is meant to abstract the computation of this filter
* ekf control: reset heading using mag_lpf data to avoid resetting on an outlier
fixes ecl issue #525
* ekf control: replace mag_states_only flag with mag_field_disturbed and
add parameter to enable or disable mag field strength check
* ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore
* ekf control: use start/stop mag functions everywhere instead of setting the flag
* ekf control: Run mag fusion depending on yaw_align instead of tilt_align
as there is no reason to fuse mag when the ekf isn't aligned
* AlphaFilter: add test for float and Vector3f
The quaternion to inverse rotation matrix function has been updated so that the rotation it produces is the inverse to that produced by the matrix library and the the inverse of the quaternion is uses. This function is now used to directly calculate an inverse rotation matrix rather than calculating the forward rotation and then transposing it.
clearance. This is the best estimate as we should not rely on a distance
sensor while on the ground. This also helps when the drone is carried
over as it avoids starting with a crazy downward distance for optical
flow scaling.
This removes the check for _rng_hgt_faulty in the decision of publishing
a fake range measurement. The reason for this is that some distance sensors
don't populate the quality flag, even if they are saturated. Hence, if we
are on the ground and not moving, it is safe to publish a fake measurement
of the distance sensor and overwrite the actual sensor data.
This commit introduces a quality measure in the range data. It is
used to properly decide whether to initialize the HAGL estimate on
sensor data or MIN_HGT parameter, as well as in the decision of
whether a 'fake' measurement should be published on the ground to
allow for optical flow take-offs.
* terrain_estimator : guard against case where latest range sample is newer than IMU sample
* EKF : control : correct detection of no optical flow fusion over a time period
Terrain validity is determined solely by successful range finder fusion and terrain state initialisation.
A range finder that has been declared faulty requires continuous range finder data fusion requires data to be continuous before the fault status _rng_hgt_faulty can be cleared. This will enforce the requirement for continuous data before fusion can commence.
Eliminate race condition caused by checking for data freshness using time stamps from buffer push instead than buffer pop events.
Consistent use of range data ready and range data fault flags. This achieved by ensuring that _rng_hgt_faulty is set to true for all range data faults, not just data freshness.
Include range data validity requirement in rangeAidConditionsMet() check.
* EKF: Move optical flow specific state reset to helper functions
* EKF: Ensure loss of optical flow aiding is handled correctly
If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference.
If flow data is unavailable for too long - declare optical flow use stopped.
Use consistent time periods for all resets
* EKF: Ensure loss of external vision aiding is handled correctly
If data is only source of aiding and has been rejected for too long - reset using data as a position.
Don't reset velocity if there is another source of aiding constraining it.
If data is unavailable for too long, declare external vision use stopped.
Use consistent time periods for all resets.
* EKF: Update parameter documentation
Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer
* EKF: make class variable units consistent with documentation
* EKF: Don't reset states when optical flow use commences if using external vision
* EKF: Stop optical flow fusion when on ground if excessive movement is detected.
* EKF: fix terrain estimator vulnerabilities
Reset estimate to sensor value if rejected for 10 seconds
Protect against user motion when on ground.
Fix unnecessary duplication of terrain validity check and separate validity update and reporting.
* EKF: remove unnecessary Info console prints
Optical flow use information can be obtained from the estimator_status.control_mode_flags message
* EKF: fix inaccurate comment
* EKF: remove duplicate calculation from terrain validity accessor function
kamilritz
bresch
Roman Bapst
Paul Riseborough
Daniel Agar
Paul Riseborough
Nico van Duijn
Carl Olsson
Kabir Mohammed
ChristophTobler