fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 16:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Make range finder data continuous check more robust

Browse Source 
Use a filtered arrival time delta to determine if range data is continuous
This commit is contained in:
devbharat
2017-02-22 14:12:05 +01:00
committed by Roman
parent d94068b88a
commit 8004e9fe7e
3 changed files with 32 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+26 -1
View File
@@ -41,6 +41,7 @@
#include "ekf.h"
#include "mathlib.h"
#include <drivers/drv_hrt.h>
bool Ekf::initHagl()
{
@@ -135,7 +136,7 @@ bool Ekf::get_terrain_vert_pos(float *ret)
{
memcpy(ret, &_terrain_vpos, sizeof(float));
if (_terrain_initialised && (_time_last_imu - _time_last_hagl_fuse < 1e6)) {
if (_terrain_initialised && _range_data_continuous) {
return true;
} else {
@@ -153,3 +154,27 @@ void Ekf::get_hagl_innov_var(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 (2Hz ie 500 ms)
/* Timing in micro seconds */
static hrt_abstime t = 0;
static hrt_abstime t_prev = 0;
static float dt = 0.0f;
t = hrt_absolute_time();
dt = t_prev != 0 ? (t - t_prev) * 1.0f : 0.0f;
t_prev = t;
dt = math::min(dt, 1.0f);
static float range_update_interval = 0.0f;
/* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */
range_update_interval = range_update_interval * (1.0f - dt) + dt * (_time_last_imu - _time_last_range);
if (range_update_interval < 5e5f) {
_range_data_continuous = true;
} else {
_range_data_continuous = false;
}
}