AP_InertialSensor: remove _ prefix from methods. use changed filter methods.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -666,7 +666,7 @@ AP_InertialSensor::init(uint16_t sample_rate)
     _calculated_harmonic_notch_freq_hz = _harmonic_notch_filter.center_freq_hz();
 
     for (uint8_t i=0; i<get_gyro_count(); i++) {
-        _gyro_harmonic_notch_filter[i].create(_harmonic_notch_filter.harmonics());
+        _gyro_harmonic_notch_filter[i].allocate_filters(_harmonic_notch_filter.harmonics());
     }
 }
 
@@ -1725,7 +1725,7 @@ void AP_InertialSensor::acal_update()
 
 // Update the harmonic notch frequency
 void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) {
-    // When disarmed, throttle is zero
+    // protect against zero as the scaled frequency
     if (is_positive(scaled_freq)) {
         _calculated_harmonic_notch_freq_hz = scaled_freq;
     }

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -228,7 +228,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
 
         // apply the harmonic notch filter
-        if (_gyro_harmonic_notch_enabled()) {
+        if (gyro_harmonic_notch_enabled()) {
             _imu._gyro_filtered[instance] = _imu._gyro_harmonic_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
         }
 
@@ -506,16 +506,15 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
     }
 
     // possily update the harmonic notch filter parameters
-    if (!is_equal(_last_harmonic_notch_bandwidth_hz, _gyro_harmonic_notch_bandwidth_hz()) ||
-        !is_equal(_last_harmonic_notch_attenuation_dB, _gyro_harmonic_notch_attenuation_dB())) {
-        _imu._gyro_harmonic_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_harmonic_notch_center_freq_hz(), _gyro_harmonic_notch_bandwidth_hz(), _gyro_harmonic_notch_attenuation_dB());
-        _last_harmonic_notch_center_freq_hz = _gyro_harmonic_notch_center_freq_hz();
-        _last_harmonic_notch_bandwidth_hz = _gyro_harmonic_notch_bandwidth_hz();
-        _last_harmonic_notch_attenuation_dB = _gyro_harmonic_notch_attenuation_dB();
-    }
-    else if (!is_equal(_last_harmonic_notch_center_freq_hz, _gyro_harmonic_notch_center_freq_hz())) {
-        _imu._gyro_harmonic_notch_filter[instance].update(_gyro_harmonic_notch_center_freq_hz());
-        _last_harmonic_notch_center_freq_hz = _gyro_harmonic_notch_center_freq_hz();
+    if (!is_equal(_last_harmonic_notch_bandwidth_hz, gyro_harmonic_notch_bandwidth_hz()) ||
+        !is_equal(_last_harmonic_notch_attenuation_dB, gyro_harmonic_notch_attenuation_dB())) {
+        _imu._gyro_harmonic_notch_filter[instance].init(_gyro_raw_sample_rate(instance), gyro_harmonic_notch_center_freq_hz(), gyro_harmonic_notch_bandwidth_hz(), gyro_harmonic_notch_attenuation_dB());
+        _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
+        _last_harmonic_notch_bandwidth_hz = gyro_harmonic_notch_bandwidth_hz();
+        _last_harmonic_notch_attenuation_dB = gyro_harmonic_notch_attenuation_dB();
+    } else if (!is_equal(_last_harmonic_notch_center_freq_hz, gyro_harmonic_notch_center_freq_hz())) {
+        _imu._gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz());
+        _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
     }
     // possily update the notch filter parameters
     if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -244,15 +244,15 @@ protected:
     uint8_t _gyro_notch_enabled(void) const { return _imu._notch_filter.enabled(); }
 
     // return the harmonic notch filter center in Hz for the sample rate
-    float _gyro_harmonic_notch_center_freq_hz() const { return _imu._calculated_harmonic_notch_freq_hz; }
+    float gyro_harmonic_notch_center_freq_hz() const { return _imu._calculated_harmonic_notch_freq_hz; }
 
     // return the harmonic notch filter bandwidth in Hz for the sample rate
-    float _gyro_harmonic_notch_bandwidth_hz(void) const { return _imu._harmonic_notch_filter.bandwidth_hz(); }
+    float gyro_harmonic_notch_bandwidth_hz(void) const { return _imu._harmonic_notch_filter.bandwidth_hz(); }
 
     // return the harmonic notch filter attenuation in dB for the sample rate
-    float _gyro_harmonic_notch_attenuation_dB(void) const { return _imu._harmonic_notch_filter.attenuation_dB(); }
+    float gyro_harmonic_notch_attenuation_dB(void) const { return _imu._harmonic_notch_filter.attenuation_dB(); }
 
-    uint8_t _gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
+    uint8_t gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
 
     // common gyro update function for all backends
     void update_gyro(uint8_t instance);