diff --git a/src/CubicSDR.cpp b/src/CubicSDR.cpp
index 27b1bd1..ac2f5d3 100644
--- a/src/CubicSDR.cpp
+++ b/src/CubicSDR.cpp
@@ -88,7 +88,7 @@ PrimaryGLContext& CubicSDR::GetContext(wxGLCanvas *canvas) {
 
 void CubicSDR::setFrequency(unsigned int freq) {
     frequency = freq;
-    demodulatorTest->getParams().frequency = freq;
+//    demodulatorTest->getParams().frequency = freq;
     SDRThreadCommand command(SDRThreadCommand::SDR_THREAD_CMD_TUNE);
     command.int_value = freq;
     threadCmdQueueSDR->push(command);
diff --git a/src/demod/DemodulatorThread.cpp b/src/demod/DemodulatorThread.cpp
index fe4c6fb..bfdb03d 100644
--- a/src/demod/DemodulatorThread.cpp
+++ b/src/demod/DemodulatorThread.cpp
@@ -3,12 +3,16 @@
 #include <vector>
 
 DemodulatorThread::DemodulatorThread(DemodulatorThreadInputQueue* pQueue) :
-        inputQueue(pQueue), visOutQueue(NULL), terminated(false), initialized(false), audio_resampler(NULL), resample_ratio(1), audio_resample_ratio(1), resampler(NULL), commandQueue(NULL), fir_filter(NULL) {
+        inputQueue(pQueue), visOutQueue(NULL), terminated(false), initialized(false), audio_resampler(NULL), resample_ratio(1), audio_resample_ratio(
+                1), resampler(NULL), commandQueue(NULL), fir_filter(NULL) {
 
     float kf = 0.75;         // modulation factor
     fdem = freqdem_create(kf);
 //    freqdem_print(fdem);
 
+    nco_shift = nco_crcf_create(LIQUID_VCO);
+    shift_freq = 0;
+
 }
 
 void DemodulatorThread::initialize() {
@@ -55,7 +59,6 @@ void DemodulatorThread::initialize() {
     audio_resampler = msresamp_crcf_create(audio_resample_ratio, As);
 //    msresamp_crcf_print(audio_resampler);
 
-
     initialized = true;
 //    std::cout << "inputResampleRate " << params.bandwidth << std::endl;
 
@@ -82,7 +85,7 @@ void DemodulatorThread::threadMain() {
                 DemodulatorThreadCommand command;
                 commandQueue->pop(command);
                 switch (command.cmd) {
-                case DemodulatorThreadCommand::SDR_THREAD_CMD_SETBANDWIDTH:
+                case DemodulatorThreadCommand::SDR_THREAD_CMD_SET_BANDWIDTH:
                     if (command.int_value < 3000) {
                         command.int_value = 3000;
                     }
@@ -92,6 +95,9 @@ void DemodulatorThread::threadMain() {
                     params.bandwidth = command.int_value;
                     paramsChanged = true;
                     break;
+                case DemodulatorThreadCommand::SDR_THREAD_CMD_SET_FREQUENCY:
+                    params.frequency = command.int_value;
+                    break;
                 }
             }
 
@@ -107,17 +113,42 @@ void DemodulatorThread::threadMain() {
             continue;
         }
 
+        // Requested frequency is not center, shift it into the center!
+        if (inp.frequency != params.frequency) {
+            if ((params.frequency - inp.frequency) != shift_freq) {
+                shift_freq = params.frequency - inp.frequency;
+                if (abs(shift_freq) <= (int)((float)(SRATE/2) * 1.5)) {
+                    nco_crcf_set_frequency(nco_shift, (2.0 * M_PI) * (((float)abs(shift_freq)) / ((float) SRATE)));
+                }
+            }
+        }
+
+        if (abs(shift_freq) > (int)((float)(SRATE/2) * 1.5)) {
+            continue;
+        }
+
         std::vector<signed char> *data = &inp.data;
         if (data->size()) {
             liquid_float_complex filtered_input[BUF_SIZE / 2];
 
+            liquid_float_complex x, y, z;
+
             for (int i = 0; i < BUF_SIZE / 2; i++) {
+                if (shift_freq != 0) {
+                    nco_crcf_step(nco_shift);
 
-                liquid_float_complex x;
-                liquid_float_complex y;
+                    z.real = (float) (*data)[i * 2] / 127.0f;
+                    z.imag = (float) (*data)[i * 2 + 1] / 127.0f;
 
-                x.real = (float) (*data)[i * 2] / 127.0f;
-                x.imag = (float) (*data)[i * 2 + 1] / 127.0f;
+                    if (shift_freq < 0) {
+                        nco_crcf_mix_up(nco_shift, z, &x);
+                    } else {
+                        nco_crcf_mix_down(nco_shift, z, &x);
+                    }
+                } else {
+                    x.real = (float) (*data)[i * 2] / 127.0f;
+                    x.imag = (float) (*data)[i * 2 + 1] / 127.0f;
+                }
 
                 firfilt_crcf_push(fir_filter, x);      // push input sample
                 firfilt_crcf_execute(fir_filter, &y);   // compute output
diff --git a/src/demod/DemodulatorThread.h b/src/demod/DemodulatorThread.h
index ef2f158..163ea26 100644
--- a/src/demod/DemodulatorThread.h
+++ b/src/demod/DemodulatorThread.h
@@ -23,7 +23,8 @@ class  DemodulatorThreadCommand {
 public:
     enum  DemodulatorThreadCommandEnum {
         SDR_THREAD_CMD_NULL,
-        SDR_THREAD_CMD_SETBANDWIDTH
+        SDR_THREAD_CMD_SET_BANDWIDTH,
+        SDR_THREAD_CMD_SET_FREQUENCY
     };
 
     DemodulatorThreadCommand() : cmd(cmd), int_value(SDR_THREAD_CMD_NULL) {
@@ -149,6 +150,9 @@ protected:
     DemodulatorThreadParameters last_params;
 
     freqdem fdem;
+    nco_crcf nco_shift;
+    int shift_freq;
+
 
     std::atomic<bool> terminated;
     std::atomic<bool> initialized;
diff --git a/src/visual/WaterfallCanvas.cpp b/src/visual/WaterfallCanvas.cpp
index 0b1f4fe..cdf89ff 100644
--- a/src/visual/WaterfallCanvas.cpp
+++ b/src/visual/WaterfallCanvas.cpp
@@ -199,7 +199,7 @@ void WaterfallCanvas::mouseMoved(wxMouseEvent& event) {
             }
 
             DemodulatorThreadCommand command;
-            command.cmd = DemodulatorThreadCommand::SDR_THREAD_CMD_SETBANDWIDTH;
+            command.cmd = DemodulatorThreadCommand::SDR_THREAD_CMD_SET_BANDWIDTH;
             demodBW = demodBW - bwDiff;
             if (demodBW < 1000) {
                 demodBW = 1000;
@@ -245,11 +245,17 @@ void WaterfallCanvas::mouseReleased(wxMouseEvent& event) {
 
         float pos = mTracker.getMouseX();
 
-        int freq = wxGetApp().getFrequency();
+        int center_freq = wxGetApp().getFrequency();
 
-        freq += (pos - 0.5) * SRATE;
+        DemodulatorInstance *demod = wxGetApp().getDemodTest();
 
-        wxGetApp().setFrequency(freq);
+        int freq = center_freq - (int)(0.5 * (float)SRATE) + (int)((float)pos * (float)SRATE);
+
+        DemodulatorThreadCommand command;
+        command.cmd = DemodulatorThreadCommand::SDR_THREAD_CMD_SET_FREQUENCY;
+        command.int_value = freq;
+
+        demod->getCommandQueue()->push(command);
 
         ((wxFrame*) parent)->GetStatusBar()->SetStatusText(
                 wxString::Format(wxT("Set center frequency: %s"),