HACK: log the actual frequencies of our notch filters

tridge · Nov 5, 2023 · df2abf5 · df2abf5
1 parent c1831ba
commit df2abf5
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 0 deletions.
diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp
@@ -20,6 +20,10 @@
 
 #include "HarmonicNotchFilter.h"
 #include <GCS_MAVLink/GCS.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <stdio.h>
 
 #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters
 
@@ -301,6 +305,8 @@ void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq
     }
 }
 
+static int dfd = -1;
+
 /*
   apply a sample to each of the underlying filters in turn and return the output
  */
@@ -311,10 +317,22 @@ T HarmonicNotchFilter<T>::apply(const T &sample)
         return sample;
     }
 
+    if (dfd == -1) {
+        dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644);
+    }
+
     T output = sample;
     for (uint8_t i = 0; i < _num_enabled_filters; i++) {
+        if (!_filters[i].initialised) {
+            ::dprintf(dfd, "------- ");
+        } else {
+            ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz);
+        }
         output = _filters[i].apply(output);
     }
+    if (_num_enabled_filters > 0) {
+        ::dprintf(dfd, "\n");
+    }
     return output;
 }
 

diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
@@ -26,9 +26,13 @@
 #include <AP_Param/AP_Param.h>
 
 
+template <class T>
+class HarmonicNotchFilter;
+
 template <class T>
 class NotchFilter {
 public:
+    friend class HarmonicNotchFilter<T>;
     // set parameters
     void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
     void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q);