Prevent phase wrap.

src/impedance.rs

@@ -12,6 +12,7 @@ use static_cell::StaticCell;
 use heapless::Vec;
 
 use num::complex::Complex;
+use core::f32::consts::PI;
 
 use crate::ad5940::*;
 use crate::ad5940_registers::*;
@@ -911,7 +912,15 @@ pub fn calculate_impedance_2_lead(data: [u32; 4]) -> ImpedanceResult {
     let (rz_mag, rz_phase) = dft_rz.to_polar();
 
     let magnitude = (rcal_mag / rz_mag) * RCAL;
-    let phase = rcal_phase - rz_phase;
+    let mut phase = rcal_phase - rz_phase;
+
+    // Normalize phase to [-π, π]
+    if phase > PI {
+        phase -= 2.0 * PI;
+    } 
+    else if phase < -PI {
+        phase += 2.0 * PI;
+    }
 
     ImpedanceResult { magnitude, phase }
 }
@@ -940,8 +949,16 @@ pub fn calculate_impedance_4_lead(data: [u32; 4], rtia: &RtiaCalibrationResult)
 
     // Calculate impedance using calibrated Rtia
     let magnitude = volt_mag / curr_mag * rtia.magnitude;
-    let phase = volt_phase - curr_phase + rtia.phase;
+    let mut phase = volt_phase - curr_phase + rtia.phase;
 
+    // Normalize phase to [-π, π]
+    if phase > PI {
+        phase -= 2.0 * PI;
+    } 
+    else if phase < -PI {
+        phase += 2.0 * PI;
+    }
+    
     let data = ImpedanceResult {
         magnitude,
         phase