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

ekf2: Make stuck detector optional

This commit is contained in:
Sverre Velten Rothmund 2023-11-07 08:53:09 +01:00 committed by Mathieu Bresciani
parent 3d16383bb4
commit 94d4dc85f8
3 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/sensor_range_finder.cpp
View File

@ -114,6 +114,12 @@ inline bool SensorRangeFinder::isDataInRange() const
void SensorRangeFinder::updateStuckCheck()
{
if(!isStuckDetectorEnabled()){
// Stuck detector disabled
_is_stuck = false;
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 (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) {

3
src/modules/ekf2/EKF/sensor_range_finder.hpp
View File

@ -60,6 +60,7 @@ public:
bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
bool isDataReady() const { return _is_sample_ready; }
bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
void setSample(const rangeSample &sample)
{
@ -131,7 +132,7 @@ private:
* Stuck check
*/
bool _is_stuck{};
float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m)
float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable
float _stuck_min_val{}; ///< minimum value for new rng measurement when being stuck
float _stuck_max_val{}; ///< maximum value for new rng measurement when being stuck

12
src/modules/ekf2/test/test_SensorRangeFinder.cpp
View File

@ -240,8 +240,16 @@ TEST_F(SensorRangeFinderTest, rangeStuck)
// THEN: the data should be marked as unhealthy
// because the sensor is "stuck"
EXPECT_FALSE(_range_finder.isDataHealthy());
EXPECT_FALSE(_range_finder.isHealthy());
if (_range_finder.isStuckDetectorEnabled()) {
EXPECT_FALSE(_range_finder.isDataHealthy());
EXPECT_FALSE(_range_finder.isHealthy());
} else {
// If stuck detector is disabled then the
// data should instantly be marked as healthy
EXPECT_TRUE(_range_finder.isDataHealthy());
EXPECT_TRUE(_range_finder.isHealthy());
}
// BUT WHEN: we continue to send samples but with changing distance
for (int i = 0; i < 2; i++) {