//
// Copyright 2010-2011,2014-2015 Ettus Research LLC
//
// This program 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 of the License, or
// (at your option) any later version.
//
// This program 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 this program. If not, see .
//
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "boost/tuple/tuple.hpp"
#include "boost/foreach.hpp"
using namespace uhd;
using namespace boost::gregorian;
using namespace boost::posix_time;
using namespace boost::algorithm;
using namespace boost::this_thread;
/*!
* A NMEA and UBX parser for the GPS serial interface.
*/
class gps_ctrl_parser {
private:
std::deque gps_data_input;
std::string parse_ubx()
{
// Assumptions:
// The deque now contains an UBX message in the format
// \xb6\x62
// where
// is 1 byte
// is 1 byte
// is 2 bytes (little-endian), length of
// is 2 bytes
uint8_t ck_a = 0;
uint8_t ck_b = 0;
if (gps_data_input.size() >= 8) {
uint8_t len_lo = gps_data_input[4];
uint8_t len_hi = gps_data_input[5];
size_t len = len_lo | (len_hi << 8);
if (gps_data_input.size() >= len + 8) {
/*
std::cerr << "DATA: ";
for (size_t i=0; i < gps_data_input.size(); i++) {
uint8_t dat = gps_data_input[i];
std::cerr << boost::format("%02x ") % (unsigned int)dat;
}
std::cerr << std::endl;
*/
uint8_t ck_a_packet = gps_data_input[len+6];
uint8_t ck_b_packet = gps_data_input[len+7];
// Range over which CRC is calculated is
for (size_t i = 2; i < len+6; i++) {
ck_a += (uint8_t)(gps_data_input[i]);
ck_b += ck_a;
}
std::string msg(gps_data_input.begin(), gps_data_input.begin() + (len + 8));
gps_data_input.erase(gps_data_input.begin(), gps_data_input.begin() + (len + 8));
if (ck_a == ck_a_packet and ck_b == ck_b_packet) {
return msg;
}
}
}
return std::string();
}
std::string parse_nmea()
{
// Assumptions:
// The deque now contains an NMEA message in the format
// $G.................*XX
// the checksum XX is dropped from the message
std::deque::iterator star;
star = std::find(gps_data_input.begin() + 2, gps_data_input.end(), '*');
if (star != gps_data_input.end()) {
std::string msg(gps_data_input.begin(), star);
// The parser will take care of the leftover *XX
gps_data_input.erase(gps_data_input.begin(), star);
return msg;
}
return std::string();
}
public:
template
void push_data(InputIterator first, InputIterator last)
{
gps_data_input.insert(gps_data_input.end(), first, last);
}
std::string get_next_message()
{
while (gps_data_input.size() >= 2) {
char header1 = gps_data_input[0];
char header2 = gps_data_input[1];
std::string parsed;
if (header1 == '$' and header2 == 'G') {
parsed = parse_nmea();
}
else if (header1 == '\xb5' and header2 == '\x62') {
parsed = parse_ubx();
}
if (parsed.empty()) {
gps_data_input.pop_front();
}
else {
return parsed;
}
}
return std::string();
}
size_t size() { return gps_data_input.size(); }
};
gps_ctrl::~gps_ctrl(void){
/* NOP */
}
/*!
* A control for GPSDO devices
*/
class gps_ctrl_impl : public gps_ctrl{
private:
std::map > sensors;
gps_ctrl_parser gps_parser;
std::string get_cached_sensor(const std::string sensor, const int freshness, const bool once, const bool touch=true) {
boost::system_time time = boost::get_system_time();
try {
// this is nasty ...
//std::cout << boost::format("Requested %s - seen? ") % sensor << sensors[sensor].get<2>() << " once? " << once << std::endl;
if(time - sensors[sensor].get<1>() < milliseconds(freshness) && (!once or !sensors[sensor].get<2>())) {
sensors[sensor] = boost::make_tuple(sensors[sensor].get<0>(), sensors[sensor].get<1>(), touch);
return sensors[sensor].get<0>();
} else {
return update_cached_sensors(sensor);
}
} catch(std::exception &e) {
UHD_MSG(warning) << "get_cached_sensor: " << e.what() << std::endl;
}
return std::string();
}
static bool is_nmea_checksum_ok(std::string nmea)
{
if (nmea.length() < 5 || nmea[0] != '$' || nmea[nmea.length()-3] != '*')
return false;
std::stringstream ss;
boost::uint32_t string_crc;
boost::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);
}
std::string update_cached_sensors(const std::string sensor) {
if (gps_detected() && gps_type == GPS_TYPE_INTERNAL_GPSDO) {
const std::list list = boost::assign::list_of("GPGGA")("GPRMC")("SERVO");
static const boost::regex status_regex("\\d\\d-\\d\\d-\\d\\d");
static const boost::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(); msg.length(); msg = _recv())
{
// Strip any end of line characters
erase_all(msg, "\r");
erase_all(msg, "\n");
if (msg.length() < 6)
{
UHD_LOGV(regularly) << __FUNCTION__ << ": Short NMEA string: " << msg << std::endl;
continue;
}
// Look for SERVO message
if (boost::regex_search(msg, status_regex, boost::regex_constants::match_continuous))
{
msgs["SERVO"] = msg;
}
else if (boost::regex_match(msg, gp_msg_regex) and is_nmea_checksum_ok(msg))
{
msgs[msg.substr(1,5)] = msg;
}
else
{
UHD_LOGV(regularly) << __FUNCTION__ << ": Malformed NMEA string: " << msg << std::endl;
}
}
boost::system_time time = boost::get_system_time();
// Update sensors with newly read data
BOOST_FOREACH(std::string key, list) {
if (msgs[key].length())
sensors[key] = boost::make_tuple(msgs[key], time, !sensor.compare(key));
}
// Return requested sensor if it was updated
if (msgs[sensor].length())
return msgs[sensor];
return std::string();
}
else if (gps_detected() && gps_type == GPS_TYPE_LEA_M8F) {
const std::list list = boost::assign::list_of("GNGGA")("GNRMC")("FIXTYPE");
// Concatenate all incoming data into the deque
for (std::string msg = _recv();
msg.length() > 0 && gps_parser.size() < (2*115200);
msg = _recv())
{
gps_parser.push_data(msg.begin(), msg.end());
}
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 = gps_parser.get_next_message(); not msg.empty(); msg = gps_parser.get_next_message())
{
/*
std::stringstream ss;
ss << "Got message ";
for (size_t m = 0; m < msg.size(); m++) {
ss << std::hex << (unsigned int)(unsigned char)msg[m] << " " << std::dec;
}
UHD_MSG(warning) << ss.str() << ":" << std::endl << msg << std::endl;
*/
// Get UBX-NAV-SOL
const uint8_t nav_sol_head[4] = {0xb5, 0x62, 0x01, 0x06};
const std::string nav_sol_head_str(reinterpret_cast(nav_sol_head), 4);
if (msg.find(nav_sol_head_str) == 0 and msg.length() == 52 + 8) {
uint8_t gpsFix = msg[6 + 10]; // header size == 6, field offset == 10
std::string fixtype;
if (gpsFix == 0) {
fixtype = "no fix";
}
else if (gpsFix == 1) {
fixtype = "dead reckoning";
}
else if (gpsFix == 2) {
fixtype = "2d fix";
}
else if (gpsFix == 3) {
fixtype = "3d fix";
}
else if (gpsFix == 4) {
fixtype = "combined fix";
}
else if (gpsFix == 5) {
fixtype = "time-only fix";
}
if (not fixtype.empty()) {
msgs["FIXTYPE"] = fixtype;
}
}
else if (msg[0] == '$') {
msgs[msg.substr(1,5)] = msg;
}
else if (msg[0] == '\xb5' and msg[1] == '\x62') { /* Ignore unsupported UBX message */ }
else {
std::stringstream ss;
ss << "Unknown message ";
for (size_t m = 0; m < msg.size(); m++) {
ss << std::hex << (unsigned int)(unsigned char)msg[m] << " " << std::dec;
}
UHD_MSG(warning) << ss.str() << ":" << std::endl << msg << std::endl;
}
}
boost::system_time time = boost::get_system_time();
// Update sensors with newly read data
BOOST_FOREACH(std::string key, list) {
if (msgs[key].length()) {
sensors[key] = boost::make_tuple(msgs[key], time, !sensor.compare(key));
}
}
// Return requested sensor if it was updated
if (msgs[sensor].length())
return msgs[sensor];
return std::string();
}
else {
UHD_MSG(error) << "get_stat(): unsupported GPS or no GPS detected" << std::endl;
return std::string();
}
}
public:
gps_ctrl_impl(uart_iface::sptr uart){
_uart = uart;
std::string reply;
bool i_heard_some_nmea = false, i_heard_something_weird = false;
gps_type = GPS_TYPE_NONE;
//first we look for an internal GPSDO
_flush(); //get whatever junk is in the rx buffer right now, and throw it away
_send("HAAAY GUYYYYS\n"); //to elicit a response from the GPSDO
// try to init LEA-M8F
init_lea_m8f();
//wait for _send(...) to return
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
//then we loop until we either timeout, or until we get a response that indicates we're a JL device
const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
while(boost::get_system_time() < comm_timeout) {
reply = _recv();
//UHD_MSG(warning) << "Received " << reply << std::endl;
if(reply.find("Command Error") != 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 for that "Command Error" response
else if(reply.substr(0, 2) == "\xB5""\x62" or
reply.substr(0, 3) == "$GN" ) {
// The u-blox LEA-M8F outputs UBX protocol messages
gps_type = GPS_TYPE_LEA_M8F;
i_heard_some_nmea = false;
break;
}
else if(reply.length() != 0) i_heard_something_weird = true; //probably wrong baud rate
}
if((i_heard_some_nmea) && (gps_type != GPS_TYPE_INTERNAL_GPSDO)) gps_type = GPS_TYPE_GENERIC_NMEA;
if((gps_type == GPS_TYPE_NONE) && i_heard_something_weird) {
UHD_MSG(error) << "GPS invalid reply \"" << reply << "\", assuming none available" << std::endl;
}
switch(gps_type) {
case GPS_TYPE_INTERNAL_GPSDO:
UHD_MSG(status) << "Found an internal GPSDO" << std::endl;
init_gpsdo();
break;
case GPS_TYPE_LEA_M8F:
UHD_MSG(status) << "Found an internal u-blox LEA-M8F GPSDO" << std::endl;
init_lea_m8f();
break;
case GPS_TYPE_GENERIC_NMEA:
UHD_MSG(status) << "Found a generic NMEA GPS device" << std::endl;
break;
case GPS_TYPE_NONE:
default:
UHD_MSG(status) << "No GPSDO found" << std::endl;
break;
}
}
~gps_ctrl_impl(void){
/* NOP */
}
//return a list of supported sensors
std::vector get_sensors(void) {
std::vector ret = boost::assign::list_of
("gps_gpgga")
("gps_gprmc")
("gps_gngga")
("gps_gnrmc")
("gps_time")
("gps_locked")
("gps_servo")
("gps_fixtype");
return ret;
}
uhd::sensor_value_t get_sensor(std::string key) {
if(key == "gps_gpgga"
or key == "gps_gprmc"
or key == "gps_gngga"
or key == "gps_gnrmc"
or key == "gps_fixtype" ) {
return sensor_value_t(
boost::to_upper_copy(key),
get_cached_sensor(boost::to_upper_copy(key.substr(4,8)), GPS_NMEA_NORMAL_FRESHNESS, false, false),
"");
}
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("GPS servo status", get_servo(), "");
}
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.
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
_send("SYST:COMM:SER:ECHO OFF\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
_send("SYST:COMM:SER:PRO OFF\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
_send("GPS:GPGGA 1\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
_send("GPS:GGAST 0\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
_send("GPS:GPRMC 1\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
_send("SERV:TRAC 0\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
}
void init_lea_m8f(void) {
// Enable the UBX-NAV-SOL and the $GNRMC messages
const uint8_t en_nav_sol[11] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4f};
_send(std::string(reinterpret_cast(en_nav_sol), sizeof(en_nav_sol)));
const uint8_t en_gnrmc[11] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xf0, 0x04, 0x01, 0xff, 0x18};
_send(std::string(reinterpret_cast(en_gnrmc), sizeof(en_gnrmc)));
}
//retrieve a raw NMEA sentence
std::string get_nmea(std::string msgtype) {
std::string reply;
const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
while(boost::get_system_time() < comm_timeout) {
if(! (msgtype.compare("GPRMC") || msgtype.compare("GNRMC")) ) {
reply = get_cached_sensor(msgtype, GPS_NMEA_FRESHNESS, true);
}
else {
reply = get_cached_sensor(msgtype, GPS_NMEA_LOW_FRESHNESS, false);
}
if(reply.size()) {
if(reply.substr(1, 5) == msgtype) return reply;
}
boost::this_thread::sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
}
throw uhd::value_error(str(boost::format("get_nmea(): no %s message found") % msgtype));
}
//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) {
_flush();
int error_cnt = 0;
ptime gps_time;
while(error_cnt < 2) {
try {
std::string reply;
if (gps_type == GPS_TYPE_LEA_M8F) {
reply = get_nmea("GNRMC");
}
else {
reply = get_nmea("GPRMC");
}
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));
}
//just trust me on this one
gps_time = ptime( date(
greg_year(boost::lexical_cast(datestr.substr(4, 2)) + 2000),
greg_month(boost::lexical_cast(datestr.substr(2, 2))),
greg_day(boost::lexical_cast(datestr.substr(0, 2)))
),
hours( boost::lexical_cast(timestr.substr(0, 2)))
+ minutes(boost::lexical_cast(timestr.substr(2, 2)))
+ seconds(boost::lexical_cast(timestr.substr(4, 2)))
);
return gps_time;
} catch(std::exception &e) {
UHD_MSG(warning) << "get_time: " << e.what() << std::endl;
_flush();
error_cnt++;
}
}
throw uhd::value_error("Timeout after no valid message found");
return gps_time; //keep gcc from complaining
}
time_t get_epoch_time(void) {
return (get_time() - from_time_t(0)).total_seconds();
}
bool gps_detected_lea_m8f(void) {
return (gps_type == GPS_TYPE_LEA_M8F);
}
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;
if (gps_type == GPS_TYPE_LEA_M8F) {
reply = get_cached_sensor("FIXTYPE", GPS_LOCK_FRESHNESS, false, false);
UHD_MSG(warning) << "FIXTYPE is " << reply << std::endl;
return reply == "3d fix";
}
else {
reply = get_cached_sensor("GPGGA", GPS_LOCK_FRESHNESS, false, false);
if(reply.size() <= 1) return false;
return (get_token(reply, 6) != "0");
}
} catch(std::exception &e) {
UHD_MSG(warning) << "locked: " << e.what() << std::endl;
error_cnt++;
}
}
throw uhd::value_error("Timeout after no valid message found");
return false;
}
std::string get_servo(void) {
//enable servo reporting
_send("SERV:TRAC 1\n");
sleep(milliseconds(GPSDO_COMMAND_DELAY_MS));
std::string reply;
const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
while(boost::get_system_time() < comm_timeout) {
reply = get_cached_sensor("SERVO", GPS_NMEA_LOW_FRESHNESS, false);
if(reply.size())
{
//disable it before leaving function
_send("SERV:TRAC 0\n");
return reply;
}
boost::this_thread::sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
}
throw uhd::value_error("get_stat(): no servo message found");
}
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_LEA_M8F,
GPS_TYPE_GENERIC_NMEA,
GPS_TYPE_NONE
} gps_type;
static const int GPS_COMM_TIMEOUT_MS = 2300;
static const int GPS_NMEA_FRESHNESS = 10;
static const int GPS_NMEA_LOW_FRESHNESS = 2500;
static const int GPS_NMEA_NORMAL_FRESHNESS = 1000;
static const int GPS_SERVO_FRESHNESS = 2500;
static const int GPS_LOCK_FRESHNESS = 2500;
static const int GPS_TIMEOUT_DELAY_MS = 200;
static const int GPSDO_COMMAND_DELAY_MS = 200;
};
/***********************************************************************
* Public make function for the GPS control
**********************************************************************/
gps_ctrl::sptr gps_ctrl::make(uart_iface::sptr uart){
return sptr(new gps_ctrl_impl(uart));
}