From 876d4150aa3da531ddd687b48afada6e43f79146 Mon Sep 17 00:00:00 2001 From: Martin Braun Date: Mon, 2 Mar 2020 15:25:13 -0800 Subject: uhd: Apply clang-format against all .cpp and .hpp files in host/ Note: template_lvbitx.{cpp,hpp} need to be excluded from the list of files that clang-format gets applied against. --- host/lib/usrp/gps_ctrl.cpp | 574 ++++++++++++++++++++++----------------------- 1 file changed, 284 insertions(+), 290 deletions(-) (limited to 'host/lib/usrp/gps_ctrl.cpp') diff --git a/host/lib/usrp/gps_ctrl.cpp b/host/lib/usrp/gps_ctrl.cpp index 07d7f4ec4..60866b85e 100644 --- a/host/lib/usrp/gps_ctrl.cpp +++ b/host/lib/usrp/gps_ctrl.cpp @@ -5,96 +5,94 @@ // SPDX-License-Identifier: GPL-3.0-or-later // -#include - -#include #include #include +#include +#include +#include #include -#include -#include +#include #include -#include #include -#include +#include +#include +#include #include +#include #include -#include #include -#include -#include +#include 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; -} +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){ +gps_ctrl::~gps_ctrl(void) +{ /* NOP */ } -class gps_ctrl_impl : public gps_ctrl{ +class gps_ctrl_impl : public gps_ctrl +{ private: std::map> sentences; boost::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 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 now = boost::get_system_time(); boost::system_time exit_time = now + milliseconds(timeout); boost::posix_time::time_duration age; - if (wait_for_next) - { + if (wait_for_next) { boost::lock_guard lock(cache_mutex); update_cache(); - //mark sentence as touched + // mark sentence as touched if (sentences.find(which) != sentences.end()) std::get<2>(sentences[which]) = true; } - while (1) - { - try - { + while (1) { + try { boost::lock_guard lock(cache_mutex); // update cache if older than a millisecond - if (now - _last_cache_update > milliseconds(1)) - { + if (now - _last_cache_update > milliseconds(1)) { update_cache(); } - if (sentences.find(which) == sentences.end()) - { + 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]); + 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) { + } catch (std::exception& e) { UHD_LOGGER_DEBUG("GPS") << "get_sentence: " << e.what(); } - if (not sentence.empty() or now > exit_time) - { + if (not sentence.empty() or now > exit_time) { break; } @@ -102,8 +100,7 @@ private: now = boost::get_system_time(); } - if (sentence.empty()) - { + if (sentence.empty()) { throw uhd::value_error("gps ctrl: No " + which + " message found"); } @@ -112,7 +109,7 @@ private: static bool is_nmea_checksum_ok(std::string nmea) { - if (nmea.length() < 5 || nmea[0] != '$' || nmea[nmea.length()-3] != '*') + if (nmea.length() < 5 || nmea[0] != '$' || nmea[nmea.length() - 3] != '*') return false; std::stringstream ss; @@ -120,310 +117,307 @@ private: uint32_t calculated_crc = 0; // get crc from string - ss << std::hex << nmea.substr(nmea.length()-2, 2); + ss << std::hex << nmea.substr(nmea.length() - 2, 2); ss >> string_crc; // calculate crc - for (size_t i = 1; i < nmea.length()-3; i++) + 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 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 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)) + void update_cache() { - // Strip any end of line characters - erase_all(msg, "\r"); - erase_all(msg, "\n"); - - if (msg.empty()) - { - // Ignore empty strings - continue; + if (not gps_detected()) { + return; } - if (msg.length() < 6) - { - UHD_LOGGER_WARNING("GPS") << __FUNCTION__ << ": Short GPSDO string: " << msg ; - continue; - } + const std::list 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 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; + } - // 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 ; + 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(); + 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); + // 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 - } + _last_cache_update = time; } - if (_gps_type == GPS_TYPE_NONE) +public: + gps_ctrl_impl(uart_iface::sptr uart) : _uart(uart), _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"; + 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 + } } - } - - 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; + 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"; + } + } - case GPS_TYPE_NONE: - default: - UHD_LOGGER_INFO("GPS") << "No GPSDO found"; - break; + 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; - // initialize cache - update_cache(); - } + case GPS_TYPE_NONE: + default: + UHD_LOGGER_INFO("GPS") << "No GPSDO found"; + break; + } - ~gps_ctrl_impl(void){ - /* NOP */ - } - - //return a list of supported sensors - std::vector get_sensors(void) { - std::vector ret{ - "gps_gpgga", - "gps_gprmc", - "gps_time", - "gps_locked", - "gps_servo" - }; - return ret; - } - - uhd::sensor_value_t get_sensor(std::string key) { - 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"); + // initialize cache + update_cache(); } - else if(key == "gps_locked") { - return sensor_value_t("GPS lock status", locked(), "locked", "unlocked"); + + ~gps_ctrl_impl(void) + { + /* NOP */ } - 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), - ""); + + // return a list of supported sensors + std::vector get_sensors(void) + { + std::vector ret{ + "gps_gpgga", "gps_gprmc", "gps_time", "gps_locked", "gps_servo"}; + return ret; } - else { - throw uhd::value_error("gps ctrl get_sensor unknown key: " + key); + + uhd::sensor_value_t get_sensor(std::string key) + { + 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 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( + 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 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 > tok(sentence); - std::vector 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.size() == 0 or timestr.size() == 0) { - 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++; + // helper function to retrieve a field from an NMEA sentence + std::string get_token(std::string sentence, size_t offset) + { + boost::tokenizer> tok(sentence); + std::vector 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]; } - throw uhd::value_error("get_time: Timeout after no valid message found"); - return gps_time; //keep gcc from complaining - } + 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.size() == 0 or timestr.size() == 0) { + 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"); - int64_t get_epoch_time(void) { - return (get_time() - from_time_t(0)).total_seconds(); - } + return gps_time; // keep gcc from complaining + } - bool gps_detected(void) { - return (_gps_type != GPS_TYPE_NONE); - } + int64_t get_epoch_time(void) + { + return (get_time() - from_time_t(0)).total_seconds(); + } + + bool gps_detected(void) + { + 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()) + 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++; - 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"); } - throw uhd::value_error("locked(): unable to determine GPS lock status"); - } - uart_iface::sptr _uart; + uart_iface::sptr _uart; - void _flush(void){ - while (not _uart->read_uart(0.0).empty()){ - //NOP + 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); - } + std::string _recv(double timeout = GPS_TIMEOUT_DELAY_MS / 1000.) + { + return _uart->read_uart(timeout); + } - enum { - GPS_TYPE_INTERNAL_GPSDO, - GPS_TYPE_GENERIC_NMEA, - GPS_TYPE_NONE - } _gps_type; + 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){ +gps_ctrl::sptr gps_ctrl::make(uart_iface::sptr uart) +{ return sptr(new gps_ctrl_impl(uart)); } -- cgit v1.2.3