From 4aae77f565aa7bc83be0706bf9c3af2ee2147c0a Mon Sep 17 00:00:00 2001
From: Jared Boone <jboone@earfeast.com>
Date: Thu, 15 Oct 2015 13:31:00 -0700
Subject: [PATCH] MatchedFilter improvements

Use single set of taps for positive and negative filters.
Reverse taps (since new samples are pushed into buffer from the high end).
Make complex multiply explicit to avoid fancy but irrelevant arithmetic checks.
Compute negative filter from conjugation of positive filter taps.
Move filter power and difference calculations into MatchedFilter.
---
 firmware/baseband/matched_filter.cpp | 34 +++++++++++++++++++++++++++-
 firmware/baseband/matched_filter.hpp | 10 ++++----
 firmware/baseband/proc_fsk.cpp       | 10 ++------
 firmware/baseband/proc_fsk.hpp       |  3 +--
 4 files changed, 41 insertions(+), 16 deletions(-)

diff --git a/firmware/baseband/matched_filter.cpp b/firmware/baseband/matched_filter.cpp
index 5146a805..48514266 100644
--- a/firmware/baseband/matched_filter.cpp
+++ b/firmware/baseband/matched_filter.cpp
@@ -21,6 +21,9 @@
 
 #include "matched_filter.hpp"
 
+// TODO: Move the fast complex multiply code to another place.
+#include "dsp_fft.hpp"
+
 namespace dsp {
 namespace matched_filter {
 
@@ -31,7 +34,36 @@ bool MatchedFilter::execute_once(
 
 	advance_decimation_phase();
 	if( is_new_decimation_cycle() ) {
-		output = std::inner_product(&samples_[0], &samples_[taps_count_], &taps_[0], sample_t { 0.0f, 0.0f });
+		// const sample_t* first1 = &samples_[0];
+		// const sample_t* const last1 = &samples_[taps_count_];
+		// const sample_t* first2 = &taps_[0];
+
+		float r_n = 0.0f;
+		float i_n = 0.0f;
+		float r_p = 0.0f;
+		float i_p = 0.0f;
+		for(size_t n=0; n<taps_count_; n++) {
+			const auto sample = samples_[n];
+			const auto tap = taps_reversed_[n];
+
+			// N: complex multiple of samples and taps (conjugate, tap.i negated).
+			// P: complex multiply of samples and taps.
+			r_n += sample.real() * tap.real();
+			i_n -= sample.real() * tap.imag();
+			r_n += sample.imag() * tap.imag();
+			i_n += sample.imag() * tap.real();
+
+			r_p += sample.real() * tap.real();
+			i_p += sample.real() * tap.imag();
+			r_p -= sample.imag() * tap.imag();
+			i_p += sample.imag() * tap.real();
+		}
+
+		const auto mag_n = std::sqrt(r_n * r_n + i_n * i_n);
+		const auto mag_p = std::sqrt(r_p * r_p + i_p * i_p);
+		const auto diff = mag_p - mag_n;
+		output = diff;
+
 		shift_by_decimation_factor();
 		return true;
 	} else {
diff --git a/firmware/baseband/matched_filter.hpp b/firmware/baseband/matched_filter.hpp
index 902b743f..e6610515 100644
--- a/firmware/baseband/matched_filter.hpp
+++ b/firmware/baseband/matched_filter.hpp
@@ -49,11 +49,11 @@ public:
 		const size_t taps_count,
 		size_t decimation_factor = 1
 	) : samples_ { std::make_unique<samples_t>(taps_count) },
-		taps_ { std::make_unique<taps_t>(taps_count) },
+		taps_reversed_ { std::make_unique<taps_t>(taps_count) },
 		taps_count_ { taps_count },
 		decimation_factor { decimation_factor }
 	{
-		std::copy(&taps[0], &taps[taps_count], &taps_[0]);
+		std::reverse_copy(&taps[0], &taps[taps_count], &taps_reversed_[0]);
 	}
 
 	template<typename T>
@@ -66,7 +66,7 @@ public:
 
 	bool execute_once(const sample_t input);
 
-	sample_t get_output() const {
+	float get_output() const {
 		return output;
 	}
 
@@ -74,11 +74,11 @@ private:
 	using samples_t = sample_t[];
 
 	const std::unique_ptr<samples_t> samples_;
-	const std::unique_ptr<taps_t> taps_;
+	const std::unique_ptr<taps_t> taps_reversed_;
 	const size_t taps_count_;
 	size_t decimation_factor { 1 };
 	size_t decimation_phase { 0 };
-	sample_t output;
+	float output;
 
 	void shift_by_decimation_factor();
 
diff --git a/firmware/baseband/proc_fsk.cpp b/firmware/baseband/proc_fsk.cpp
index 4500506d..a26c224a 100644
--- a/firmware/baseband/proc_fsk.cpp
+++ b/firmware/baseband/proc_fsk.cpp
@@ -85,14 +85,8 @@ void FSKProcessor::execute(buffer_c8_t buffer) {
 			static_cast<float>(channel.p[i].real()),
 			static_cast<float>(channel.p[i].imag())
 		};
-		mf_0.execute_once(sample);
-		if( mf_1.execute_once(sample) ) {
-			const auto value_0 = mf_0.get_output();
-			const float mag_0 = std::sqrt(value_0.real() * value_0.real() + value_0.imag() * value_0.imag());
-			const auto value_1 = mf_1.get_output();
-			const float mag_1 = std::sqrt(value_1.real() * value_1.real() + value_1.imag() * value_1.imag());
-			const float diff = mag_1 - mag_0;
-			clock_recovery(diff);
+		if( mf.execute_once(sample) ) {
+			clock_recovery(mf.get_output());
 		}
 	}
 
diff --git a/firmware/baseband/proc_fsk.hpp b/firmware/baseband/proc_fsk.hpp
index 7d82ac16..d5669914 100644
--- a/firmware/baseband/proc_fsk.hpp
+++ b/firmware/baseband/proc_fsk.hpp
@@ -57,8 +57,7 @@ private:
 	const fir_taps_real<64>& channel_filter_taps = taps_64_lp_031_070_tfilter;
 	dsp::decimate::FIRAndDecimateBy2Complex<64> channel_filter { channel_filter_taps.taps };
 
-	dsp::matched_filter::MatchedFilter mf_0 { baseband::ais::rrc_taps_8_n, 1 };
-	dsp::matched_filter::MatchedFilter mf_1 { baseband::ais::rrc_taps_8_p, 1 };
+	dsp::matched_filter::MatchedFilter mf { baseband::ais::rrc_taps_8_p, 1 };
 
 	clock_recovery::ClockRecovery<clock_recovery::FixedErrorFilter> clock_recovery {
 		static_cast<float>(sampling_rate / 4),