PLL

QAM/qam.c

@@ -314,6 +314,56 @@ void po(qam_system* qam, double complex* s_with_preamble, double complex* preamb
     }
 }
 
+// Costas loop
+void pll(qam_system* qam, double complex* s_with_preamble, double complex* r_corr, int L, int total_samples, int nb_symbols, double Kp, double Ki, double alpha, FILE* fp_error) {
+    double phase_est = 0.0;
+    double integrator = 0.0;
+    double filtered_error = 0.0;
+
+    int start_data_idx = L * qam->N;
+    int N = qam->N;
+
+    for (int k = 0; k < nb_symbols; k++) {
+        double complex r_k = 0;
+        for (int n = 0; n < N; n++) {
+            int idx = start_data_idx + k * N + n;
+            double t = (double)idx / qam->Fs;
+            r_k += s_with_preamble[idx] * cexp(-2.0 * I * M_PI * qam->Fc * t) / A;
+        }
+        double complex r_symbol = r_k / N;
+
+        r_symbol *= cexp(-I * phase_est);
+
+        double min_dist = INFINITY;
+        double complex closest = 0;
+        for (int idx = 0; idx < qam->M; idx++) {
+            double dist = cabs(r_symbol - qam->constellation[idx]);
+            if (dist < min_dist) {
+                min_dist = dist;
+                closest = qam->constellation[idx];
+            }
+        }
+
+        double error = carg(r_symbol * conj(closest));
+
+        filtered_error = (1.0 - alpha) * filtered_error + alpha * error;
+
+        integrator += Ki * filtered_error;
+        phase_est += Kp * filtered_error + integrator;
+
+        if (fp_error) {
+            fprintf(fp_error, "%d % .8f\n", k, 100 * filtered_error);
+            fflush(fp_error);
+        }
+
+        for (int n = 0; n < N; n++) {
+            int idx = start_data_idx + k * N + n;
+            r_corr[idx] = s_with_preamble[idx] * cexp(-I * phase_est);
+        }
+    }
+}
+
+
 int main () {
     // Initialisation du system qam
     qam_system qam;
@@ -354,7 +404,7 @@ int main () {
     double complex* s_with_preamble = concat_preamble_signal(preamble_mod, L, s_mod, nb_symbols, qam.N);
 
     // Ajout du bruit
-    add_noise(s_with_preamble, total_samples, 10);
+    add_noise(s_with_preamble, total_samples, 24);
 
     FILE *fp_ref = fopen("constellation_ref.dat", "w");
     fill_constellation_data(&qam, fp_ref);
@@ -364,7 +414,7 @@ int main () {
     add_dephasage(s_with_preamble, M_PI / 6.0 , total_samples);
     
     // AJout de decalage de fréquence
-    add_freq(&qam, s_with_preamble, 10, total_samples); 
+    add_freq(&qam, s_with_preamble, 50, total_samples); 
 
     // Estimation / correction du CFO
     cfo(s_with_preamble, qam.N, L, qam.Fs, total_samples, qam.Fc);
@@ -373,10 +423,21 @@ int main () {
     // Correction phase
     po(&qam, s_with_preamble, preamble, L, total_samples);
 
+    // PLL
+    double complex* s_corrected = malloc(sizeof(double complex) * total_samples);
+    double Kp = 0.5;
+    double Ki = 0.01;
+    double alpha = 0.1;
+    FILE *fp_pll_error = fopen("pll_error.dat", "w");
+    pll(&qam, s_with_preamble, s_corrected, L, total_samples, nb_symbols, Kp, Ki, alpha, fp_pll_error);
+    fclose(fp_pll_error);
+
+
+
     // Démodulation
     FILE *fp_constel = fopen("constellation.dat", "w");
     uint8_t* output_bits = (uint8_t*)malloc(nb_bits * sizeof(uint8_t));
-    demodulate(&qam, s_with_preamble + L * qam.N, nb_symbols, output_bits, fp_constel);
+    demodulate(&qam, s_corrected + L * qam.N, nb_symbols, output_bits, fp_constel);
     fclose(fp_constel);
 
     // Reconstruction du texte