accela: elide NaN output values

1 files changed, 21 insertions, 4 deletions

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index ab5fda07..2fb00739 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -58,15 +58,32 @@ FTNoIR_Filter::FTNoIR_Filter() : first_run(true)
     }
 }
 
+static inline bool nanp(double value)
+{
+    return std::isnan(value) || std::isinf(value);
+}
+
+static inline double elide_nan(double value, double def)
+{
+    if (nanp(value))
+    {
+        if (nanp(def))
+            return 0;
+        return def;
+    }
+    return value;
+}
+
 void FTNoIR_Filter::filter(const double* input, double *output)
 {
     if (first_run)
     {
         for (int i = 0; i < 6; i++)
         {
-            output[i] = input[i];
-            last_output[i] = input[i];
-            smoothed_input[i] = input[i];
+            const double f = nanp(input[i]) ? 0 : input[i];
+            output[i] = f;
+            last_output[i] = f;
+            smoothed_input[i] = f;
         }
         first_run = false;
         t.start();
@@ -104,7 +121,7 @@ void FTNoIR_Filter::filter(const double* input, double *output)
             : result >= in;
         const double ret = done ? in : result;
         
-        last_output[i] = output[i] = ret;
+        last_output[i] = output[i] = elide_nan(ret, last_output[i]);
     }
 }