From 1699c50676485417da65bcd4c0814e8ddeeb58e0 Mon Sep 17 00:00:00 2001
From: "Charles J. Cliffe" <cj@cubicproductions.com>
Date: Thu, 7 Jan 2016 00:35:02 -0500
Subject: [PATCH] Internal frequency locking support

---
 src/CubicSDR.cpp           | 19 +++++++++++++++++++
 src/CubicSDR.h             |  6 ++++++
 src/sdr/SoapySDRThread.cpp | 27 ++++++++++++++++++++++++++-
 src/sdr/SoapySDRThread.h   |  8 ++++++--
 4 files changed, 57 insertions(+), 3 deletions(-)

diff --git a/src/CubicSDR.cpp b/src/CubicSDR.cpp
index 2555e9a..c97638b 100644
--- a/src/CubicSDR.cpp
+++ b/src/CubicSDR.cpp
@@ -449,6 +449,25 @@ long long CubicSDR::getFrequency() {
     return frequency;
 }
 
+
+void CubicSDR::lockFrequency(long long freq) {
+    frequency_locked.store(true);
+    lock_freq.store(freq);
+    
+    if (sdrThread && !sdrThread->isTerminated()) {
+        sdrThread->lockFrequency(freq);
+    }
+}
+
+bool CubicSDR::isFrequencyLocked() {
+    return frequency_locked.load();
+}
+
+void CubicSDR::unlockFrequency() {
+    frequency_locked.store(false);
+    sdrThread->unlockFrequency();
+}
+
 void CubicSDR::setSampleRate(long long rate_in) {
     sampleRate = rate_in;
     sdrThread->setSampleRate(sampleRate);
diff --git a/src/CubicSDR.h b/src/CubicSDR.h
index e188ff7..abace68 100644
--- a/src/CubicSDR.h
+++ b/src/CubicSDR.h
@@ -84,6 +84,10 @@ public:
     
     void setFrequency(long long freq);
     long long getFrequency();
+    
+    void lockFrequency(long long freq);
+    bool isFrequencyLocked();
+    void unlockFrequency();
 
     void setOffset(long long ofs);
     long long getOffset();
@@ -195,6 +199,8 @@ private:
     std::string notifyMessage;
     std::string modulePath;
     std::mutex notify_busy;
+    std::atomic_bool frequency_locked;
+    std::atomic_llong lock_freq;
 #ifdef USE_HAMLIB
     RigThread *rigThread;
     std::thread *t_Rig;
diff --git a/src/sdr/SoapySDRThread.cpp b/src/sdr/SoapySDRThread.cpp
index 08410ca..3e8ad32 100644
--- a/src/sdr/SoapySDRThread.cpp
+++ b/src/sdr/SoapySDRThread.cpp
@@ -33,6 +33,9 @@ SDRThread::SDRThread() : IOThread(), buffers("SDRThreadBuffers") {
     agc_mode_changed.store(false);
     gain_value_changed.store(false);
     setting_value_changed.store(false);
+    frequency_lock_init.store(false);
+    frequency_locked.store(false);
+    lock_freq.store(0);
 }
 
 SDRThread::~SDRThread() {
@@ -241,7 +244,12 @@ void SDRThread::updateSettings() {
     }
     
     if (freq_changed.load()) {
-        device->setFrequency(SOAPY_SDR_RX,0,"RF",frequency.load() - offset.load());
+        if (frequency_locked.load() && !frequency_lock_init.load()) {
+            device->setFrequency(SOAPY_SDR_RX,0,"RF",lock_freq.load());
+            frequency_lock_init.store(true);
+        } else if (!frequency_locked.load()) {
+            device->setFrequency(SOAPY_SDR_RX,0,"RF",frequency.load() - offset.load());
+        }
         freq_changed.store(false);
     }
     
@@ -372,6 +380,23 @@ long long SDRThread::getFrequency() {
     return frequency.load();
 }
 
+void SDRThread::lockFrequency(long long freq) {
+    lock_freq.store(freq);
+    frequency_locked.store(true);
+    frequency_lock_init.store(false);
+    setFrequency(freq);
+}
+
+bool SDRThread::isFrequencyLocked() {
+    return frequency_locked.load();
+}
+
+void SDRThread::unlockFrequency() {
+    frequency_locked.store(false);
+    frequency_lock_init.store(false);
+    freq_changed.store(true);
+}
+
 void SDRThread::setOffset(long long ofs) {
     offset.store(ofs);
     offset_changed.store(true);
diff --git a/src/sdr/SoapySDRThread.h b/src/sdr/SoapySDRThread.h
index 4c21103..978920e 100644
--- a/src/sdr/SoapySDRThread.h
+++ b/src/sdr/SoapySDRThread.h
@@ -60,6 +60,10 @@ public:
     void setFrequency(long long freq);
     long long getFrequency();
     
+    void lockFrequency(long long freq);
+    bool isFrequencyLocked();
+    void unlockFrequency();
+    
     void setOffset(long long ofs);
     long long getOffset();
     
@@ -98,11 +102,11 @@ protected:
     std::map<std::string, bool> settingChanged;
 
     std::atomic<uint32_t> sampleRate;
-    std::atomic_llong frequency, offset;
+    std::atomic_llong frequency, offset, lock_freq;
     std::atomic_int ppm, numElems, numChannels;
     std::atomic_bool hasPPM, hasHardwareDC;
     std::atomic_bool agc_mode, rate_changed, freq_changed, offset_changed,
-        ppm_changed, device_changed, agc_mode_changed, gain_value_changed, setting_value_changed;
+        ppm_changed, device_changed, agc_mode_changed, gain_value_changed, setting_value_changed, frequency_locked, frequency_lock_init;
 
     std::mutex gain_busy;
     std::map<std::string, float> gainValues;