Added CTCSS decoder in NFM RX

RSSI output is now pitch instead of PWM
Disabled RSSI output in WBFM mode

firmware/baseband/proc_nfm_audio.cpp

@@ -21,6 +21,7 @@
  */
 
 #include "proc_nfm_audio.hpp"
+#include "sine_table_int8.hpp"
 #include "portapack_shared_memory.hpp"
 
 #include "event_m4.hpp"
@@ -29,7 +30,7 @@
 #include <cstddef>
 
 void NarrowbandFMAudio::execute(const buffer_c8_t& buffer) {
-	bool new_state;
+	//bool new_state;
 	
 	if( !configured ) {
 		return;
@@ -42,30 +43,63 @@ void NarrowbandFMAudio::execute(const buffer_c8_t& buffer) {
 	feed_channel_stats(channel_out);
 	channel_spectrum.feed(channel_out, channel_filter_pass_f, channel_filter_stop_f);
 
-	if (!pwmrssi_enabled) {
+	if (!pitch_rssi_enabled) {
+		// Normal mode, output demodulated audio
 		auto audio = demod.execute(channel_out, audio_buffer);
 		audio_output.write(audio);
-	} else {
-		for (c = 0; c < 32; c++) {
-			if (synth_acc < pwmrssi_avg)
-				pwmrssi_audio_buffer.p[c] = 32767;
-			else
-				pwmrssi_audio_buffer.p[c] = -32768;
-			
-			if (synth_acc < synth_div)
-				synth_acc++;
-			else
-				synth_acc = 0;
-		}
-
-		audio_output.write(pwmrssi_audio_buffer);
 		
-		new_state = audio_output.is_squelched();
+		if (ctcss_detect_enabled) {
+			/* 24kHz int16_t[16]
+			 * -> FIR filter, <300Hz pass, >300Hz stop, gain of 1
+			 * -> 12kHz int16_t[8] */
+			auto audio_ctcss = ctcss_filter.execute(audio, work_audio_buffer);
+			
+			// s16 to f32 for hpf
+			std::array<float, 8> audio_f;
+			for (size_t i = 0; i < audio_ctcss.count; i++) {
+				audio_f[i] = audio_ctcss.p[i] * ki;
+			}
+			
+			hpf.execute_in_place(buffer_f32_t {
+				audio_f.data(),
+				audio_ctcss.count,
+				audio_ctcss.sampling_rate
+				});
+			
+			// Zero-crossing detection
+			for (size_t c = 0; c < audio_ctcss.count; c++) {
+				cur_sample = audio_f[c];
+				if (cur_sample * prev_sample < 0.0) {
+					z_acc += z_timer;
+					z_timer = 0;
+					z_count++;
+				} else
+					z_timer++;
+				prev_sample = cur_sample;
+			}
+			
+			if (z_count >= 30) {
+				ctcss_message.value = (100 * 12000 / 2 * z_count) / z_acc;
+				shared_memory.application_queue.push(ctcss_message);
+				z_count = 0;
+				z_acc = 0;
+			}
+		}
+	} else {
+		// Direction-finding mode; output tone with pitch related to RSSI
+		for (size_t c = 0; c < 16; c++) {
+			tone_buffer.p[c] = (sine_table_i8[(tone_phase & 0xFF000000U) >> 24]) * 128;
+			tone_phase += tone_delta;
+		}
+		
+		audio_output.write(tone_buffer);
+		
+		/*new_state = audio_output.is_squelched();
 		
 		if (new_state && !old_state)
 			shared_memory.application_queue.push(sig_message);
 		
-		old_state = new_state;	
+		old_state = new_state;*/	
 	}
 }
 
@@ -84,8 +118,8 @@ void NarrowbandFMAudio::on_message(const Message* const message) {
 		capture_config(*reinterpret_cast<const CaptureConfigMessage*>(message));
 		break;
 	
-	case Message::ID::PWMRSSIConfigure:
-		pwmrssi_config(*reinterpret_cast<const PWMRSSIConfigureMessage*>(message));
+	case Message::ID::PitchRSSIConfigure:
+		pitch_rssi_config(*reinterpret_cast<const PitchRSSIConfigureMessage*>(message));
 		break;
 		
 	default:
@@ -114,16 +148,15 @@ void NarrowbandFMAudio::configure(const NBFMConfigureMessage& message) {
 	channel_spectrum.set_decimation_factor(std::floor(channel_filter_output_fs / (channel_filter_pass_f + channel_filter_stop_f)));
 	audio_output.configure(message.audio_hpf_config, message.audio_deemph_config, (float)message.squelch_level / 100.0);
 	
-	synth_acc = 0;
-	
+	hpf.configure(audio_24k_hpf_30hz_config);
+	ctcss_filter.configure(taps_64_lp_025_025.taps);
+
 	configured = true;
 }
 
-void NarrowbandFMAudio::pwmrssi_config(const PWMRSSIConfigureMessage& message) {
-	pwmrssi_enabled = message.enabled;
-	pwmrssi_avg = message.avg / 3;
-	synth_div = message.synth_div;
-	synth_acc = 0;
+void NarrowbandFMAudio::pitch_rssi_config(const PitchRSSIConfigureMessage& message) {
+	pitch_rssi_enabled = message.enabled;
+	tone_delta = (message.rssi + 1000) * ((1ULL << 32) / 24000);
 }
 
 void NarrowbandFMAudio::capture_config(const CaptureConfigMessage& message) {

firmware/baseband/proc_nfm_audio.hpp

@@ -29,6 +29,7 @@
 
 #include "dsp_decimate.hpp"
 #include "dsp_demodulate.hpp"
+#include "dsp_iir.hpp"
 
 #include "audio_output.hpp"
 #include "spectrum_collector.hpp"
@@ -52,16 +53,21 @@ private:
 		dst.data(),
 		dst.size()
 	};
-	std::array<float, 32> audio { };
-	const buffer_f32_t audio_buffer {
-		audio.data(),
-		audio.size()
+	const buffer_s16_t work_audio_buffer {
+		(int16_t*)dst.data(),
+		sizeof(dst) / sizeof(int16_t)
 	};
 	
-	std::array<int16_t, 32> pwm { };
-	const buffer_s16_t pwmrssi_audio_buffer {
-		(int16_t*)pwm.data(),
-		sizeof(pwm) / sizeof(int16_t)
+	std::array<int16_t, 16> audio { };
+	const buffer_s16_t audio_buffer {
+		(int16_t*)audio.data(),
+		sizeof(audio) / sizeof(int16_t)
+	};
+	
+	std::array<int16_t, 16> tone { };
+	const buffer_s16_t tone_buffer {
+		(int16_t*)tone.data(),
+		sizeof(tone) / sizeof(int16_t)
 	};
 
 	dsp::decimate::FIRC8xR16x24FS4Decim8 decim_0 { };
@@ -69,25 +75,34 @@ private:
 	dsp::decimate::FIRAndDecimateComplex channel_filter { };
 	uint32_t channel_filter_pass_f = 0;
 	uint32_t channel_filter_stop_f = 0;
+	
+	// For CTCSS decoding
+	dsp::decimate::FIR64AndDecimateBy2Real ctcss_filter { };
+	IIRBiquadFilter hpf { };
 
 	dsp::demodulate::FM demod { };
 
 	AudioOutput audio_output { };
-	bool old_state { };
 
 	SpectrumCollector channel_spectrum { };
 	
-	unsigned int c { 0 }, synth_acc { 0 };
-	uint32_t synth_div { 0 };
-	bool pwmrssi_enabled = false;
-	uint32_t pwmrssi_avg { 0 };
+	uint32_t tone_phase { 0 };
+	uint32_t tone_delta { 0 };
+	bool pitch_rssi_enabled { false };
+	
+	float cur_sample { }, prev_sample { };
+	uint32_t z_acc { 0}, z_timer { 0 }, z_count { 0 };
+	bool ctcss_detect_enabled { true };
+	static constexpr float k = 32768.0f;
+	static constexpr float ki = 1.0f / k;
 
 	bool configured { false };
-	void pwmrssi_config(const PWMRSSIConfigureMessage& message);
+	void pitch_rssi_config(const PitchRSSIConfigureMessage& message);
 	void configure(const NBFMConfigureMessage& message);
 	void capture_config(const CaptureConfigMessage& message);
 	
-	RequestSignalMessage sig_message { RequestSignalMessage::Signal::Squelched };
+	//RequestSignalMessage sig_message { RequestSignalMessage::Signal::Squelched };
+	CodedSquelchMessage ctcss_message { 0 };
 };
 
 #endif/*__PROC_NFM_AUDIO_H__*/

firmware/baseband/proc_wfm_audio.cpp

@@ -45,49 +45,32 @@ void WidebandFMAudio::execute(const buffer_c8_t& buffer) {
 		channel_spectrum.feed(channel, channel_filter_pass_f, channel_filter_stop_f);
 	}
 
-	if ( !pwmrssi_enabled ) {
-		/* 384kHz complex<int16_t>[256]
-		 * -> FM demodulation
-		 * -> 384kHz int16_t[256] */
-		/* TODO: To improve adjacent channel rejection, implement complex channel filter:
-		 *		pass < +/- 100kHz, stop > +/- 200kHz
-		 */
+	/* 384kHz complex<int16_t>[256]
+	 * -> FM demodulation
+	 * -> 384kHz int16_t[256] */
+	/* TODO: To improve adjacent channel rejection, implement complex channel filter:
+	 *		pass < +/- 100kHz, stop > +/- 200kHz
+	 */
 
-		auto audio_oversampled = demod.execute(channel, work_audio_buffer);
+	auto audio_oversampled = demod.execute(channel, work_audio_buffer);
 
-		/* 384kHz int16_t[256]
-		 * -> 4th order CIC decimation by 2, gain of 1
-		 * -> 192kHz int16_t[128] */
-		auto audio_4fs = audio_dec_1.execute(audio_oversampled, work_audio_buffer);
+	/* 384kHz int16_t[256]
+	 * -> 4th order CIC decimation by 2, gain of 1
+	 * -> 192kHz int16_t[128] */
+	auto audio_4fs = audio_dec_1.execute(audio_oversampled, work_audio_buffer);
 
-		/* 192kHz int16_t[128]
-		 * -> 4th order CIC decimation by 2, gain of 1
-		 * -> 96kHz int16_t[64] */
-		auto audio_2fs = audio_dec_2.execute(audio_4fs, work_audio_buffer);
+	/* 192kHz int16_t[128]
+	 * -> 4th order CIC decimation by 2, gain of 1
+	 * -> 96kHz int16_t[64] */
+	auto audio_2fs = audio_dec_2.execute(audio_4fs, work_audio_buffer);
 
-		/* 96kHz int16_t[64]
-		 * -> FIR filter, <15kHz (0.156fs) pass, >19kHz (0.198fs) stop, gain of 1
-		 * -> 48kHz int16_t[32] */
-		auto audio = audio_filter.execute(audio_2fs, work_audio_buffer);
+	/* 96kHz int16_t[64]
+	 * -> FIR filter, <15kHz (0.156fs) pass, >19kHz (0.198fs) stop, gain of 1
+	 * -> 48kHz int16_t[32] */
+	auto audio = audio_filter.execute(audio_2fs, work_audio_buffer);
 
-		/* -> 48kHz int16_t[32] */
-		audio_output.write(audio);
-	} else {
-		for (c = 0; c < 32; c++) {
-			if (synth_acc < pwmrssi_avg)
-				pwmrssi_audio_buffer.p[c] = 32767;
-			else
-				pwmrssi_audio_buffer.p[c] = -32768;
-			
-			if (synth_acc < synth_div)		// 48kHz / 96 = 500Hz
-				synth_acc++;
-			else
-				synth_acc = 0;
-		}
-
-		/* -> 48kHz int16_t[32] */
-		audio_output.write(pwmrssi_audio_buffer);
-	}
+	/* -> 48kHz int16_t[32] */
+	audio_output.write(audio);
 	
 }
 
@@ -105,10 +88,6 @@ void WidebandFMAudio::on_message(const Message* const message) {
 	case Message::ID::CaptureConfig:
 		capture_config(*reinterpret_cast<const CaptureConfigMessage*>(message));
 		break;
-	
-	case Message::ID::PWMRSSIConfigure:
-		pwmrssi_config(*reinterpret_cast<const PWMRSSIConfigureMessage*>(message));
-		break;
 		
 	default:
 		break;
@@ -137,18 +116,9 @@ void WidebandFMAudio::configure(const WFMConfigureMessage& message) {
 
 	channel_spectrum.set_decimation_factor(1);
 
-	synth_acc = 0;
-
 	configured = true;
 }
 
-void WidebandFMAudio::pwmrssi_config(const PWMRSSIConfigureMessage& message) {
-	pwmrssi_enabled = message.enabled;
-	pwmrssi_avg = message.avg;
-	synth_div = message.synth_div;
-	synth_acc = 0;
-}
-
 void WidebandFMAudio::capture_config(const CaptureConfigMessage& message) {
 	if( message.config ) {
 		audio_output.set_stream(std::make_unique<StreamInput>(message.config));

firmware/baseband/proc_wfm_audio.hpp

@@ -55,12 +55,6 @@ private:
 		(int16_t*)dst.data(),
 		sizeof(dst) / sizeof(int16_t)
 	};
-	
-	std::array<int16_t, 32> pwm { };
-	const buffer_s16_t pwmrssi_audio_buffer {
-		(int16_t*)pwm.data(),
-		sizeof(pwm) / sizeof(int16_t)
-	};
 
 	dsp::decimate::FIRC8xR16x24FS4Decim4 decim_0 { };
 	dsp::decimate::FIRC16xR16x16Decim2 decim_1 { };
@@ -77,14 +71,8 @@ private:
 	SpectrumCollector channel_spectrum { };
 	size_t spectrum_interval_samples = 0;
 	size_t spectrum_samples = 0;
-	
-	unsigned int c { 0 }, synth_acc { 0 };
-	uint32_t synth_div { 0 };
-	bool pwmrssi_enabled { false };
-	uint32_t pwmrssi_avg { 0 };
 
 	bool configured { false };
-	void pwmrssi_config(const PWMRSSIConfigureMessage& message);
 	void configure(const WFMConfigureMessage& message);
 	void capture_config(const CaptureConfigMessage& message);
 };