handle multiple CTCSS tones with same power

Author: charlie-foxtrot

.devcontainer/devcontainer.json

@@ -22,7 +22,7 @@
 				"twxs.cmake",
 				"streetsidesoftware.code-spell-checker",
 				"ms-azuretools.vscode-docker",
-				"GitHub.vscode-github-actions"		
+				"GitHub.vscode-github-actions"
 			]
 		}
 	},

src/ctcss.cpp

@@ -146,17 +146,25 @@ void CTCSS::process_audio_sample(const float &sample) {
 
     enough_samples_ = true;
 
-	// if this is sample fills out the window then check if the strongest tone is
-	// the CTCSS tone we are looking for
+	// if this is sample fills out the window then check if one of the "strongest"
+	// tones is the CTCSS tone we are looking for.  NOTE: there can be multiple "strongest"
+	// tones based on floating point math
 	vector<ToneDetectorSet::PowerIndex> tone_powers;
 	float avg_power = powers_.sorted_powers(tone_powers);
-    if (tone_powers[0].freq == ctcss_freq_ && tone_powers[0].power > avg_power) {
+	float ctcss_tone_power = 0.0;
+	for( const auto i:tone_powers) {
+		if (i.freq == ctcss_freq_) {
+			ctcss_tone_power = i.power;
+			break;
+		}
+	}
+	if (ctcss_tone_power == tone_powers[0].power && ctcss_tone_power > avg_power) {
         debug_print("CTCSS tone of %f Hz detected\n", ctcss_freq_);
         has_tone_ = true;
         found_count_++;
     } else {
-        debug_print("CTCSS tone of %f Hz not detected - highest power was %f Hz at %f\n",
-					ctcss_freq_, tone_powers[0].freq, tone_powers[0].power);
+        debug_print("CTCSS tone of %f Hz not detected - highest power was %f Hz at %f vs %f\n",
+					ctcss_freq_, tone_powers[0].freq, tone_powers[0].power, ctcss_tone_power);
         has_tone_ = false;
         not_found_count_++;
     }

src/squelch.cpp

@@ -20,6 +20,7 @@
 #include "squelch.h"
 
 #ifdef DEBUG_SQUELCH
+#include <errno.h>   // errno
 #include <string.h>  // strerror()
 #endif /* DEBUG_SQUELCH _*/
 
@@ -488,6 +489,8 @@ void Squelch::calculate_noise_floor(void) {
 
 	noise_floor_ = noise_floor_ * decay_factor + std::min(pre_filter_.capped_, noise_floor_) * new_factor + 1e-6f;
 
+	debug_print("%zu: noise floor is now %f\n", sample_count_, noise_floor_);
+
 	// Need to update moving_avg_cap_ - depends on noise_floor_
 	calculate_moving_avg_cap();
 
@@ -556,8 +559,8 @@ bool Squelch::currently_flapping(void) const {
 					   ('post_filter_capped', np.single),
 					   ('current_state', np.intc),
 					   ('delay', np.intc),
-					   ('low_signalcount', np.intc)
-					   ('ctcss_fast_has_tone', np.intc)
+					   ('low_signalcount', np.intc),
+					   ('ctcss_fast_has_tone', np.intc),
 					   ('ctcss_slow_has_tone', np.intc)
 					  ])
 

src/test_generate_signal.cpp

@@ -236,7 +236,7 @@ TEST_F(GenerateSignalTest, get_sample_two_tones) {
 	Tone tone1(sample_rate, tone1_freq, tone1_ampl);
 	Tone tone2(sample_rate, tone2_freq, tone2_ampl);
 	for (int i = 0 ; i < 60 * sample_rate ; ++i) {
-		ASSERT_FLOAT_EQ(signal.get_sample(), tone1.get_sample() + tone2.get_sample());
+		ASSERT_NEAR(signal.get_sample(), tone1.get_sample() + tone2.get_sample(), 0.000001);
 	}
 }