uhd: implement frequency correction

4 files changed, 31 insertions, 9 deletions

diff --git a/lib/uhd/uhd_sink_c.cc b/lib/uhd/uhd_sink_c.cc
index 48a3550..b226cbd 100644
--- a/lib/uhd/uhd_sink_c.cc
+++ b/lib/uhd/uhd_sink_c.cc
@@ -39,6 +39,8 @@ uhd_sink_c::uhd_sink_c(const std::string &args) :
     gr_hier_block2("uhd_sink_c",
                    args_to_io_signature(args),
                    gr_make_io_signature (0, 0, 0)),
+    _center_freq(0.0f),
+    _freq_corr(0.0f),
     _lo_offset(0.0f)
 {
   size_t nchan = 1;
@@ -172,13 +174,16 @@ osmosdr::freq_range_t uhd_sink_c::get_freq_range( size_t chan )
 
 double uhd_sink_c::set_center_freq( double freq, size_t chan )
 {
-  //_snk->set_center_freq(freq, chan);
+  #define APPLY_PPM_CORR(val, ppm) ((val) * (1.0 + (ppm) * 0.000001))
 
-  // advanced tuning with tune_request_t
-  uhd::tune_request_t tune_req(freq, _lo_offset);
+  double corr_freq = APPLY_PPM_CORR( freq, _freq_corr );
 
+  // advanced tuning with tune_request_t
+  uhd::tune_request_t tune_req(corr_freq, _lo_offset);
   _snk->set_center_freq(tune_req, chan);
 
+  _center_freq = freq;
+
   return get_center_freq(chan);
 }
 
@@ -189,12 +194,16 @@ double uhd_sink_c::get_center_freq( size_t chan )
 
 double uhd_sink_c::set_freq_corr( double ppm, size_t chan )
 {
+  _freq_corr = ppm;
+
+  set_center_freq( _center_freq );
+
   return get_freq_corr(chan);
 }
 
 double uhd_sink_c::get_freq_corr( size_t chan )
 {
-  return 0; // frequency correction is not supported with UHD
+  return _freq_corr;
 }
 
 std::vector<std::string> uhd_sink_c::get_gain_names( size_t chan )
diff --git a/lib/uhd/uhd_sink_c.h b/lib/uhd/uhd_sink_c.h
index b7872e0..e82d289 100644
--- a/lib/uhd/uhd_sink_c.h
+++ b/lib/uhd/uhd_sink_c.h
@@ -80,6 +80,8 @@ public:
   osmosdr::freq_range_t get_bandwidth_range( size_t chan = 0 );
 
 private:
+  double _center_freq;
+  double _freq_corr;
   double _lo_offset;
   boost::shared_ptr<uhd_usrp_sink> _snk;
 };
diff --git a/lib/uhd/uhd_source_c.cc b/lib/uhd/uhd_source_c.cc
index bb13b26..eefb6ff 100644
--- a/lib/uhd/uhd_source_c.cc
+++ b/lib/uhd/uhd_source_c.cc
@@ -40,6 +40,8 @@ uhd_source_c::uhd_source_c(const std::string &args) :
     gr_hier_block2("uhd_source_c",
                    gr_make_io_signature (0, 0, 0),
                    args_to_io_signature(args)),
+    _center_freq(0.0f),
+    _freq_corr(0.0f),
     _lo_offset(0.0f)
 {
   size_t nchan = 1;
@@ -173,13 +175,16 @@ osmosdr::freq_range_t uhd_source_c::get_freq_range( size_t chan )
 
 double uhd_source_c::set_center_freq( double freq, size_t chan )
 {
-  //_src->set_center_freq(freq, chan);
+  #define APPLY_PPM_CORR(val, ppm) ((val) * (1.0 + (ppm) * 0.000001))
 
-  // advanced tuning with tune_request_t
-  uhd::tune_request_t tune_req(freq, _lo_offset);
+  double corr_freq = APPLY_PPM_CORR( freq, _freq_corr );
 
+  // advanced tuning with tune_request_t
+  uhd::tune_request_t tune_req(corr_freq, _lo_offset);
   _src->set_center_freq(tune_req, chan);
 
+  _center_freq = freq;
+
   return get_center_freq(chan);
 }
 
@@ -190,12 +195,16 @@ double uhd_source_c::get_center_freq( size_t chan )
 
 double uhd_source_c::set_freq_corr( double ppm, size_t chan )
 {
-  return get_freq_corr(chan);
+  _freq_corr = ppm;
+
+  set_center_freq( _center_freq );
+
+  return get_freq_corr( chan );
 }
 
 double uhd_source_c::get_freq_corr( size_t chan )
 {
-  return 0; // frequency correction is not supported with UHD
+  return _freq_corr;
 }
 
 std::vector<std::string> uhd_source_c::get_gain_names( size_t chan )
diff --git a/lib/uhd/uhd_source_c.h b/lib/uhd/uhd_source_c.h
index eebc901..cb11008 100644
--- a/lib/uhd/uhd_source_c.h
+++ b/lib/uhd/uhd_source_c.h
@@ -82,6 +82,8 @@ public:
   osmosdr::freq_range_t get_bandwidth_range( size_t chan = 0 );
 
 private:
+  double _center_freq;
+  double _freq_corr;
   double _lo_offset;
   boost::shared_ptr<uhd_usrp_source> _src;
 };