Added a costas loop algorithm

libcsdr.c

@@ -1909,36 +1909,44 @@ char* timing_recovery_get_string_from_algorithm(timing_recovery_algorithm_t algo
 
 typedef struct bpsk_costas_loop_state_s
 {
-	float pll_alpha;
-	float pll_beta;
-	float pll_iir_temp;
-	float pll_phase;
+	float rc_filter_alpha;
+	float vco_phase_addition_multiplier;
+	float vco_phase;
+	float last_lpfi_output;
+	float last_lpfq_output;
 } bpsk_costas_loop_state_t;
 
-void init_bpsk_carrier_recovery_cc(bpsk_costas_loop_state_t* state) 
+void init_bpsk_costas_loop_cc(bpsk_costas_loop_state_t* state, float samples_per_bits) 
 { 
-	state->pll_alpha = state->pll_beta = 0; //TODO  <--
-	state->pll_phase = 0;
-	state->pll_iir_temp = 0;
+	state->vco_phase = 0;
+	float virtual_sampling_rate = 10000;
+	float virtual_data_rate = virtual_sampling_rate / samples_per_bits;
+	float rc_filter_cutoff = virtual_data_rate/2;
+	float rc_filter_rc = 1/(2*M_PI*rc_filter_cutoff); //as of Equation 24 in Feigin
+	float virtual_sampling_dt = 1.0/virtual_sampling_rate;
+	state->rc_filter_alpha = virtual_sampling_dt/(rc_filter_rc+virtual_sampling_dt); //https://en.wikipedia.org/wiki/Low-pass_filter
+	float rc_filter_omega_cutoff = 2*M_PI*rc_filter_cutoff;
+	state->vco_phase_addition_multiplier = 8*rc_filter_omega_cutoff; //as of Equation 25 in Feigin, assuming input signal amplitude of 1 (to 1V) and (state->vco_phase_addition_multiplier*<vco_input>), a value in radians, will be added to the vco_phase directly.
 }
 
 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;
+		float input_phase = atan2(input[i].q, input[i].i);
+		float input_and_vco_mixed_phase = input_phase - state->vco_phase;
+		complexf input_and_vco_mixed_sample; 
+		e_powj(&input_and_vco_mixed_sample, input_and_vco_mixed_phase);
+		float loop_output_i = 
+			input_and_vco_mixed_sample.i * state->rc_filter_alpha + state->last_lpfi_output * (1-state->rc_filter_alpha);
+		float loop_output_q = 
+			input_and_vco_mixed_sample.q * state->rc_filter_alpha + state->last_lpfq_output * (1-state->rc_filter_alpha);
+		state->last_lpfi_output = loop_output_i;
+		state->last_lpfq_output = loop_output_q;
+		float vco_phase_addition = loop_output_i * loop_output_q * state->vco_phase_addition_multiplier;
+		state->vco_phase += vco_phase_addition;
+		while(state->vco_phase>PI) state->vco_phase-=2*PI;
+		while(state->vco_phase<-PI) state->vco_phase+=2*PI;
 	}
 }
 
@@ -2029,7 +2037,6 @@ void convert_s24_f(unsigned char* input, float* output, int input_size, int bige
 	}
 }
 
-
 int trivial_vectorize()
 {
 	//this function is trivial to vectorize and should pass on both NEON and SSE