uhd: added meta-range clip and implemented in dboards, fixed step calculation

7 files changed, 82 insertions, 37 deletions

diff --git a/host/include/uhd/types/ranges.hpp b/host/include/uhd/types/ranges.hpp
index 0811fc4c8..206c64726 100644
--- a/host/include/uhd/types/ranges.hpp
+++ b/host/include/uhd/types/ranges.hpp
@@ -93,6 +93,14 @@ namespace uhd{
         //! Get the overall step value for this meta-range.
         const T step(void) const;
 
+        /*!
+         * Clip the target value to a possible range value.
+         * \param value the value to clip to this range
+         * \param clip_step if true, clip to steps as well
+         * \return a value that is in one of the ranges
+         */
+         const T clip(const T &value, bool clip_step = false) const;
+
     };
 
     typedef meta_range_t<float> gain_range_t;
diff --git a/host/include/uhd/types/ranges.ipp b/host/include/uhd/types/ranges.ipp
index cb1628c53..7fd1bf2eb 100644
--- a/host/include/uhd/types/ranges.ipp
+++ b/host/include/uhd/types/ranges.ipp
@@ -19,6 +19,7 @@
 #define INCLUDED_UHD_TYPES_RANGES_IPP
 
 #include <boost/math/special_functions/round.hpp>
+#include <boost/foreach.hpp>
 #include <algorithm>
 #include <stdexcept>
 #include <iostream>
@@ -69,6 +70,21 @@ namespace uhd{
      * meta_range_t implementation code
      ******************************************************************/
 
+    namespace /*anon*/{
+        template <typename T> inline
+        void check_meta_range_monotonic(const meta_range_t<T> &mr){
+            if (mr.empty()){
+                throw std::runtime_error("meta-range cannot be empty");
+            }
+            for (size_t i = 1; i < mr.size(); i++){
+                if (mr.at(i).start() < mr.at(i-1).stop()){
+                    throw std::runtime_error("meta-range is not monotonic");
+                }
+            }
+        }
+    } //namespace /*anon*/
+
+
     template <typename T> meta_range_t<T>::meta_range_t(void){
         /* NOP */
     }
@@ -91,42 +107,60 @@ namespace uhd{
     }
 
     template <typename T> const T meta_range_t<T>::start(void) const{
-        if (this->empty()){
-            throw std::runtime_error("cannot calculate overall start on empty meta-range");
-        }
-        T min_start = this->at(0).start();
-        for (size_t i = 1; i < this->size(); i++){
-            min_start = std::min(min_start, this->at(i).start());
+        check_meta_range_monotonic(*this);
+        T min_start = this->front().start();
+        BOOST_FOREACH(const range_t<T> &r, (*this)){
+            min_start = std::min(min_start, r.start());
         }
         return min_start;
     }
 
     template <typename T> const T meta_range_t<T>::stop(void) const{
-        if (this->empty()){
-            throw std::runtime_error("cannot calculate overall stop on empty meta-range");
-        }
-        T max_stop = this->at(0).stop();
-        for (size_t i = 1; i < this->size(); i++){
-            max_stop = std::max(max_stop, this->at(i).stop());
+        check_meta_range_monotonic(*this);
+        T max_stop = this->front().stop();
+        BOOST_FOREACH(const range_t<T> &r, (*this)){
+            max_stop = std::max(max_stop, r.stop());
         }
         return max_stop;
     }
 
     template <typename T> const T meta_range_t<T>::step(void) const{
-        if (this->empty()){
-            throw std::runtime_error("cannot calculate overall step on empty meta-range");
+        check_meta_range_monotonic(*this);
+        std::vector<T> non_zero_steps;
+        range_t<T> last = this->front();
+        BOOST_FOREACH(const range_t<T> &r, (*this)){
+            //steps at each range
+            if (r.step() != T(0)) non_zero_steps.push_back(r.step());
+            //and steps in-between ranges
+            T ibtw_step = r.start() - last.stop();
+            if (ibtw_step != T(0)) non_zero_steps.push_back(ibtw_step);
+            //store ref to last
+            last = r;
         }
-        T min_step  = this->at(0).step();
-        for (size_t i = 1; i < this->size(); i++){
-            if (this->at(i).start() < this->at(i-1).stop()){
-                throw std::runtime_error("cannot calculate overall range when start(n) < stop(n-1) ");
+        if (non_zero_steps.empty()) return T(0); //all zero steps, its zero...
+        return *std::min_element(non_zero_steps.begin(), non_zero_steps.end());
+    }
+
+    template <typename T> const T meta_range_t<T>::clip(
+        const T &value, bool clip_step
+    ) const{
+        check_meta_range_monotonic(*this);
+        T last_stop = this->front().stop();
+        BOOST_FOREACH(const range_t<T> &r, (*this)){
+            //in-between ranges, clip to nearest
+            if (value < r.start()){
+                return (std::abs(value - r.start()) < std::abs(value - last_stop))?
+                    r.start() : last_stop;
             }
-            if (this->at(i).start() != this->at(i).stop()){
-                min_step = std::min(min_step, this->at(i).step());
+            //in this range, clip here
+            if (value <= r.stop()){
+                if (not clip_step or r.step() == T(0)) return value;
+                return boost::math::round((value - r.start())/r.step())*r.step() + r.start();
             }
-            min_step = std::min(min_step, this->at(i).start() - this->at(i-1).stop());
+            //continue on to the next range
+            last_stop = r.stop();
         }
-        return min_step;
+        return last_stop;
     }
 
 } //namespace uhd
diff --git a/host/lib/usrp/dboard/db_dbsrx.cpp b/host/lib/usrp/dboard/db_dbsrx.cpp
index 70bf03158..5316fb952 100644
--- a/host/lib/usrp/dboard/db_dbsrx.cpp
+++ b/host/lib/usrp/dboard/db_dbsrx.cpp
@@ -229,7 +229,7 @@ dbsrx::~dbsrx(void){
  * Tuning
  **********************************************************************/
 void dbsrx::set_lo_freq(double target_freq){
-    target_freq = std::clip(target_freq, dbsrx_freq_range.start(), dbsrx_freq_range.stop());
+    target_freq = dbsrx_freq_range.clip(target_freq);
 
     double actual_freq=0.0, pfd_freq=0.0, ref_clock=0.0;
     int R=0, N=0, r=0, m=0;
@@ -417,7 +417,7 @@ void dbsrx::set_lo_freq(double target_freq){
  */
 static int gain_to_gc2_vga_reg(float &gain){
     int reg = 0;
-    gain = std::clip(gain, dbsrx_gain_ranges["GC2"].start(), dbsrx_gain_ranges["GC2"].stop());
+    gain = dbsrx_gain_ranges["GC2"].clip(gain);
 
     // Half dB steps from 0-5dB, 1dB steps from 5-24dB
     if (gain < 5) {
@@ -443,7 +443,7 @@ static int gain_to_gc2_vga_reg(float &gain){
  */
 static float gain_to_gc1_rfvga_dac(float &gain){
     //clip the input
-    gain = std::clip(gain, dbsrx_gain_ranges["GC1"].start(), dbsrx_gain_ranges["GC1"].stop());
+    gain = dbsrx_gain_ranges["GC1"].clip(gain);
 
     //voltage level constants
     static const float max_volts = float(1.2), min_volts = float(2.7);
diff --git a/host/lib/usrp/dboard/db_rfx.cpp b/host/lib/usrp/dboard/db_rfx.cpp
index bb9e19708..4e73fb3a3 100644
--- a/host/lib/usrp/dboard/db_rfx.cpp
+++ b/host/lib/usrp/dboard/db_rfx.cpp
@@ -292,7 +292,7 @@ double rfx_xcvr::set_lo_freq(
     ) % (target_freq/1e6) << std::endl;
 
     //clip the input
-    target_freq = std::clip(target_freq, _freq_range.start(), _freq_range.stop());
+    target_freq = _freq_range.clip(target_freq);
     if (_div2[unit]) target_freq *= 2;
 
     //map prescalers to the register enums
diff --git a/host/lib/usrp/dboard/db_tvrx.cpp b/host/lib/usrp/dboard/db_tvrx.cpp
index 94ab86898..17fdad74a 100644
--- a/host/lib/usrp/dboard/db_tvrx.cpp
+++ b/host/lib/usrp/dboard/db_tvrx.cpp
@@ -277,7 +277,7 @@ static double gain_interp(double gain, boost::array<double, 17> db_vector, boost
 
 static float rf_gain_to_voltage(float gain, double lo_freq){
     //clip the input
-    gain = std::clip(gain, get_tvrx_gain_ranges()["RF"].start(), get_tvrx_gain_ranges()["RF"].stop());
+    gain = get_tvrx_gain_ranges()["RF"].clip(gain);
 
     //first we need to find out what band we're in, because gains are different across different bands
     std::string band = get_band(lo_freq + tvrx_if_freq);
@@ -305,7 +305,7 @@ static float rf_gain_to_voltage(float gain, double lo_freq){
 
 static float if_gain_to_voltage(float gain){
     //clip the input
-    gain = std::clip(gain, get_tvrx_gain_ranges()["IF"].start(), get_tvrx_gain_ranges()["IF"].stop());
+    gain = get_tvrx_gain_ranges()["IF"].clip(gain);
 
     double gain_volts = gain_interp(gain, tvrx_if_gains_db, tvrx_gains_volts);
     double dac_volts = gain_volts / opamp_gain;
@@ -337,7 +337,7 @@ void tvrx::set_gain(float gain, const std::string &name){
  */
 
 void tvrx::set_freq(double freq) {
-    freq = std::clip(freq, tvrx_freq_range.start(), tvrx_freq_range.stop());
+    freq = tvrx_freq_range.clip(freq);
     std::string prev_band = get_band(_lo_freq - tvrx_if_freq);
     std::string new_band = get_band(freq);
 
diff --git a/host/lib/usrp/dboard/db_wbx.cpp b/host/lib/usrp/dboard/db_wbx.cpp
index d68cd4eaa..dd5bd600b 100644
--- a/host/lib/usrp/dboard/db_wbx.cpp
+++ b/host/lib/usrp/dboard/db_wbx.cpp
@@ -198,16 +198,15 @@ wbx_xcvr::~wbx_xcvr(void){
  **********************************************************************/
 static int rx_pga0_gain_to_iobits(float &gain){
     //clip the input
-    gain = std::clip<float>(gain, wbx_rx_gain_ranges["PGA0"].start(), wbx_rx_gain_ranges["PGA0"].stop());
+    gain = wbx_rx_gain_ranges["PGA0"].clip(gain);
 
     //convert to attenuation and update iobits for atr
     float attn = wbx_rx_gain_ranges["PGA0"].stop() - gain;
 
     //calculate the attenuation
-    int attn_code = int(floor(attn*2));
+    int attn_code = boost::math::iround(attn*2);
     int iobits = ((~attn_code) << RX_ATTN_SHIFT) & RX_ATTN_MASK;
 
-    
     if (wbx_debug) std::cerr << boost::format(
         "WBX Attenuation: %f dB, Code: %d, IO Bits %x, Mask: %x"
     ) % attn % attn_code % (iobits & RX_ATTN_MASK) % RX_ATTN_MASK << std::endl;
@@ -220,7 +219,7 @@ static int rx_pga0_gain_to_iobits(float &gain){
 
 static float tx_pga0_gain_to_dac_volts(float &gain){
     //clip the input
-    gain = std::clip<float>(gain, wbx_tx_gain_ranges["PGA0"].start(), wbx_tx_gain_ranges["PGA0"].stop());
+    gain = wbx_tx_gain_ranges["PGA0"].clip(gain);
 
     //voltage level constants
     static const float max_volts = float(0.5), min_volts = float(1.4);
@@ -328,7 +327,7 @@ double wbx_xcvr::set_lo_freq(
     ) % (target_freq/1e6) << std::endl;
 
     //clip the input
-    target_freq = std::clip(target_freq, wbx_freq_range.start(), wbx_freq_range.stop());
+    target_freq = wbx_freq_range.clip(target_freq);
 
     //map prescaler setting to mininmum integer divider (N) values (pg.18 prescaler)
     static const uhd::dict<int, int> prescaler_to_min_int_div = map_list_of
diff --git a/host/lib/usrp/dboard/db_xcvr2450.cpp b/host/lib/usrp/dboard/db_xcvr2450.cpp
index d3084b0a0..a3a1e6242 100644
--- a/host/lib/usrp/dboard/db_xcvr2450.cpp
+++ b/host/lib/usrp/dboard/db_xcvr2450.cpp
@@ -84,7 +84,11 @@ static const uhd::dict<std::string, gain_range_t> xcvr_tx_gain_ranges = map_list
     ("BB", gain_range_t(0, 5, 1.5))
 ;
 static const uhd::dict<std::string, gain_range_t> xcvr_rx_gain_ranges = map_list_of
-    ("LNA", gain_range_t(0, 30.5, 15))
+    ("LNA", gain_range_t(list_of
+        (range_t<float>(0))
+        (range_t<float>(15))
+        (range_t<float>(30.5))
+    ))
     ("VGA", gain_range_t(0, 62, 2.0))
 ;
 
@@ -262,8 +266,8 @@ void xcvr2450::update_atr(void){
  **********************************************************************/
 void xcvr2450::set_lo_freq(double target_freq){
 
-    //clip the input to the range (TODO FIXME not right)
-    target_freq = std::clip(target_freq, xcvr_freq_range.start(), xcvr_freq_range.stop());
+    //clip the input to the range
+    target_freq = xcvr_freq_range.clip(target_freq);
 
     //variables used in the calculation below
     double scaler = xcvr2450::is_highband(target_freq)? (4.0/5.0) : (4.0/3.0);