From 2d5c27d3dc4aaf4df4f4bd4673561ab46328611c Mon Sep 17 00:00:00 2001
From: ha7ilm <retzlerandras@gmail.com>
Date: Tue, 14 Mar 2017 09:12:09 +0100
Subject: [PATCH] Added bpsk_costas_loop_cc base

---
 csdr.c    |  2 +-
 libcsdr.c | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/csdr.c b/csdr.c
index 735f8c0..131b165 100644
--- a/csdr.c
+++ b/csdr.c
@@ -2376,7 +2376,7 @@ int main(int argc, char *argv[])
 		timing_recovery_state_t state =	timing_recovery_init(algorithm, decimation, add_q);
 
 		int debug_i=0;
-		state.debug_writefiles = 0;
+		state.debug_writefiles = 1;
 		state.debug_force = !!debug_n; //should remove that later
 		FREAD_C;
 		for(;;)
diff --git a/libcsdr.c b/libcsdr.c
index ebb0502..363d6b3 100644
--- a/libcsdr.c
+++ b/libcsdr.c
@@ -1820,6 +1820,41 @@ char* timing_recovery_get_string_from_algorithm(timing_recovery_algorithm_t algo
 	return "INVALID";
 }
 
+typedef struct bpsk_costas_loop_state_s
+{
+	float pll_alpha;
+	float pll_beta;
+	float pll_iir_temp;
+	float pll_phase;
+} bpsk_costas_loop_state_t;
+
+void init_bpsk_carrier_recovery_cc(bpsk_costas_loop_state_t* state) 
+{ 
+	state->pll_alpha = state->pll_beta = 0; //TODO  <--
+	state->pll_phase = 0;
+	state->pll_iir_temp = 0;
+}
+
+void bpsk_costas_loop_cc(complexf* input, complexf* output, int input_size, bpsk_costas_loop_state_t* state)
+{
+	complexf ejphi; //downconvert with that
+	for(int i=0;i<input_size;i++)
+	{
+		e_powj(&ejphi, -state->pll_phase);
+		cmult(&output[i], &input[i], &ejphi);
+
+		float error = iof(output,i) * qof(output, i);
+		if(error>2) error=2;
+		if(error<-2) error=-2;
+
+		state->pll_phase = error * state->pll_alpha + state->pll_iir_temp;
+		state->pll_iir_temp += error * state->pll_beta;
+
+		while(state->pll_phase>PI) state->pll_phase-=2*PI;
+		while(state->pll_phase<-PI) state->pll_phase+=2*PI;
+	}
+}
+
 
 /*
   _____        _                                            _