From 5c08b1144201f9a76dbb2d08b4a83e801a5cee08 Mon Sep 17 00:00:00 2001
From: Balduin <balduin@auterion.com>
Date: Mon, 27 Jan 2025 09:38:16 +0100
Subject: [PATCH] DataValidator: handle low noise scale

Previously, DataValidator would automatically reject sensor data as
stale if (almost) constant.

But if setting SIH_NOISE_SCALE = 0 for deterministic sim, constant
sensor data are to be expected.

This adds logic to not flag sensor values as stale if noise scale is
very small. If the sensor has *actually* gone stale with very low noise
scale, this means we cannot detect it anymore.
---
 .../sensors/data_validator/DataValidator.cpp  | 23 +++++++++++++++--
 .../sensors/data_validator/DataValidator.hpp  | 25 +++++++++++++++++--
 .../simulation/simulator_sih/sih_params.c     |  2 +-
 test/mavsdk_tests/autopilot_tester.cpp        |  2 +-
 4 files changed, 46 insertions(+), 6 deletions(-)

diff --git a/src/modules/sensors/data_validator/DataValidator.cpp b/src/modules/sensors/data_validator/DataValidator.cpp
index 75b2f88e88..25d1b0d720 100644
--- a/src/modules/sensors/data_validator/DataValidator.cpp
+++ b/src/modules/sensors/data_validator/DataValidator.cpp
@@ -44,6 +44,11 @@
 #include <px4_platform_common/log.h>
 #include <drivers/drv_hrt.h>
 
+DataValidator::DataValidator() :
+	ModuleParams(nullptr)
+{
+}
+
 void DataValidator::put(uint64_t timestamp, float val, uint32_t error_count_in, uint8_t priority_in)
 {
 	float data[dimensions] = {val};  // sets the first value and all others to 0
@@ -99,6 +104,16 @@ void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_
 
 float DataValidator::confidence(uint64_t timestamp)
 {
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		_noise_scale = _sih_noise_scale.get();
+	}
+
 
 	float ret = 1.0f;
 
@@ -112,8 +127,12 @@ float DataValidator::confidence(uint64_t timestamp)
 		_error_mask |= ERROR_FLAG_TIMEOUT;
 		ret = 0.0f;
 
-	} else if (_value_equal_count > _value_equal_count_threshold) {
-		/* we got the exact same sensor value N times in a row */
+	} else if (_value_equal_count > _value_equal_count_threshold && !(fabsf(_noise_scale) < 0.000001f)) {
+		/*
+		we got the exact same sensor value N times in a row
+		If SIH noise scale is very low, constant sensor data are to be expected. In that case, don't
+		reject it as stale.
+		*/
 		_error_mask |= ERROR_FLAG_STALE_DATA;
 		ret = 0.0f;
 
diff --git a/src/modules/sensors/data_validator/DataValidator.hpp b/src/modules/sensors/data_validator/DataValidator.hpp
index 9c4bb105b1..7f0a10486f 100644
--- a/src/modules/sensors/data_validator/DataValidator.hpp
+++ b/src/modules/sensors/data_validator/DataValidator.hpp
@@ -44,12 +44,22 @@
 #include <math.h>
 #include <stdint.h>
 
-class DataValidator
+#include <px4_platform_common/module_params.h>
+
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+
+using namespace time_literals;
+
+
+
+class DataValidator : public ModuleParams
 {
 public:
 	static const unsigned dimensions = 3;
 
-	DataValidator() = default;
+
+	DataValidator();
 	~DataValidator() = default;
 
 	/**
@@ -182,6 +192,8 @@ private:
 	float _rms[dimensions] {};  /**< root mean square error */
 	float _value[dimensions] {}; /**< last value */
 
+	float _noise_scale{1.};
+
 	unsigned _value_equal_count{0}; /**< equal values in a row */
 	unsigned _value_equal_count_threshold{
 		VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */
@@ -197,4 +209,13 @@ private:
 	/* we don't want this class to be copied */
 	DataValidator(const DataValidator &) = delete;
 	DataValidator operator=(const DataValidator &) = delete;
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIH_NOISE_SCALE>) _sih_noise_scale
+	)
+
+
+
 };
diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c
index fabc091338..e28215df44 100644
--- a/src/modules/simulation/simulator_sih/sih_params.c
+++ b/src/modules/simulation/simulator_sih/sih_params.c
@@ -347,4 +347,4 @@ PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0);
  *
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_FLOAT(SIH_NOISE_SCALE, 0.01f);
+PARAM_DEFINE_FLOAT(SIH_NOISE_SCALE, 1.0f);
diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 253e9aebb1..a7d9065b40 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -1039,6 +1039,6 @@ void AutopilotTester::check_airspeed_is_invalid()
 	// If the airspeed was invalidated during the flight, the airspeed is sent in the
 	// telemetry is NAN and stays so with the default parameter settings.
 	const Telemetry::FixedwingMetrics &metrics = getTelemetry()->fixedwing_metrics();
-	std::cout << "Reported airspeed after failure: " << metrics.airspeed_m_s ;
+	std::cout << "Reported airspeed after failure: " << metrics.airspeed_m_s << "\n";
 	REQUIRE(!std::isfinite(metrics.airspeed_m_s));
 }