dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

cleanup checks of range finder data 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2019-10-14 08:17:56 +02:00 committed by Paul Riseborough
parent 4d37065f1b
commit 00f49e62c2
5 changed files with 167 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/CMakeLists.txt
View File

@ -46,6 +46,7 @@ add_library(ecl_EKF
terrain_estimator.cpp
vel_pos_fusion.cpp
gps_yaw_fusion.cpp
range_finder_checks.cpp
)
add_dependencies(ecl_EKF prebuild_targets)

113
EKF/control.cpp
View File

@ -118,9 +118,9 @@ void Ekf::controlFusionModes()
// Get range data from buffer and check validity
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed);
checkRangeDataValidity();
updateRangeDataValidity();
if (_range_data_ready && !_rng_hgt_faulty) {
if (_range_data_ready && _rng_hgt_valid) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
@ -910,23 +910,25 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case we are using range finder for height
if (_control_status.flags.rng_hgt) {
// check if range finder data is available
const rangeSample &rng_init = _range_buffer.get_newest();
bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check if baro data is available
const baroSample &baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// reset to baro if we have no range data and baro data is available
bool reset_to_baro = !rng_data_available && baro_data_available;
bool reset_to_baro = !_rng_hgt_valid && baro_data_available;
// reset to range data if it is available
bool reset_to_rng = rng_data_available;
if (_rng_hgt_valid) {
if (reset_to_baro) {
// reset the height mode
setControlRangeHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
} else if (reset_to_baro) {
// set height sensor health
_rng_hgt_faulty = true;
_baro_hgt_faulty = false;
// reset the height mode
@ -936,17 +938,6 @@ void Ekf::controlHeightSensorTimeouts()
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to baro");
} else if (reset_to_rng) {
// set height sensor health
_rng_hgt_faulty = false;
// reset the height mode
setControlRangeHeight();
// request a reset
reset_height = true;
ECL_WARN_TIMESTAMPED("EKF rng hgt timeout - reset to rng hgt");
} else {
// we have nothing to reset to
reset_height = false;
@ -1016,7 +1007,7 @@ void Ekf::controlHeightFusion()
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = true;
@ -1062,7 +1053,7 @@ void Ekf::controlHeightFusion()
}
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = _range_data_ready;
@ -1098,7 +1089,7 @@ void Ekf::controlHeightFusion()
// Determine if GPS should be used as the height source
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = true;
@ -1161,7 +1152,7 @@ void Ekf::controlHeightFusion()
}
if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt
&& (!_range_data_ready || _rng_hgt_faulty)) {
&& (!_range_data_ready || !_rng_hgt_valid)) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
@ -1210,78 +1201,6 @@ void Ekf::checkRangeAidSuitability()
}
}
void Ekf::checkRangeDataValidity()
{
// check if out of date
if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) {
_rng_hgt_faulty = true;
return;
}
// Don't allow faulty flag to clear unless range data is continuous
if (_rng_hgt_faulty && !_range_data_continuous) {
return;
}
// Don't run the checks after this unless we have retrieved new data from the buffer
if (!_range_data_ready) {
return;
} else {
// reset fault status when we get new data
_rng_hgt_faulty = (_range_sample_delayed.quality == 0);
}
// Check if excessively tilted
if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) {
_rng_hgt_faulty = true;
return;
}
// Check if out of range
if ((_range_sample_delayed.rng > _rng_valid_max_val)
|| (_range_sample_delayed.rng < _rng_valid_min_val)) {
if (_control_status.flags.in_air) {
_rng_hgt_faulty = true;
return;
} else {
// Range finders can fail to provide valid readings when resting on the ground
// or being handled by the user, which prevents use of as a primary height sensor.
// To work around this issue, we replace out of range data with the expected on ground value.
_range_sample_delayed.rng = _params.rng_gnd_clearance;
return;
}
}
// Check for "stuck" range finder measurements when range was not valid for certain period
// This handles a failure mode observed with some lidar sensors
if (((_range_sample_delayed.time_us - _time_last_rng_ready) > (uint64_t)10e6) &&
_control_status.flags.in_air) {
// require a variance of rangefinder values to check for "stuck" measurements
if (_rng_stuck_max_val - _rng_stuck_min_val > _params.range_stuck_threshold) {
_time_last_rng_ready = _range_sample_delayed.time_us;
_rng_stuck_min_val = 0.0f;
_rng_stuck_max_val = 0.0f;
_control_status.flags.rng_stuck = false;
} else {
if (_range_sample_delayed.rng > _rng_stuck_max_val) {
_rng_stuck_max_val = _range_sample_delayed.rng;
}
if (_rng_stuck_min_val < 0.1f || _range_sample_delayed.rng < _rng_stuck_min_val) {
_rng_stuck_min_val = _range_sample_delayed.rng;
}
_control_status.flags.rng_stuck = true;
_rng_hgt_faulty = true;
}
} else {
_time_last_rng_ready = _range_sample_delayed.time_us;
}
}
void Ekf::controlAirDataFusion()
{
// control activation and initialisation/reset of wind states required for airspeed fusion

7
EKF/ekf.h
View File

@ -464,8 +464,9 @@ private:
// height sensor status
bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use
bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent
bool _rng_hgt_faulty{false}; ///< true if valid range finder height data is unavailable for use
bool _rng_hgt_valid{false}; ///< true if range finder sample retrieved from buffer is valid
int _primary_hgt_source{VDIST_SENSOR_BARO}; ///< specifies primary source of height data
uint64_t _time_bad_rng_signal_quality{0}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis)
// imu fault status
uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec)
@ -648,7 +649,7 @@ private:
bool isRangeAidSuitable() { return _is_range_aid_suitable; }
// check for "stuck" range finder measurements when rng was not valid for certain period
void checkRangeDataValidity();
void updateRangeDataValidity();
// return the square of two floating point numbers - used in auto coded sections
static constexpr float sq(float var) { return var * var; }
@ -697,7 +698,7 @@ private:
void resetWindStates();
// check that the range finder data is continuous
void checkRangeDataContinuity();
void updateRangeDataContinuity();
// Increase the yaw error variance of the quaternions
// Argument is additional yaw variance in rad**2

144
EKF/range_finder_checks.cpp Normal file
View File

@ -0,0 +1,144 @@
/****************************************************************************
*
* Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file range_finder_checks.cpp
* Perform checks on range finder data in order to evaluate validity.
*
*
*/
#include "ekf.h"
#define RNG_BAD_SIG_HYST (uint64_t)3e6 // range finder data needs to have 3 seconds of non-zero signal quality to be valid
// check that the range finder data is continuous
void Ekf::updateRangeDataContinuity()
{
// update range data continuous flag (1Hz ie 2000 ms)
/* Timing in micro seconds */
/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
float alpha = 0.5f * _dt_update;
_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
(_imu_sample_delayed.time_us - _range_sample_delayed.time_us);
_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);
if (_dt_last_range_update_filt_us < 2e6f) {
_range_data_continuous = true;
} else {
_range_data_continuous = false;
}
}
void Ekf::updateRangeDataValidity()
{
updateRangeDataContinuity();
// check if out of date
if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) {
_rng_hgt_valid = false;
return;
}
// Don't allow faulty flag to clear unless range data is continuous
if (!_rng_hgt_valid && !_range_data_continuous) {
return;
}
// Don't run the checks after this unless we have retrieved new data from the buffer
if (!_range_data_ready) {
return;
}
if (_range_sample_delayed.quality == 0) {
_time_bad_rng_signal_quality = _imu_sample_delayed.time_us;
_rng_hgt_valid = false;
} else if (_time_bad_rng_signal_quality > 0 && _imu_sample_delayed.time_us - _time_bad_rng_signal_quality > RNG_BAD_SIG_HYST) {
_time_bad_rng_signal_quality = 0;
_rng_hgt_valid = true;
}
// Check if excessively tilted
if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) {
_rng_hgt_valid = false;
return;
}
// Check if out of range
if ((_range_sample_delayed.rng > _rng_valid_max_val)
|| (_range_sample_delayed.rng < _rng_valid_min_val)) {
if (_control_status.flags.in_air) {
_rng_hgt_valid = false;
return;
} else {
// Range finders can fail to provide valid readings when resting on the ground
// or being handled by the user, which prevents use of as a primary height sensor.
// To work around this issue, we replace out of range data with the expected on ground value.
_range_sample_delayed.rng = _params.rng_gnd_clearance;
return;
}
}
// Check for "stuck" range finder measurements when range was not valid for certain period
// This handles a failure mode observed with some lidar sensors
if (((_range_sample_delayed.time_us - _time_last_rng_ready) > (uint64_t)10e6) &&
_control_status.flags.in_air) {
// require a variance of rangefinder values to check for "stuck" measurements
if (_rng_stuck_max_val - _rng_stuck_min_val > _params.range_stuck_threshold) {
_time_last_rng_ready = _range_sample_delayed.time_us;
_rng_stuck_min_val = 0.0f;
_rng_stuck_max_val = 0.0f;
_control_status.flags.rng_stuck = false;
} else {
if (_range_sample_delayed.rng > _rng_stuck_max_val) {
_rng_stuck_max_val = _range_sample_delayed.rng;
}
if (_rng_stuck_min_val < 0.1f || _range_sample_delayed.rng < _rng_stuck_min_val) {
_rng_stuck_min_val = _range_sample_delayed.rng;
}
_control_status.flags.rng_stuck = true;
_rng_hgt_valid = false;
}
} else {
_time_last_rng_ready = _range_sample_delayed.time_us;
}
}

23
EKF/terrain_estimator.cpp
View File

@ -56,7 +56,7 @@ bool Ekf::initHagl()
_terrain_var = sq(_params.rng_gnd_clearance);
initialized = true;
} else if (!_rng_hgt_faulty
} else if (_rng_hgt_valid
&& (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5
&& _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
// if we have a fresh measurement, use it to initialise the terrain estimator
@ -87,9 +87,6 @@ bool Ekf::initHagl()
void Ekf::runTerrainEstimator()
{
// Perform a continuity check on range finder data
checkRangeDataContinuity();
// Perform initialisation check and
// on ground, continuously reset the terrain estimator
if (!_terrain_initialised || !_control_status.flags.in_air) {
@ -110,7 +107,7 @@ void Ekf::runTerrainEstimator()
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
// Fuse range finder data if available
if (_range_data_ready && !_rng_hgt_faulty) {
if (_range_data_ready && _rng_hgt_valid) {
fuseHagl();
// update range sensor angle parameters in case they have changed
@ -321,19 +318,3 @@ void Ekf::getHaglInnovVar(float *hagl_innov_var)
{
memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
}
// check that the range finder data is continuous
void Ekf::checkRangeDataContinuity()
{
// update range data continuous flag (1Hz ie 2000 ms)
/* Timing in micro seconds */
/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
float alpha = 0.5f * _dt_update;
_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
(_imu_sample_delayed.time_us - _range_sample_delayed.time_us);
_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);
_range_data_continuous = (_dt_last_range_update_filt_us < 2e6f);
}