From 1beb6d70f00b8cd5f4e7129bead7940c43f6bcba Mon Sep 17 00:00:00 2001
From: Cavan O'Horo <116211548+cav2094@users.noreply.github.com>
Date: Thu, 26 Feb 2026 00:55:35 -0500
Subject: [PATCH] =?UTF-8?q?fix(mathlib):=20correct=20MedianFilter=20compar?=
 =?UTF-8?q?ator=20to=20satisfy=20strict-weak=20=E2=80=A6=20(#26583)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* fix(mathlib): correct MedianFilter comparator to satisfy strict-weak ordering

- Add explicit NaN handling before comparison operators
- NaN sorted to high end, ensuring finite values cluster at median index
- Guard NaN checks with if constexpr for non-floating-point types
- Replace float equality check with < and > to avoid -Wfloat-equal

Fixes #25917

* fix(mathlib): remove type_traits dependency in MedianFilter

Replace std::is_floating_point_v<T> with a self-contained template
specialization to avoid <type_traits> header, which is unavailable
on NuttX targets compiled with -nostdinc++.

* fixed formating

* test(mathlib): add majority-finite and majority-NaN window tests for MedianFilter

* Moved structs inside namespace math

* clean up

* add two more tests

---------

Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
---
 src/lib/mathlib/math/filter/MedianFilter.hpp  | 30 ++++++++++-
 .../mathlib/math/test/MedianFilterTest.cpp    | 51 +++++++++++++++++++
 2 files changed, 80 insertions(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/filter/MedianFilter.hpp b/src/lib/mathlib/math/filter/MedianFilter.hpp
index b483e59e7c..3fcdc800c3 100644
--- a/src/lib/mathlib/math/filter/MedianFilter.hpp
+++ b/src/lib/mathlib/math/filter/MedianFilter.hpp
@@ -48,6 +48,11 @@
 namespace math
 {
 
+template<typename U> struct is_floating_point { static constexpr bool value = false; };
+template<> struct is_floating_point<float> { static constexpr bool value = true; };
+template<> struct is_floating_point<double> { static constexpr bool value = true; };
+template<> struct is_floating_point<long double> { static constexpr bool value = true; };
+
 template<typename T, int WINDOW = 3>
 class MedianFilter
 {
@@ -82,7 +87,30 @@ private:
 
 	static int cmp(const void *a, const void *b)
 	{
-		return (*(T *)a >= *(T *)b) ? 1 : -1;
+		const T va = *(const T *)a;
+		const T vb = *(const T *)b;
+
+		if constexpr(is_floating_point<T>::value) {
+			if (!__builtin_isfinite(va) && !__builtin_isfinite(vb)) {
+				return 0;
+
+			} else if (!__builtin_isfinite(va)) {
+				return  1;
+
+			} else if (!__builtin_isfinite(vb)) {
+				return -1;
+			}
+		}
+
+		if (va < vb) {
+			return -1;
+		}
+
+		if (va > vb) {
+			return  1;
+		}
+
+		return 0;
 	}
 
 	T _buffer[WINDOW] {};
diff --git a/src/lib/mathlib/math/test/MedianFilterTest.cpp b/src/lib/mathlib/math/test/MedianFilterTest.cpp
index f74cc7ef76..2cbafc26b8 100644
--- a/src/lib/mathlib/math/test/MedianFilterTest.cpp
+++ b/src/lib/mathlib/math/test/MedianFilterTest.cpp
@@ -91,3 +91,54 @@ TEST_F(MedianFilterTest, test5i_100)
 		EXPECT_EQ(median_filter5.apply(i), max(0, i - 2));
 	}
 }
+
+TEST_F(MedianFilterTest, test5f_nan_majority_finite)
+{
+	MedianFilter<float, 5> median_filter5;
+	median_filter5.insert(1.0f);
+	median_filter5.insert(2.0f);
+	median_filter5.insert(NAN);
+	median_filter5.insert(3.0f);
+	median_filter5.insert(NAN);
+	EXPECT_TRUE(PX4_ISFINITE(median_filter5.median()));
+	EXPECT_EQ(median_filter5.median(), 3.0f);
+}
+
+TEST_F(MedianFilterTest, test5f_nan_majority_nan)
+{
+	MedianFilter<float, 5> median_filter5;
+	median_filter5.insert(NAN);
+	median_filter5.insert(NAN);
+	median_filter5.insert(1.0f);
+	median_filter5.insert(NAN);
+	median_filter5.insert(2.0f);
+	EXPECT_FALSE(PX4_ISFINITE(median_filter5.median()));
+}
+
+TEST_F(MedianFilterTest, test3f_equal_values)
+{
+	MedianFilter<float, 3> median_filter3;
+	median_filter3.insert(5.0f);
+	median_filter3.insert(5.0f);
+	median_filter3.insert(5.0f);
+	EXPECT_EQ(median_filter3.median(), 5.0f);
+}
+
+TEST_F(MedianFilterTest, test3f_nan_spike_recovery)
+{
+	MedianFilter<float, 3> median_filter3;
+
+	// Fill with finite values
+	median_filter3.apply(1.0f);
+	median_filter3.apply(2.0f);
+	EXPECT_EQ(median_filter3.apply(3.0f), 2.0f); // window: [1, 2, 3]
+
+	// NaN spike enters — majority still finite
+	EXPECT_TRUE(PX4_ISFINITE(median_filter3.apply(NAN))); // window: [2, 3, NaN]
+	EXPECT_EQ(median_filter3.median(), 3.0f);
+
+	// Recovery — NaN leaves the window
+	EXPECT_EQ(median_filter3.apply(4.0f), 4.0f); // window: [3, NaN, 4] → sorted [3, 4, NaN] → median 4
+	EXPECT_EQ(median_filter3.apply(5.0f), 5.0f); // window: [NaN, 4, 5] → sorted [4, 5, NaN] → median 5
+	EXPECT_EQ(median_filter3.apply(6.0f), 5.0f); // window: [4, 5, 6] → all finite again
+}