diff options
Diffstat (limited to 'host')
| -rw-r--r-- | host/lib/usrp/gpsd_iface.cpp | 65 |
1 files changed, 64 insertions, 1 deletions
diff --git a/host/lib/usrp/gpsd_iface.cpp b/host/lib/usrp/gpsd_iface.cpp index 62b6939c5..5dbeebb97 100644 --- a/host/lib/usrp/gpsd_iface.cpp +++ b/host/lib/usrp/gpsd_iface.cpp @@ -64,7 +64,7 @@ public: _bthread.swap(t); - _sensors = boost::assign::list_of("gps_locked")("gps_time")("gps_position"); + _sensors = boost::assign::list_of("gps_locked")("gps_time")("gps_position")("gps_gprmc"); } virtual ~gpsd_iface_impl(void) @@ -90,6 +90,9 @@ public: } else if (key == "gps_time") { return sensor_value_t( "GPS epoch time", int(_epoch_time()), "seconds"); + } else if (key == "gps_gprmc") { + return sensor_value_t( + "GPRMC", _gps_gprmc(), ""); } else if (key == "gps_position") { return sensor_value_t( "GPS Position", str( @@ -164,6 +167,66 @@ private: // member functions return tmp; } + float _zeroize(float x) + { + return std::isnan(x) ? 0.0 : x; + } + + int _nmea_checksum(const std::string &s) + { + if ((s.at(0) != '$')) + return 0; + + boost::uint8_t sum = '\0'; + for (size_t i = 1; i < s.size(); i++) + sum ^= static_cast<boost::uint8_t>(s.at(i)); + + return sum; + } + + double _deg_to_dm(double angle) + { + double fraction, integer; + fraction = std::modf(angle, &integer); + return std::floor(angle) * 100 + fraction * 60; + } + + std::string _gps_gprmc(void) + { + struct tm tm; + time_t intfixtime; + + boost::shared_lock<boost::shared_mutex> l(_d_mutex); + + tm.tm_mday = tm.tm_mon = tm.tm_year = 0; + tm.tm_hour = tm.tm_min = tm.tm_sec = 0; + + if (std::isnan(_gps_data.fix.time) == 0) { + intfixtime = (time_t) _gps_data.fix.time; + (void)gmtime_r(&intfixtime, &tm); + tm.tm_mon++; + tm.tm_year %= 100; + } + std::string string = str(boost::format( + "$GPRMC,%02d%02d%02d,%c,%09.4f,%c,%010.4f,%c,%.4f,%.3f,%02d%02d%02d,,") + % tm.tm_hour + % tm.tm_min + % tm.tm_sec + % (_gps_data.status ? 'A' : 'V') + % _zeroize(_deg_to_dm(std::fabs(_gps_data.fix.latitude))) + % ((_gps_data.fix.latitude > 0) ? 'N' : 'S') + % _zeroize(_deg_to_dm(std::fabs(_gps_data.fix.longitude))) + % ((_gps_data.fix.longitude > 0) ? 'E' : 'W') + % _zeroize(_gps_data.fix.speed * MPS_TO_KNOTS) + % _zeroize(_gps_data.fix.track) + % tm.tm_mday % tm.tm_mon % tm.tm_year); + + string.append(str( + boost::format("*%02X") % _nmea_checksum(string))); + + return string; + } + private: // members std::vector<std::string> _sensors; bool _detected; |