uhd: Added command interface to uhd source, modified command syntax

author: Martin Braun <martin.braun@ettus.com> 2014-07-03 00:11:29 +0200
committer: Martin Braun <martin.braun@ettus.com> 2014-07-07 23:12:08 +0200
commit: 350d285a27ee6ea0f448a778551cdd3a5ffcedba (patch)
tree: 0f6333dbf3cfeeb22b52d4a1aa200ff71fb6efce /gr-uhd
parent: 1eaa96b61b495dc669a69fa660a44ec86cf057cc (diff)
6 files changed, 187 insertions, 47 deletions
diff --git a/gr-uhd/examples/python/freq_hopping.py b/gr-uhd/examples/python/freq_hopping.py
index 903b0b23d4..ba1704309b 100755
--- a/gr-uhd/examples/python/freq_hopping.py
+++ b/gr-uhd/examples/python/freq_hopping.py
@@ -114,7 +114,7 @@ class FrequencyHopperSrc(gr.hier_block2):
             if i > 0 and post_tuning:
                 tune_tag.offset -= 1 # Move it to last sample of previous burst
             tune_tag.key = pmt.string_to_symbol('tx_freq')
-            tune_tag.value = pmt.to_pmt((0, self.hop_sequence[i]))
+            tune_tag.value = pmt.to_pmt(self.hop_sequence[i])
             tag_list.append(tune_tag)
             length_tag = gr.tag_t()
             length_tag.offset = i * burst_length
diff --git a/gr-uhd/lib/usrp_common.h b/gr-uhd/lib/usrp_common.h
new file mode 100644
index 0000000000..26b743a90f
--- /dev/null
+++ b/gr-uhd/lib/usrp_common.h
@@ -0,0 +1,100 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2014 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * GNU Radio is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3, or (at your option)
+ * any later version.
+ *
+ * GNU Radio is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef INCLUDED_GR_UHD_USRP_COMMON_H
+#define INCLUDED_GR_UHD_USRP_COMMON_H
+
+#include <pmt/pmt.h>
+#include <boost/dynamic_bitset.hpp>
+
+namespace gr {
+  namespace uhd {
+
+    //! Helper function for msg_handler_command:
+    // - Extracts command and the command value from the command PMT
+    // - Returns true if the command PMT is well formed
+    // - If a channel is given, return that as well, otherwise return -1
+    static bool _unpack_chan_command(
+	std::string &command,
+	pmt::pmt_t &cmd_val,
+	int &chan,
+	const pmt::pmt_t &cmd_pmt
+    ) {
+      chan = -1; // Default value
+      if (pmt::is_tuple(cmd_pmt) and (pmt::length(cmd_pmt) == 2 or pmt::length(cmd_pmt) == 3)) {
+	command = pmt::symbol_to_string(pmt::tuple_ref(cmd_pmt, 0));
+	cmd_val = pmt::tuple_ref(cmd_pmt, 1);
+	if (pmt::length(cmd_pmt) == 3) {
+	  chan = pmt::to_long(pmt::tuple_ref(cmd_pmt, 1));
+	}
+      }
+      else if (pmt::is_pair(cmd_pmt)) {
+	command = pmt::symbol_to_string(pmt::car(cmd_pmt));
+	cmd_val = pmt::car(cmd_pmt);
+      }
+      else {
+	return false;
+      }
+      return true;
+    }
+
+    //! Helper function for msg_handler_command:
+    // - Sets a value in vector_to_update to cmd_val, depending on chan
+    // - If chan is a positive integer, it will set vector_to_update[chan]
+    // - If chan is -1, it depends on minus_one_updates_all:
+    //   - Either set vector_to_update[0] or
+    //   - Set *all* entries in vector_to_update
+    // - Returns a dynamic_bitset, all indexes that where changed in
+    //   vector_to_update are set to 1
+    template <typename T>
+    static boost::dynamic_bitset<> _update_vector_from_cmd_val(
+	std::vector<T> &vector_to_update,
+	int chan,
+	const T cmd_val,
+	bool minus_one_updates_all = false
+    ) {
+      boost::dynamic_bitset<> vals_updated(vector_to_update.size());
+      if (chan == -1) {
+	if (minus_one_updates_all) {
+	  for (size_t i = 0; i < vector_to_update.size(); i++) {
+	    if (vector_to_update[i] != cmd_val) {
+	      vals_updated[i] = true;
+	      vector_to_update[i] = cmd_val;
+	    }
+	  }
+	  return vals_updated;
+	}
+	chan = 0;
+      }
+      if (vector_to_update[chan] != cmd_val) {
+	vector_to_update[chan] = cmd_val;
+	vals_updated[chan] = true;
+      }
+
+      return vals_updated;
+    }
+
+  } /* namespace uhd */
+} /* namespace gr */
+
+#endif /* INCLUDED_GR_UHD_USRP_COMMON_H */
+
diff --git a/gr-uhd/lib/usrp_sink_impl.cc b/gr-uhd/lib/usrp_sink_impl.cc
index b6a98391be..6942b024ee 100644
--- a/gr-uhd/lib/usrp_sink_impl.cc
+++ b/gr-uhd/lib/usrp_sink_impl.cc
@@ -22,6 +22,7 @@
 
 #include <climits>
 #include <stdexcept>
+#include "usrp_common.h"
 #include "usrp_sink_impl.h"
 #include "gr_uhd_common.h"
 #include <gnuradio/io_signature.h>
@@ -30,6 +31,8 @@
 namespace gr {
   namespace uhd {
 
+    static const size_t ALL_CHANS = ::uhd::usrp::multi_usrp::ALL_CHANS;
+
     usrp_sink::sptr
     usrp_sink::make(const ::uhd::device_addr_t &device_addr,
 		    const ::uhd::io_type_t &io_type,
@@ -75,8 +78,7 @@ namespace gr {
 	_curr_freq(stream_args.channels.size(), 0.0),
 	_curr_lo_offset(stream_args.channels.size(), 0.0),
 	_curr_gain(stream_args.channels.size(), 0.0),
-	_chans_to_tune(stream_args.channels.size(), false),
-	_call_tune(false)
+	_chans_to_tune(stream_args.channels.size())
     {
       if(stream_args.cpu_format == "fc32")
         _type = boost::make_shared< ::uhd::io_type_t >(::uhd::io_type_t::COMPLEX_FLOAT32);
@@ -89,11 +91,6 @@ namespace gr {
 	  pmt::mp("command"),
 	  boost::bind(&usrp_sink_impl::msg_handler_command, this, _1)
       );
-      //message_port_register_in(pmt::mp("query"));
-      //set_msg_handler(
-	  //pmt::mp("query"),
-	  //boost::bind(&usrp_sink_impl::msg_handler_query, this, _1)
-      //);
     }
 
     usrp_sink_impl::~usrp_sink_impl()
@@ -569,9 +566,8 @@ namespace gr {
       _metadata.time_spec += ::uhd::time_spec_t(0, num_sent, _sample_rate);
 
       // Some post-processing tasks if we actually transmitted the entire burst
-      if (_call_tune and num_sent == size_t(ninput_items)) {
+      if (_chans_to_tune.any() and num_sent == size_t(ninput_items)) {
 	_set_center_freq_from_internals_allchans();
-	_call_tune = false;
       }
 
       return num_sent;
@@ -724,7 +720,6 @@ namespace gr {
           // Otherwise, tune after work()
 	  _curr_freq[freq_cmd_chan] = freq_cmd_freq;
 	  _chans_to_tune[freq_cmd_chan] = true;
-          _call_tune = true;
         }
       }
 
@@ -801,48 +796,40 @@ namespace gr {
 
 
     /************** External interfaces (RPC + Message passing) ********************/
-    // Helper function for msg_handler_command: Extracts chan and command value from
-    // the 2-tuple in cmd_val, updates the value in vector_to_update[chan] and returns
-    // true if it was different from the old value.
-    bool _unpack_chan_command(pmt::pmt_t &cmd_val, int &chan, std::vector<double> &vector_to_update)
-    {
-      chan = pmt::to_long(pmt::tuple_ref(cmd_val, 0));
-      double new_value = pmt::to_double(pmt::tuple_ref(cmd_val, 1));
-      if (new_value == vector_to_update[chan]) {
-	return false;
-      } else {
-	vector_to_update[chan] = new_value;
-	return true;
-      }
-    }
-
     void usrp_sink_impl::msg_handler_command(pmt::pmt_t msg)
     {
-      const std::string command(pmt::symbol_to_string(pmt::car(msg)));
-      pmt::pmt_t value(pmt::cdr(msg));
-      int chan = 0;
+      std::string command;
+      pmt::pmt_t cmd_value;
+      int chan = -1;
+      if (not _unpack_chan_command(command, cmd_value, chan, msg)) {
+	GR_LOG_ALERT(d_logger, "Error while unpacking command PMT.");
+      }
       if (command == "freq") {
-	if (_unpack_chan_command(value, chan, _curr_freq)) {
-	  _set_center_freq_from_internals(chan);
-	}
+	_chans_to_tune = _update_vector_from_cmd_val<double>(
+	    _curr_freq, chan, pmt::to_double(cmd_value), true
+	);
+	_set_center_freq_from_internals_allchans();
       } else if (command == "lo_offset") {
-	if (_unpack_chan_command(value, chan, _curr_lo_offset)) {
-	  _set_center_freq_from_internals(chan);
-	}
+	_chans_to_tune = _update_vector_from_cmd_val<double>(
+	    _curr_lo_offset, chan, pmt::to_double(cmd_value), true
+	);
+	_set_center_freq_from_internals_allchans();
       } else if (command == "gain") {
-	if (_unpack_chan_command(value, chan, _curr_gain)) {
-	  set_gain(_curr_gain[chan], chan);
+	boost::dynamic_bitset<> chans_to_change = _update_vector_from_cmd_val<double>(
+	    _curr_gain, chan, pmt::to_double(cmd_value), true
+	);
+	if (chans_to_change.any()) {
+	  for (size_t i = 0; i < chans_to_change.size(); i++) {
+	    if (chans_to_change[i]) {
+	      set_gain(_curr_gain[i], i);
+	    }
+	  }
 	}
       } else {
 	GR_LOG_ALERT(d_logger, boost::format("Received unknown command: %s") % command);
       }
     }
 
-    void usrp_sink_impl::msg_handler_query(pmt::pmt_t msg)
-    {
-      //tbi
-    }
-
     void
     usrp_sink_impl::setup_rpc()
     {
diff --git a/gr-uhd/lib/usrp_sink_impl.h b/gr-uhd/lib/usrp_sink_impl.h
index 12a25658e3..4cfc1091e2 100644
--- a/gr-uhd/lib/usrp_sink_impl.h
+++ b/gr-uhd/lib/usrp_sink_impl.h
@@ -126,10 +126,6 @@ namespace gr {
       ::uhd::tune_result_t _set_center_freq_from_internals(size_t chan);
       //! Calls _set_center_freq_from_internals() on all channels
       void _set_center_freq_from_internals_allchans();
-      //! Receives commands and handles them
-      void msg_handler_command(pmt::pmt_t msg);
-      //! Receives queries and posts a response
-      void msg_handler_query(pmt::pmt_t msg);
 
       ::uhd::usrp::multi_usrp::sptr _dev;
       const ::uhd::stream_args_t _stream_args;
@@ -150,6 +146,9 @@ namespace gr {
       const pmt::pmt_t _length_tag_key;
       long _nitems_to_send;
 
+      /****** Command interface related **********/
+      //! Receives commands and handles them
+      void msg_handler_command(pmt::pmt_t msg);
       //! Stores the last value we told the USRP to tune to for every channel
       // (this is not necessarily the true value the USRP is currently tuned to!).
       // We could theoretically ask the device, but during streaming, we want to minimize
@@ -159,7 +158,7 @@ namespace gr {
       std::vector<double> _curr_lo_offset;
       //! Stores the last gain value we told the USRP to have for every channel.
       std::vector<double> _curr_gain;
-      std::vector<bool> _chans_to_tune;
+      boost::dynamic_bitset<> _chans_to_tune;
       bool _call_tune;
     };
 
diff --git a/gr-uhd/lib/usrp_source_impl.cc b/gr-uhd/lib/usrp_source_impl.cc
index e57db13a0b..278bb3d652 100644
--- a/gr-uhd/lib/usrp_source_impl.cc
+++ b/gr-uhd/lib/usrp_source_impl.cc
@@ -20,6 +20,7 @@
  * Boston, MA 02110-1301, USA.
  */
 
+#include "usrp_common.h"
 #include "usrp_source_impl.h"
 #include "gr_uhd_common.h"
 #include <gnuradio/io_signature.h>
@@ -80,6 +81,12 @@ namespace gr {
       str << name() << unique_id();
       _id = pmt::string_to_symbol(str.str());
       _dev = ::uhd::usrp::multi_usrp::make(device_addr);
+
+      message_port_register_in(pmt::mp("command"));
+      set_msg_handler(
+	  pmt::mp("command"),
+	  boost::bind(&usrp_source_impl::msg_handler_command, this, _1)
+      );
     }
 
     usrp_source_impl::~usrp_source_impl()
@@ -670,6 +677,38 @@ namespace gr {
       return num_samps;
     }
 
+
+    /************** External interfaces (RPC + Message passing) ********************/
+    void usrp_source_impl::msg_handler_command(pmt::pmt_t msg)
+    {
+      std::string command;
+      pmt::pmt_t cmd_value;
+      int chan = -1;
+      if (not _unpack_chan_command(command, cmd_value, chan, msg)) {
+	GR_LOG_ALERT(d_logger, "Error while unpacking command PMT.");
+      }
+      if (command == "freq") {
+	double freq = pmt::to_double(cmd_value);
+	for (size_t i = 0; i < _nchan; i++) {
+	  if (chan == -1 or chan == int(i)) {
+	    set_center_freq(freq, i);
+	  }
+	}
+	// TODO: implement
+      //} else if (command == "lo_offset") {
+	//;
+      } else if (command == "gain") {
+	double gain = pmt::to_double(cmd_value);
+	for (size_t i = 0; i < _nchan; i++) {
+	  if (chan == -1 or chan == int(i)) {
+	    set_gain(gain, i);
+	  }
+	}
+      } else {
+	GR_LOG_ALERT(d_logger, boost::format("Received unknown command: %s") % command);
+      }
+    }
+
     void
     usrp_source_impl::setup_rpc()
     {
diff --git a/gr-uhd/lib/usrp_source_impl.h b/gr-uhd/lib/usrp_source_impl.h
index 3cfa1aad35..3f0dd4afde 100644
--- a/gr-uhd/lib/usrp_source_impl.h
+++ b/gr-uhd/lib/usrp_source_impl.h
@@ -145,6 +145,21 @@ namespace gr {
       double _center_freq;
 
       boost::recursive_mutex d_mutex;
+
+      /****** Command interface related **********/
+      //! Receives commands and handles them
+      void msg_handler_command(pmt::pmt_t msg);
+      //! Stores the last value we told the USRP to tune to for every channel
+      // (this is not necessarily the true value the USRP is currently tuned to!).
+      // We could theoretically ask the device, but during streaming, we want to minimize
+      // communication with the USRP.
+      std::vector<double> _curr_freq;
+      //! Stores the last value we told the USRP to have the LO offset for every channel.
+      std::vector<double> _curr_lo_offset;
+      //! Stores the last gain value we told the USRP to have for every channel.
+      std::vector<double> _curr_gain;
+      boost::dynamic_bitset<> _chans_to_tune;
+      bool _call_tune;
     };
 
   } /* namespace uhd */
author	Martin Braun <martin.braun@ettus.com>	2014-07-03 00:11:29 +0200
committer	Martin Braun <martin.braun@ettus.com>	2014-07-07 23:12:08 +0200
commit	350d285a27ee6ea0f448a778551cdd3a5ffcedba (patch)
tree	0f6333dbf3cfeeb22b52d4a1aa200ff71fb6efce /gr-uhd
parent	1eaa96b61b495dc669a69fa660a44ec86cf057cc (diff)