CFO & PO

QAM/qam.c

@@ -234,6 +234,86 @@ double complex* concat_preamble_signal(double complex* preamble_mod, int preambl
     return s_concat;
 }
 
+// Coarse Frequency Offset avec préambule avec N = 1 (lag 1)
+void cfo(double complex* s_with_preamble, int N, int L, double Fs, int total_samples, double Fc) {
+    double complex sum = 0;
+    int lag = 5;
+    for (int n = lag; n < L * N; n++) {
+        sum += s_with_preamble[n] * conj(s_with_preamble[n - lag]);
+    }
+    double phi = carg(sum);
+    double f_est = (phi / (2.0 * M_PI * lag)) * Fs;
+    double f_offset = f_est - Fc;
+    printf("CFO estimé : %f Hz \n", f_offset);
+    for (int n = 0; n < total_samples; n++) {
+        s_with_preamble[n] *= cexp(-I * 2.0 * M_PI * f_offset * n / Fs);
+    }
+}
+
+// Fine Frequency Offset (FFO) Correction
+void ffo(qam_system* qam, double complex* s_with_preamble, double complex* preamble_ref, int L, int total_samples) {
+    
+    double complex r_preamble[L];
+    for (int k = 0; k < L; k++) {
+        double complex r = 0;
+        for (int n = 0; n < qam->N; n++) {
+            int idx = k * qam->N + n;
+            r += s_with_preamble[idx] * cexp(-2 * I * M_PI * qam->Fc * ((double)idx / qam->Fs)) / A;
+        }
+        r /= qam->N;
+        r_preamble[k] = r;
+    }
+
+    double complex diff_phase_sum = 0;
+    double Ts_symbole = qam->N / qam->Fs;
+    
+    for (int k = 1; k < L; k++) {
+        double complex error_k = r_preamble[k] * conj(preamble_ref[k]);
+        double complex error_k_minus_1 = r_preamble[k - 1] * conj(preamble_ref[k - 1]);
+        
+        diff_phase_sum += error_k * conj(error_k_minus_1);
+    }
+    
+    double phi_sym = carg(diff_phase_sum);
+    double delta_f_fine_est = phi_sym / (2.0 * M_PI * Ts_symbole);
+
+    printf("FFO estimé (Delta f fine) : %f Hz\n", delta_f_fine_est);
+    
+    for (int n = 0; n < total_samples; n++) {
+        s_with_preamble[n] *= cexp(-I * 2.0 * M_PI * delta_f_fine_est * n / qam->Fs);
+    }
+}
+
+// Phase Offset (PO) Correction
+void po(qam_system* qam, double complex* s_with_preamble, double complex* preamble_ref, int L, int total_samples) {
+    double complex r_preamble[L];
+    
+    for (int k = 0; k < L; k++) {
+        double complex r = 0;
+        for (int n = 0; n < qam->N; n++) {
+            int idx = k * qam->N + n;
+            r += s_with_preamble[idx] * cexp(-2 * I * M_PI * qam->Fc * ((double)idx / qam->Fs)) / A;
+        }
+        r /= qam->N;
+        r_preamble[k] = r;
+    }
+
+    double complex phase_error_sum = 0;
+    for (int k = 0; k < L; k++) {
+        phase_error_sum += r_preamble[k] * conj(preamble_ref[k]);
+    }
+
+    double phi_est = carg(phase_error_sum);
+
+    printf("Phase Offset (PO) estimée : %f radians (soit %f degrés)\n", phi_est, phi_est * 180.0 / M_PI);
+
+    double complex phase_corr = cexp(-I * phi_est);
+
+    for (int n = 0; n < total_samples; n++) {
+        s_with_preamble[n] *= phase_corr; 
+    }
+}
+
 int main () {
     // Initialisation du system qam
     qam_system qam;
@@ -274,22 +354,29 @@ 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, 0);
+    add_noise(s_with_preamble, total_samples, 10);
 
     FILE *fp_ref = fopen("constellation_ref.dat", "w");
     fill_constellation_data(&qam, fp_ref);
     fclose(fp_ref);
 
     // Ajout de dephasage
-    //add_dephasage(s, M_PI / 6.0, total_samples);
+    add_dephasage(s_with_preamble, M_PI / 6.0 , total_samples);
     
     // AJout de decalage de fréquence
-    add_freq(&qam, s_with_preamble, 0, total_samples); 
+    add_freq(&qam, s_with_preamble, 10, total_samples); 
+
+    // Estimation / correction du CFO
+    cfo(s_with_preamble, qam.N, L, qam.Fs, total_samples, qam.Fc);
+    ffo(&qam, s_with_preamble, preamble, L, total_samples);
+
+    // Correction phase
+    po(&qam, s_with_preamble, preamble, L, total_samples);
 
     // Démodulation
     FILE *fp_constel = fopen("constellation.dat", "w");
     uint8_t* output_bits = (uint8_t*)malloc(nb_bits * sizeof(uint8_t));
-    demodulate(&qam, s_mod, nb_symbols, output_bits, fp_constel);
+    demodulate(&qam, s_with_preamble + L * qam.N, nb_symbols, output_bits, fp_constel);
     fclose(fp_constel);
 
     // Reconstruction du texte
@@ -307,7 +394,10 @@ int main () {
     free(input_bits);
     free(output_bits);
     free(symbols);
-    free(s);
+    free(preamble);
+    free(preamble_mod);
+    free(s_with_preamble);
+    free(texte_recup);
     free_constellation(&qam);
 
     return 0;