diff options
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.cpp | 25 |
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]); } } |