From 38000f3305af7483e71d1d68dc6577402c0f78f4 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Fri, 18 Aug 2017 12:30:51 +0200
Subject: [PATCH] UDP sink plugin: implemented NFM

---
 plugins/channeltx/udpsink/udpsink.cpp | 40 ++++++++++++++++++++++++---
 plugins/channeltx/udpsink/udpsink.h   |  2 ++
 2 files changed, 38 insertions(+), 4 deletions(-)

diff --git a/plugins/channeltx/udpsink/udpsink.cpp b/plugins/channeltx/udpsink/udpsink.cpp
index 83239f2b5..37cd9e8dd 100644
--- a/plugins/channeltx/udpsink/udpsink.cpp
+++ b/plugins/channeltx/udpsink/udpsink.cpp
@@ -44,6 +44,7 @@ UDPSink::UDPSink(MessageQueue* uiMessageQueue, UDPSinkGUI* udpSinkGUI, BasebandS
     m_squelchOpenCount(0),
     m_squelchCloseCount(0),
     m_squelchThreshold(4800),
+    m_modPhasor(0.0f),
     m_settingsMutex(QMutex::Recursive)
 {
     setObjectName("UDPSink");
@@ -113,11 +114,10 @@ void UDPSink::pull(Sample& sample)
 
 void UDPSink::modulateSample()
 {
-    //Real t;
-    Sample s;
-
-    if (m_running.m_sampleFormat == FormatS16LE)
+    if (m_running.m_sampleFormat == FormatS16LE) // Linear I/Q transponding
     {
+        Sample s;
+
         m_udpHandler.readSample(s);
 
         uint64_t magsq = s.m_real * s.m_real + s.m_imag * s.m_imag;
@@ -138,6 +138,38 @@ void UDPSink::modulateSample()
             m_modSample.imag(0.0f);
         }
     }
+    else if ((m_running.m_sampleFormat == FormatNFMMono) || (m_running.m_sampleFormat == FormatNFM))
+    {
+        FixReal t;
+        Sample s;
+
+        if (m_running.m_sampleFormat == FormatNFMMono)
+        {
+            m_udpHandler.readSample(t);
+        }
+        else
+        {
+            m_udpHandler.readSample(s);
+            t = s.m_real;
+        }
+
+        m_inMovingAverage.feed((t*t)/1073741824.0);
+        m_inMagsq = m_inMovingAverage.average();
+
+        calculateSquelch(m_inMagsq);
+
+        if (m_squelchOpen)
+        {
+            m_modPhasor += (m_running.m_fmDeviation / m_running.m_inputSampleRate) * (t / 32768.0) * M_PI * 2.0f;
+            m_modSample.real(cos(m_modPhasor) * 10362.2f * m_running.m_gain);
+            m_modSample.imag(sin(m_modPhasor) * 10362.2f * m_running.m_gain);
+        }
+        else
+        {
+            m_modSample.real(0.0f);
+            m_modSample.imag(0.0f);
+        }
+    }
     else
     {
         m_modSample.real(0.0f);
diff --git a/plugins/channeltx/udpsink/udpsink.h b/plugins/channeltx/udpsink/udpsink.h
index 0c9dd2b4a..a4cfe0f13 100644
--- a/plugins/channeltx/udpsink/udpsink.h
+++ b/plugins/channeltx/udpsink/udpsink.h
@@ -256,6 +256,8 @@ private:
     int  m_squelchCloseCount;
     int m_squelchThreshold;
 
+    float m_modPhasor; //!< Phasor for FM modulation
+
     QMutex m_settingsMutex;
 
     static const int m_sampleRateAverageItems = 17;