From 4d0bef497ba0fb77abb42e39cab624d04bd591ef Mon Sep 17 00:00:00 2001
From: Pedro <ptsmonteiro@gmail.com>
Date: Thu, 30 Nov 2023 10:54:37 +0100
Subject: [PATCH] Deliver frequency offset info to frame handler

---
 modem/demodulator.py      | 25 ++++++++++++++++++++++++-
 modem/frame_dispatcher.py | 10 +++++++---
 2 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/modem/demodulator.py b/modem/demodulator.py
index 4297aa4d..5b0e3399 100644
--- a/modem/demodulator.py
+++ b/modem/demodulator.py
@@ -363,9 +363,32 @@ class Demodulator():
             # data[1] = freedv session
             # data[2] = bytes_per_frame
             # data[3] = snr
-            self.data_queue_received.put([data[0], data[1], data[2], data[3]])
+
+            item = {
+                'payload': data[0],
+                'freedv': data[1],
+                'bytes_per_frame': data[2],
+                'snr': data[3],
+                'frequency_offset': self.get_frequency_offset(data[1]),
+            }
+
+            self.data_queue_received.put(item)
             self.modem_received_queue.task_done()
 
+    def get_frequency_offset(self, freedv: ctypes.c_void_p) -> float:
+        """
+        Ask codec2 for the calculated (audio) frequency offset of the received signal.
+
+        :param freedv: codec2 instance to query
+        :type freedv: ctypes.c_void_p
+        :return: Offset of audio frequency in Hz
+        :rtype: float
+        """
+        modemStats = codec2.MODEMSTATS()
+        codec2.api.freedv_get_modem_extended_stats(freedv, ctypes.byref(modemStats))
+        offset = round(modemStats.foff) * (-1)
+        return offset
+
     def demodulate_audio(
             self,
             audiobuffer: codec2.audio_buffer,
diff --git a/modem/frame_dispatcher.py b/modem/frame_dispatcher.py
index f491e30e..b613c5f4 100644
--- a/modem/frame_dispatcher.py
+++ b/modem/frame_dispatcher.py
@@ -152,10 +152,14 @@ class DISPATCHER():
             # [2] bytes_per_frame
             # [3] snr
             self.new_process_data(
-                bytes_out=data[0], freedv=data[1], bytes_per_frame=data[2], snr=data[3]
+                data['payload'],
+                data['freedv'],
+                data['bytes_per_frame'],
+                data['snr'],
+                data['frequency_offset'],
             )
 
-    def new_process_data(self, bytes_out, freedv, bytes_per_frame: int, snr) -> None:
+    def new_process_data(self, bytes_out, freedv, bytes_per_frame: int, snr, offset) -> None:
         # get frame as dictionary
         deconstructed_frame = self.frame_factory.deconstruct(bytes_out)
         frametype = deconstructed_frame["frame_type_int"]
@@ -174,7 +178,7 @@ class DISPATCHER():
                                 MODEM_TRANSMIT_QUEUE,
                                 self.arq_sessions)
         
-        handler.handle(deconstructed_frame, snr, 0, freedv, bytes_per_frame)
+        handler.handle(deconstructed_frame, snr, offset, freedv, bytes_per_frame)
 
     def old_process_data(self, bytes_out, freedv, bytes_per_frame: int, snr) -> None:
         """