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

ekf2: selector use hysteresis for healthy flag

Browse Source 
- require that an instance is healthy continuously for >= 1s before
allowing it to be used
This commit is contained in:
Daniel Agar
2021-12-14 09:52:36 -05:00
parent eb69f15d3a
commit 3fba7288af
3 changed files with 21 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2Selector.hpp
+6 -2
View File
@@ -39,6 +39,7 @@
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/time.h>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
@@ -107,7 +108,9 @@ private:
estimator_odometry_sub{ORB_ID(estimator_odometry), i},
estimator_wind_sub{ORB_ID(estimator_wind), i},
instance(i)
{}
{
healthy.set_hysteresis_time_from(false, 1_s);
}
uORB::SubscriptionCallbackWorkItem estimator_attitude_sub;
uORB::SubscriptionCallbackWorkItem estimator_status_sub;
@@ -130,7 +133,8 @@ private:
float combined_test_ratio{NAN};
float relative_test_ratio{NAN};
bool healthy{false};
systemlib::Hysteresis healthy{false};
bool warning{false};
bool filter_fault{false};
bool timeout{false};