mirror of
https://github.com/saymrwulf/uhd.git
synced 2026-05-15 21:01:26 +00:00
b6119e581e
This is a very mechanical task that could almost have been done with sed. Boost versions of mutexes and locks were removed, and replaced with std:: versions. The replacement tables are as follows: == Mutexes == - boost::mutex -> std::mutex - boost::recursive_mutex -> std::recursive_mutex Mutexes behave identically between Boost and std:: and have the same API. == Locks == C++11 has only two types of lock that we use/need in UHD: - std::lock_guard: Identical to boost::lock_guard - std::unique_lock: Identical to boost::unique_lock Boost also has boost::mutex::scoped_lock, which is a typedef for boost::unique_lock<>. However, we often have used scoped_lock where we meant to use lock_guard<>. The name is a bit misleading, "scoped lock" sounding a bit like an RAII mechanism. Therefore, some previous boost::mutex::scoped_lock are now std::lock_guard<>. std::unique_lock is required when doing more than RAII locking (i.e., unlocking, relocking, usage with condition variables, etc.). == Condition Variables == Condition variables were out of the scope of this lock/mutex change, but in UHD, we inconsistently use boost::condition vs. boost::condition_variable. The former is a templated version of the latter, and thus works fine with std::mutex'es. Therefore, some boost::condition_variable where changed to boost::condition. All locks and mutexes use `#include <mutex>`. The corresponding Boost includes were removed. In some cases, this exposed issues with implicit Boost includes elsewhere. The missing explicit includes were added.
424 lines
14 KiB
C++
424 lines
14 KiB
C++
//
|
|
// Copyright 2010-2011,2014-2016 Ettus Research LLC
|
|
// Copyright 2018 Ettus Research, a National Instruments Company
|
|
//
|
|
// SPDX-License-Identifier: GPL-3.0-or-later
|
|
//
|
|
|
|
#include <uhd/exception.hpp>
|
|
#include <uhd/types/sensors.hpp>
|
|
#include <uhd/usrp/gps_ctrl.hpp>
|
|
#include <uhd/utils/log.hpp>
|
|
#include <stdint.h>
|
|
#include <boost/algorithm/string.hpp>
|
|
#include <boost/date_time.hpp>
|
|
#include <boost/date_time/posix_time/posix_time_types.hpp>
|
|
#include <boost/format.hpp>
|
|
#include <boost/thread/thread_time.hpp>
|
|
#include <boost/tokenizer.hpp>
|
|
#include <chrono>
|
|
#include <ctime>
|
|
#include <mutex>
|
|
#include <regex>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <tuple>
|
|
|
|
using namespace uhd;
|
|
using namespace boost::posix_time;
|
|
using namespace boost::algorithm;
|
|
|
|
namespace {
|
|
constexpr int GPS_COMM_TIMEOUT_MS = 1300;
|
|
constexpr int GPS_NMEA_NORMAL_FRESHNESS = 1000;
|
|
constexpr int GPS_SERVO_FRESHNESS = 1000;
|
|
constexpr int GPS_LOCK_FRESHNESS = 2500;
|
|
constexpr int GPS_TIMEOUT_DELAY_MS = 200;
|
|
constexpr int GPSDO_COMMAND_DELAY_MS = 200;
|
|
} // namespace
|
|
|
|
/*!
|
|
* A control for GPSDO devices
|
|
*/
|
|
|
|
gps_ctrl::~gps_ctrl(void)
|
|
{
|
|
/* NOP */
|
|
}
|
|
|
|
class gps_ctrl_impl : public gps_ctrl
|
|
{
|
|
private:
|
|
std::map<std::string, std::tuple<std::string, boost::system_time, bool>> sentences;
|
|
std::mutex cache_mutex;
|
|
boost::system_time _last_cache_update;
|
|
|
|
std::string get_sentence(const std::string which,
|
|
const int max_age_ms,
|
|
const int timeout,
|
|
const bool wait_for_next = false)
|
|
{
|
|
std::string sentence;
|
|
boost::system_time now = boost::get_system_time();
|
|
boost::system_time exit_time = now + milliseconds(timeout);
|
|
boost::posix_time::time_duration age;
|
|
|
|
if (wait_for_next) {
|
|
std::lock_guard<std::mutex> lock(cache_mutex);
|
|
update_cache();
|
|
// mark sentence as touched
|
|
if (sentences.find(which) != sentences.end())
|
|
std::get<2>(sentences[which]) = true;
|
|
}
|
|
while (1) {
|
|
try {
|
|
std::lock_guard<std::mutex> lock(cache_mutex);
|
|
|
|
// update cache if older than a millisecond
|
|
if (now - _last_cache_update > milliseconds(1)) {
|
|
update_cache();
|
|
}
|
|
|
|
if (sentences.find(which) == sentences.end()) {
|
|
age = milliseconds(max_age_ms);
|
|
} else {
|
|
age = boost::get_system_time() - std::get<1>(sentences[which]);
|
|
}
|
|
if (age < milliseconds(max_age_ms)
|
|
and (not(wait_for_next and std::get<2>(sentences[which])))) {
|
|
sentence = std::get<0>(sentences[which]);
|
|
std::get<2>(sentences[which]) = true;
|
|
}
|
|
} catch (std::exception& e) {
|
|
UHD_LOGGER_DEBUG("GPS") << "get_sentence: " << e.what();
|
|
}
|
|
|
|
if (not sentence.empty() or now > exit_time) {
|
|
break;
|
|
}
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
now = boost::get_system_time();
|
|
}
|
|
|
|
if (sentence.empty()) {
|
|
throw uhd::value_error("gps ctrl: No " + which + " message found");
|
|
}
|
|
|
|
return sentence;
|
|
}
|
|
|
|
static bool is_nmea_checksum_ok(std::string nmea)
|
|
{
|
|
if (nmea.length() < 5 || nmea[0] != '$' || nmea[nmea.length() - 3] != '*')
|
|
return false;
|
|
|
|
std::stringstream ss;
|
|
uint32_t string_crc;
|
|
uint32_t calculated_crc = 0;
|
|
|
|
// get crc from string
|
|
ss << std::hex << nmea.substr(nmea.length() - 2, 2);
|
|
ss >> string_crc;
|
|
|
|
// calculate crc
|
|
for (size_t i = 1; i < nmea.length() - 3; i++)
|
|
calculated_crc ^= nmea[i];
|
|
|
|
// return comparison
|
|
return (string_crc == calculated_crc);
|
|
}
|
|
|
|
void update_cache()
|
|
{
|
|
if (not gps_detected()) {
|
|
return;
|
|
}
|
|
|
|
const std::list<std::string> keys{"GPGGA", "GPRMC", "SERVO"};
|
|
static const std::regex servo_regex("^\\d\\d-\\d\\d-\\d\\d.*$");
|
|
static const std::regex gp_msg_regex("^\\$GP.*,\\*[0-9A-F]{2}$");
|
|
std::map<std::string, std::string> msgs;
|
|
|
|
// Get all GPSDO messages available
|
|
// Creating a map here because we only want the latest of each message type
|
|
for (std::string msg = _recv(0); not msg.empty(); msg = _recv(0)) {
|
|
// Strip any end of line characters
|
|
erase_all(msg, "\r");
|
|
erase_all(msg, "\n");
|
|
|
|
if (msg.empty()) {
|
|
// Ignore empty strings
|
|
continue;
|
|
}
|
|
|
|
if (msg.length() < 6) {
|
|
UHD_LOGGER_WARNING("GPS")
|
|
<< __FUNCTION__ << ": Short GPSDO string: " << msg;
|
|
continue;
|
|
}
|
|
|
|
// Look for SERVO message
|
|
if (std::regex_search(
|
|
msg, servo_regex, std::regex_constants::match_continuous)) {
|
|
msgs["SERVO"] = msg;
|
|
} else if (std::regex_match(msg, gp_msg_regex) and is_nmea_checksum_ok(msg)) {
|
|
msgs[msg.substr(1, 5)] = msg;
|
|
} else {
|
|
UHD_LOGGER_WARNING("GPS")
|
|
<< __FUNCTION__ << ": Malformed GPSDO string: " << msg;
|
|
}
|
|
}
|
|
|
|
boost::system_time time = boost::get_system_time();
|
|
|
|
// Update sentences with newly read data
|
|
for (std::string key : keys) {
|
|
if (not msgs[key].empty()) {
|
|
sentences[key] = std::make_tuple(msgs[key], time, false);
|
|
}
|
|
}
|
|
|
|
_last_cache_update = time;
|
|
}
|
|
|
|
public:
|
|
gps_ctrl_impl(uart_iface::sptr uart) : _uart(uart), _gps_type(GPS_TYPE_NONE)
|
|
{
|
|
std::string reply;
|
|
bool i_heard_some_nmea = false, i_heard_something_weird = false;
|
|
|
|
// first we look for an internal GPSDO
|
|
_flush(); // get whatever junk is in the rx buffer right now, and throw it away
|
|
|
|
_send("*IDN?\r\n"); // request identity from the GPSDO
|
|
|
|
// then we loop until we either timeout, or until we get a response that indicates
|
|
// we're a JL device maximum response time was measured at ~320ms, so we set the
|
|
// timeout at 650ms
|
|
const boost::system_time comm_timeout =
|
|
boost::get_system_time() + milliseconds(650);
|
|
while (boost::get_system_time() < comm_timeout) {
|
|
reply = _recv();
|
|
// known devices are JL "FireFly", "GPSTCXO", and "LC_XO"
|
|
if (reply.find("FireFly") != std::string::npos
|
|
or reply.find("LC_XO") != std::string::npos
|
|
or reply.find("GPSTCXO") != std::string::npos) {
|
|
_gps_type = GPS_TYPE_INTERNAL_GPSDO;
|
|
break;
|
|
} else if (reply.substr(0, 3) == "$GP") {
|
|
i_heard_some_nmea = true; // but keep looking
|
|
} else if (not reply.empty()) {
|
|
// wrong baud rate or firmware still initializing
|
|
i_heard_something_weird = true;
|
|
_send("*IDN?\r\n"); // re-send identity request
|
|
} else {
|
|
// _recv timed out
|
|
_send("*IDN?\r\n"); // re-send identity request
|
|
}
|
|
}
|
|
|
|
if (_gps_type == GPS_TYPE_NONE) {
|
|
if (i_heard_some_nmea) {
|
|
_gps_type = GPS_TYPE_GENERIC_NMEA;
|
|
} else if (i_heard_something_weird) {
|
|
UHD_LOGGER_ERROR("GPS")
|
|
<< "GPS invalid reply \"" << reply << "\", assuming none available";
|
|
}
|
|
}
|
|
|
|
switch (_gps_type) {
|
|
case GPS_TYPE_INTERNAL_GPSDO:
|
|
erase_all(reply, "\r");
|
|
erase_all(reply, "\n");
|
|
UHD_LOGGER_INFO("GPS") << "Found an internal GPSDO: " << reply;
|
|
init_gpsdo();
|
|
break;
|
|
|
|
case GPS_TYPE_GENERIC_NMEA:
|
|
UHD_LOGGER_INFO("GPS") << "Found a generic NMEA GPS device";
|
|
break;
|
|
|
|
case GPS_TYPE_NONE:
|
|
default:
|
|
UHD_LOGGER_INFO("GPS") << "No GPSDO found";
|
|
break;
|
|
}
|
|
|
|
// initialize cache
|
|
update_cache();
|
|
}
|
|
|
|
~gps_ctrl_impl(void) override
|
|
{
|
|
/* NOP */
|
|
}
|
|
|
|
// return a list of supported sensors
|
|
std::vector<std::string> get_sensors(void) override
|
|
{
|
|
std::vector<std::string> ret{
|
|
"gps_gpgga", "gps_gprmc", "gps_time", "gps_locked", "gps_servo"};
|
|
return ret;
|
|
}
|
|
|
|
uhd::sensor_value_t get_sensor(std::string key) override
|
|
{
|
|
if (key == "gps_gpgga" or key == "gps_gprmc") {
|
|
return sensor_value_t(boost::to_upper_copy(key),
|
|
get_sentence(boost::to_upper_copy(key.substr(4, 8)),
|
|
GPS_NMEA_NORMAL_FRESHNESS,
|
|
GPS_TIMEOUT_DELAY_MS),
|
|
"");
|
|
} else if (key == "gps_time") {
|
|
return sensor_value_t("GPS epoch time", int(get_epoch_time()), "seconds");
|
|
} else if (key == "gps_locked") {
|
|
return sensor_value_t("GPS lock status", locked(), "locked", "unlocked");
|
|
} else if (key == "gps_servo") {
|
|
return sensor_value_t(boost::to_upper_copy(key),
|
|
get_sentence(boost::to_upper_copy(key.substr(4, 8)),
|
|
GPS_SERVO_FRESHNESS,
|
|
GPS_TIMEOUT_DELAY_MS),
|
|
"");
|
|
} else {
|
|
throw uhd::value_error("gps ctrl get_sensor unknown key: " + key);
|
|
}
|
|
}
|
|
|
|
private:
|
|
void init_gpsdo(void)
|
|
{
|
|
// issue some setup stuff so it spits out the appropriate data
|
|
// none of these should issue replies so we don't bother looking for them
|
|
// we have to sleep between commands because the JL device, despite not
|
|
// acking, takes considerable time to process each command.
|
|
const std::vector<std::string> init_cmds = {"SYST:COMM:SER:ECHO OFF\r\n",
|
|
"SYST:COMM:SER:PRO OFF\r\n",
|
|
"GPS:GPGGA 1\r\n",
|
|
"GPS:GGAST 0\r\n",
|
|
"GPS:GPRMC 1\r\n",
|
|
"SERV:TRAC 1\r\n"};
|
|
|
|
for (const auto& cmd : init_cmds) {
|
|
_send(cmd);
|
|
std::this_thread::sleep_for(
|
|
std::chrono::milliseconds(GPSDO_COMMAND_DELAY_MS));
|
|
}
|
|
}
|
|
|
|
// helper function to retrieve a field from an NMEA sentence
|
|
std::string get_token(std::string sentence, size_t offset)
|
|
{
|
|
boost::tokenizer<boost::escaped_list_separator<char>> tok(sentence);
|
|
std::vector<std::string> toked;
|
|
tok.assign(sentence); // this can throw
|
|
toked.assign(tok.begin(), tok.end());
|
|
|
|
if (toked.size() <= offset) {
|
|
throw uhd::value_error(
|
|
str(boost::format("Invalid response \"%s\"") % sentence));
|
|
}
|
|
return toked[offset];
|
|
}
|
|
|
|
ptime get_time(void)
|
|
{
|
|
int error_cnt = 0;
|
|
ptime gps_time;
|
|
while (error_cnt < 2) {
|
|
try {
|
|
// wait for next GPRMC string
|
|
std::string reply = get_sentence(
|
|
"GPRMC", GPS_NMEA_NORMAL_FRESHNESS, GPS_COMM_TIMEOUT_MS, true);
|
|
|
|
std::string datestr = get_token(reply, 9);
|
|
std::string timestr = get_token(reply, 1);
|
|
|
|
if (datestr.empty() or timestr.empty()) {
|
|
throw uhd::value_error(
|
|
str(boost::format("Invalid response \"%s\"") % reply));
|
|
}
|
|
|
|
struct tm raw_date;
|
|
raw_date.tm_year =
|
|
std::stoi(datestr.substr(4, 2)) + 2000 - 1900; // years since 1900
|
|
raw_date.tm_mon =
|
|
std::stoi(datestr.substr(2, 2)) - 1; // months since january (0-11)
|
|
raw_date.tm_mday = std::stoi(datestr.substr(0, 2)); // dom (1-31)
|
|
raw_date.tm_hour = std::stoi(timestr.substr(0, 2));
|
|
raw_date.tm_min = std::stoi(timestr.substr(2, 2));
|
|
raw_date.tm_sec = std::stoi(timestr.substr(4, 2));
|
|
gps_time = boost::posix_time::ptime_from_tm(raw_date);
|
|
|
|
UHD_LOG_TRACE(
|
|
"GPS", "GPS time: " + boost::posix_time::to_simple_string(gps_time));
|
|
return gps_time;
|
|
|
|
} catch (std::exception& e) {
|
|
UHD_LOGGER_DEBUG("GPS") << "get_time: " << e.what();
|
|
error_cnt++;
|
|
}
|
|
}
|
|
throw uhd::value_error("get_time: Timeout after no valid message found");
|
|
|
|
return gps_time; // keep gcc from complaining
|
|
}
|
|
|
|
int64_t get_epoch_time(void)
|
|
{
|
|
return (get_time() - from_time_t(0)).total_seconds();
|
|
}
|
|
|
|
bool gps_detected(void) override
|
|
{
|
|
return (_gps_type != GPS_TYPE_NONE);
|
|
}
|
|
|
|
bool locked(void)
|
|
{
|
|
int error_cnt = 0;
|
|
while (error_cnt < 3) {
|
|
try {
|
|
std::string reply =
|
|
get_sentence("GPGGA", GPS_LOCK_FRESHNESS, GPS_COMM_TIMEOUT_MS);
|
|
if (reply.empty())
|
|
error_cnt++;
|
|
else
|
|
return (get_token(reply, 6) != "0");
|
|
} catch (std::exception& e) {
|
|
UHD_LOGGER_DEBUG("GPS") << "locked: " << e.what();
|
|
error_cnt++;
|
|
}
|
|
}
|
|
throw uhd::value_error("locked(): unable to determine GPS lock status");
|
|
}
|
|
|
|
uart_iface::sptr _uart;
|
|
|
|
void _flush(void)
|
|
{
|
|
while (not _uart->read_uart(0.0).empty()) {
|
|
// NOP
|
|
}
|
|
}
|
|
|
|
std::string _recv(double timeout = GPS_TIMEOUT_DELAY_MS / 1000.)
|
|
{
|
|
return _uart->read_uart(timeout);
|
|
}
|
|
|
|
void _send(const std::string& buf)
|
|
{
|
|
return _uart->write_uart(buf);
|
|
}
|
|
|
|
enum { GPS_TYPE_INTERNAL_GPSDO, GPS_TYPE_GENERIC_NMEA, GPS_TYPE_NONE } _gps_type;
|
|
};
|
|
|
|
/***********************************************************************
|
|
* Public make function for the GPS control
|
|
**********************************************************************/
|
|
gps_ctrl::sptr gps_ctrl::make(uart_iface::sptr uart)
|
|
{
|
|
return sptr(new gps_ctrl_impl(uart));
|
|
}
|