diff --git a/debug.c b/debug.c
index 165a6af..b3b49c5 100644
--- a/debug.c
+++ b/debug.c
@@ -157,6 +157,11 @@ int main(int argc, char **argv)
 		float cnt_freq = fclampf(1200.0 + cargf(cnt_q[out] * conjf(cnt_last)) / (2.0 * M_PI * dstep), 1100.0, 1300.0);
 		float dat_freq = fclampf(1900.0 + cargf(dat_q[out] * conjf(dat_last)) / (2.0 * M_PI * dstep), 1500.0, 2300.0);
 
+		if (cabsf(cnt_q[out]) > cabsf(dat_q[out]))
+			dat_freq = 1500.0;
+		else
+			cnt_freq = 1300.0;
+
 		cnt_last = cnt_q[out];
 		dat_last = dat_q[out];
 
diff --git a/decode.c b/decode.c
index f716758..3d0dc40 100644
--- a/decode.c
+++ b/decode.c
@@ -368,6 +368,11 @@ int demodulate(struct pcm *pcm, float *cnt_freq, float *dat_freq, float *drate)
 	*cnt_freq = fclampf(1200.0 + cargf(cnt_q[out] * conjf(cnt_last)) / (2.0 * M_PI * dstep), 1100.0, 1300.0);
 	*dat_freq = fclampf(1900.0 + cargf(dat_q[out] * conjf(dat_last)) / (2.0 * M_PI * dstep), 1500.0, 2300.0);
 
+	if (cabsf(cnt_q[out]) > cabsf(dat_q[out]))
+		*dat_freq = 1500.0;
+	else
+		*cnt_freq = 1300.0;
+
 	cnt_last = cnt_q[out];
 	dat_last = dat_q[out];