NotchFilter push initialization/reset into filter

- this simplifies the reset by allowing a notch filter to reset as
needed
 - improves cascade initialization, on reset each filter will reset
properly from the previous

src/lib/mathlib/math/filter/NotchFilter.hpp

@@ -66,26 +66,24 @@ public:
 	 */
 	inline T apply(const T &sample)
 	{
-		// Direct Form I implementation
-		T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 *
-			   _delay_element_output_2;
+		if (!_initialized) {
+			reset(sample);
+			_initialized = true;
+		}
 
-		// shift inputs
-		_delay_element_2 = _delay_element_1;
-		_delay_element_1 = sample;
-
-		// shift outputs
-		_delay_element_output_2 = _delay_element_output_1;
-		_delay_element_output_1 = output;
-
-		return output;
+		return applyInternal(sample);
 	}
 
 	// Filter array of samples in place using the direct form I
 	inline void applyArray(T samples[], int num_samples)
 	{
+		if (!_initialized) {
+			reset(samples[0]);
+			_initialized = true;
+		}
+
 		for (int n = 0; n < num_samples; n++) {
-			samples[n] = apply(samples[n]);
+			samples[n] = applyInternal(samples[n]);
 		}
 	}
 
@@ -130,6 +128,8 @@ public:
 		_b2 = b[2];
 	}
 
+	void reset() { _initialized = false; }
+
 	void reset(const T &sample)
 	{
 		const T input = isFinite(sample) ? sample : T{};
@@ -140,6 +140,8 @@ public:
 		if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) {
 			_delay_element_output_1 = _delay_element_output_2 = {};
 		}
+
+		_initialized = true;
 	}
 
 	void disable()
@@ -160,9 +162,34 @@ public:
 
 		_a1 = 0.f;
 		_a2 = 0.f;
+
+		_initialized = false;
 	}
 
 protected:
+
+	/**
+	 * Add a new raw value to the filter using the Direct Form I
+	 *
+	 * @return retrieve the filtered result
+	 */
+	inline T applyInternal(const T &sample)
+	{
+		// Direct Form I implementation
+		T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 *
+			   _delay_element_output_2;
+
+		// shift inputs
+		_delay_element_2 = _delay_element_1;
+		_delay_element_1 = sample;
+
+		// shift outputs
+		_delay_element_output_2 = _delay_element_output_1;
+		_delay_element_output_1 = output;
+
+		return output;
+	}
+
 	T _delay_element_1{};
 	T _delay_element_2{};
 	T _delay_element_output_1{};
@@ -179,6 +206,8 @@ protected:
 	float _notch_freq{};
 	float _bandwidth{};
 	float _sample_freq{};
+
+	bool _initialized{false};
 };
 
 /**
@@ -242,7 +271,7 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
 	_a1 = _b1;
 	_a2 = (1.f - alpha) * a0_inv;
 
-	if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) {
+	if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a2)) {
 		disable();
 	}
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -171,7 +171,7 @@ void VehicleAngularVelocity::ResetFilters()
 			// angular velocity notch
 			_notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(),
 					_param_imu_gyro_nf_bw.get());
-			_notch_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis));
+			_notch_filter_velocity[axis].reset();
 
 			// angular acceleration low pass
 			if ((_param_imu_dgyro_cutoff.get() > 0.f)
@@ -548,11 +548,9 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 					}
 
 					if (reset) {
-						const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
-
 						for (int axis = 0; axis < 3; axis++) {
 							for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
-								_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset(reset_angular_velocity(axis));
+								_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset();
 							}
 						}
 
@@ -633,8 +631,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 
 						// force reset if the notch frequency jumps significantly
 						if (force || reset || (notch_freq_diff > bandwidth)) {
-							const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
-							nf.reset(reset_angular_velocity(axis));
+							nf.reset();
 							perf_count(_dynamic_notch_filter_fft_reset_perf);
 						}