AP_InertialSensor: init all notch center frequencies

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -986,7 +986,9 @@ AP_InertialSensor::init(uint16_t loop_rate)
         if (!notch.params.enabled() && !fft_enabled) {
             continue;
         }
-        notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz();
+        for (uint8_t i = 0; i < ARRAY_SIZE(notch.calculated_notch_freq_hz); i++) {
+            notch.calculated_notch_freq_hz[i] = notch.params.center_freq_hz();
+        }
         notch.num_calculated_notch_frequencies = 1;
         notch.num_dynamic_notches = 1;
 #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)

libraries/Filter/HarmonicNotchFilter.cpp

@@ -20,9 +20,21 @@
 
 #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
 
+/*
+  optional logging for SITL only of all notch frequencies
+ */
+#ifndef NOTCH_DEBUG_LOGGING
+#define NOTCH_DEBUG_LOGGING 0
+#endif
+
+
 // table of user settable parameters
 const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
 
@@ -177,13 +189,8 @@ void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint32_t harm
   expand the number of filters at runtime, allowing for RPM sources such as lua scripts
  */
 template <class T>
-void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
+void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
 {
-    uint8_t num_filters = _num_harmonics * num_notches * _composite_notches;
-    if (num_filters <= _num_filters) {
-        // enough already
-        return;
-    }
     if (_alloc_has_failed) {
         // we've failed to allocate before, don't try again
         return;
@@ -192,15 +199,15 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
       note that we rely on the semaphore in
       AP_InertialSensor_Backend.cpp to make this thread safe
      */
-    auto filters = new NotchFilter<T>[num_filters];
+    auto filters = new NotchFilter<T>[total_notches];
     if (filters == nullptr) {
         _alloc_has_failed = true;
         return;
     }
     memcpy(filters, _filters, sizeof(filters[0])*_num_filters);
     auto _old_filters = _filters;
     _filters = filters;
-    _num_filters = num_filters;
+    _num_filters = total_notches;
     delete[] _old_filters;
 }
 
@@ -217,7 +224,7 @@ void HarmonicNotchFilter<T>::update(float center_freq_hz)
 
     // adjust the fundamental center frequency to be in the allowable range
     const float nyquist_limit = _sample_freq_hz * 0.48f;
-    center_freq_hz = constrain_float(center_freq_hz, 1.0f, nyquist_limit);
+    center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit);
 
     _num_enabled_filters = 0;
     // update all of the filters using the new center frequency and existing A & Q
@@ -261,15 +268,16 @@ void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq
     // adjust the frequencies to be in the allowable range
     const float nyquist_limit = _sample_freq_hz * 0.48f;
 
-    if (num_centers > _num_filters) {
+    uint16_t total_notches = num_centers * _num_harmonics * _composite_notches;
+    if (total_notches > _num_filters) {
         // alloc realloc of filters
-        expand_filter_count(num_centers);
+        expand_filter_count(total_notches);
     }
 
     _num_enabled_filters = 0;
 
     // update all of the filters using the new center frequencies and existing A & Q
-    for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
+    for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
         const uint8_t harmonic_n = i / num_centers;
         const uint8_t center_n = i % num_centers;
         // the filters are ordered by center and then harmonic so
@@ -278,7 +286,7 @@ void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq
             continue;
         }
 
-        const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 1.0f, nyquist_limit);
+        const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit);
         if (_composite_notches != 2) {
             // only enable the filter if its center frequency is below the nyquist frequency
             if (notch_center < nyquist_limit) {
@@ -311,10 +319,29 @@ T HarmonicNotchFilter<T>::apply(const T &sample)
         return sample;
     }
 
+#if NOTCH_DEBUG_LOGGING
+    static int dfd = -1;
+    if (dfd == -1) {
+        dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644);
+    }
+#endif
+
     T output = sample;
-    for (uint8_t i = 0; i < _num_enabled_filters; i++) {
+    for (uint16_t i = 0; i < _num_enabled_filters; i++) {
+#if NOTCH_DEBUG_LOGGING
+        if (!_filters[i].initialised) {
+            ::dprintf(dfd, "------- ");
+        } else {
+            ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz);
+        }
+#endif
         output = _filters[i].apply(output);
     }
+#if NOTCH_DEBUG_LOGGING
+    if (_num_enabled_filters > 0) {
+        ::dprintf(dfd, "\n");
+    }
+#endif
     return output;
 }
 
@@ -328,7 +355,7 @@ void HarmonicNotchFilter<T>::reset()
         return;
     }
 
-    for (uint8_t i = 0; i < _num_filters; i++) {
+    for (uint16_t i = 0; i < _num_filters; i++) {
         _filters[i].reset();
     }
 }

libraries/Filter/HarmonicNotchFilter.h

@@ -32,7 +32,7 @@ class HarmonicNotchFilter {
     // allocate a bank of notch filters for this harmonic notch filter
     void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches);
     // expand filter bank with new filters
-    void expand_filter_count(uint8_t num_notches);
+    void expand_filter_count(uint16_t total_notches);
     // initialize the underlying filters using the provided filter parameters
     void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
     // update the underlying filters' center frequencies using center_freq_hz as the fundamental
@@ -60,11 +60,11 @@ class HarmonicNotchFilter {
     // number of notches that make up a composite notch
     uint8_t _composite_notches;
     // number of allocated filters
-    uint8_t _num_filters;
+    uint16_t _num_filters;
     // pre-calculated number of harmonics
     uint8_t _num_harmonics;
     // number of enabled filters
-    uint8_t _num_enabled_filters;
+    uint16_t _num_enabled_filters;
     bool _initialised;
 
     // have we failed to expand filters?

libraries/Filter/NotchFilter.cpp

@@ -71,7 +71,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
                                           _center_freq_hz * NOTCH_MAX_SLEW_UPPER);
     }
 
-    if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {
+    if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {
         float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz;
         float alpha = sinf(omega) / (2 * Q);
         b0 =  1.0 + alpha*sq(A);

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);