Revision 7af605b2 host/lib/usrp/gps_ctrl.cpp
| b/host/lib/usrp/gps_ctrl.cpp | ||
|---|---|---|
| 17 | 17 |
|
| 18 | 18 |
#include <uhd/usrp/gps_ctrl.hpp> |
| 19 | 19 |
#include <uhd/utils/msg.hpp> |
| 20 |
#include <uhd/utils/props.hpp> |
|
| 20 | 21 |
#include <uhd/exception.hpp> |
| 22 |
#include <uhd/types/sensors.hpp> |
|
| 23 |
#include <boost/algorithm/string.hpp> |
|
| 21 | 24 |
#include <boost/cstdint.hpp> |
| 22 | 25 |
#include <boost/date_time/posix_time/posix_time.hpp> |
| 23 | 26 |
#include <boost/thread/thread.hpp> |
| 24 |
#include <boost/algorithm/string/trim.hpp> |
|
| 25 | 27 |
#include <boost/tokenizer.hpp> |
| 26 | 28 |
|
| 27 | 29 |
using namespace uhd; |
| ... | ... | |
| 42 | 44 |
|
| 43 | 45 |
std::string reply; |
| 44 | 46 |
bool i_heard_some_nmea = false, i_heard_something_weird = false; |
| 45 |
|
|
| 46 | 47 |
gps_type = GPS_TYPE_NONE; |
| 47 |
|
|
| 48 |
// set_uart_baud_rate(GPS_UART, 115200); |
|
| 49 |
//first we look for a Jackson Labs Firefly (since that's what we sell with the USRP2+...) |
|
| 50 |
|
|
| 48 |
|
|
| 49 |
//first we look for a Jackson Labs Firefly (since that's what we provide...) |
|
| 51 | 50 |
_recv(); //get whatever junk is in the rx buffer right now, and throw it away |
| 52 | 51 |
_send("HAAAY GUYYYYS\n"); //to elicit a response from the Firefly
|
| 53 | 52 |
|
| ... | ... | |
| 55 | 54 |
int timeout = GPS_TIMEOUT_TRIES; |
| 56 | 55 |
while(timeout--) {
|
| 57 | 56 |
reply = safe_gps_read(); |
| 58 |
if(trim_right_copy(reply) == "Command Error") {
|
|
| 57 |
if(boost::trim_right_copy(reply) == "Command Error") {
|
|
| 59 | 58 |
gps_type = GPS_TYPE_JACKSON_LABS; |
| 60 | 59 |
break; |
| 61 | 60 |
} |
| ... | ... | |
| 66 | 65 |
|
| 67 | 66 |
if((i_heard_some_nmea) && (gps_type != GPS_TYPE_JACKSON_LABS)) gps_type = GPS_TYPE_GENERIC_NMEA; |
| 68 | 67 |
|
| 69 |
//otherwise, we can try some other common baud rates looking to see if a GPS is connected (todo, later) |
|
| 70 | 68 |
if((gps_type == GPS_TYPE_NONE) && i_heard_something_weird) {
|
| 71 | 69 |
UHD_MSG(error) << "GPS invalid reply \"" << reply << "\", assuming none available" << std::endl; |
| 72 | 70 |
} |
| ... | ... | |
| 84 | 82 |
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS)); |
| 85 | 83 |
_send("SYST:COMM:SER:PRO OFF\n");
|
| 86 | 84 |
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS)); |
| 87 |
_send("GPS:GPGGA 0\n");
|
|
| 85 |
_send("GPS:GPGGA 1\n");
|
|
| 88 | 86 |
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS)); |
| 89 | 87 |
_send("GPS:GGAST 0\n");
|
| 90 | 88 |
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS)); |
| 91 | 89 |
_send("GPS:GPRMC 1\n");
|
| 92 | 90 |
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS)); |
| 93 |
|
|
| 91 |
_send("GPS:GPGSA 1\n");
|
|
| 92 |
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS)); |
|
| 94 | 93 |
// break; |
| 95 | 94 |
|
| 96 | 95 |
case GPS_TYPE_GENERIC_NMEA: |
| ... | ... | |
| 104 | 103 |
found_gprmc = true; |
| 105 | 104 |
break; |
| 106 | 105 |
} |
| 107 |
boost::this_thread::sleep(boost::posix_time::milliseconds(200));
|
|
| 106 |
boost::this_thread::sleep(boost::posix_time::milliseconds(GPS_TIMEOUT_DELAY_MS));
|
|
| 108 | 107 |
} |
| 109 | 108 |
if(!found_gprmc) {
|
| 110 | 109 |
if(gps_type == GPS_TYPE_JACKSON_LABS) UHD_MSG(error) << "Firefly GPS not locked or warming up." << std::endl; |
| ... | ... | |
| 118 | 117 |
break; |
| 119 | 118 |
|
| 120 | 119 |
} |
| 121 |
|
|
| 122 |
|
|
| 123 | 120 |
} |
| 124 | 121 |
|
| 125 | 122 |
~gps_ctrl_impl(void){
|
| ... | ... | |
| 132 | 129 |
|
| 133 | 130 |
ptime get_time(void) {
|
| 134 | 131 |
std::string reply; |
| 135 |
ptime now; |
|
| 136 | 132 |
boost::tokenizer<boost::escaped_list_separator<char> > tok(reply); |
| 137 | 133 |
std::vector<std::string> toked; |
| 138 |
int timeout = GPS_TIMEOUT_TRIES; |
|
| 139 |
bool found_gprmc = false; |
|
| 140 |
switch(gps_type) {
|
|
| 141 |
case GPS_TYPE_JACKSON_LABS: //deprecated in favor of a single NMEA parser |
|
| 142 |
case GPS_TYPE_GENERIC_NMEA: |
|
| 143 | 134 |
|
| 144 |
while(timeout--) {
|
|
| 135 |
reply = get_nmea("GPRMC");
|
|
| 136 |
//make sure we got something |
|
| 137 |
if(reply.size() <= 1) {
|
|
| 138 |
return ptime(); |
|
| 139 |
} |
|
| 140 |
|
|
| 141 |
tok.assign(reply); |
|
| 142 |
toked.assign(tok.begin(), tok.end()); |
|
| 143 |
|
|
| 144 |
if(not toked.size() == 12) {
|
|
| 145 |
UHD_MSG(error) << "get_time(): invalid GPRMC time sentence received."; |
|
| 146 |
return ptime(); |
|
| 147 |
} |
|
| 148 |
|
|
| 149 |
return ptime( date( |
|
| 150 |
greg_year(boost::lexical_cast<int>(toked[9].substr(4, 2)) + 2000), //just trust me on this one |
|
| 151 |
greg_month(boost::lexical_cast<int>(toked[9].substr(2, 2))), |
|
| 152 |
greg_day(boost::lexical_cast<int>(toked[9].substr(0, 2))) |
|
| 153 |
), |
|
| 154 |
hours( boost::lexical_cast<int>(toked[1].substr(0, 2))) |
|
| 155 |
+ minutes(boost::lexical_cast<int>(toked[1].substr(2, 2))) |
|
| 156 |
+ seconds(boost::lexical_cast<int>(toked[1].substr(4, 2))) |
|
| 157 |
); |
|
| 158 |
} |
|
| 159 |
|
|
| 160 |
//retrieve a raw NMEA sentence for user parsing |
|
| 161 |
std::string get_nmea(std::string type) {
|
|
| 162 |
type.insert(0, "$"); |
|
| 163 |
std::string reply; |
|
| 164 |
if(not gps_detected()) {
|
|
| 165 |
UHD_MSG(error) << "get_nmea(): unsupported GPS or no GPS detected"; |
|
| 166 |
return std::string(); |
|
| 167 |
} |
|
| 168 |
int timeout = GPS_TIMEOUT_TRIES; |
|
| 169 |
while(timeout--) {
|
|
| 145 | 170 |
reply = safe_gps_read(); |
| 146 |
if(reply.substr(0, 6) == "$GPRMC") {
|
|
| 147 |
found_gprmc = true; |
|
| 148 |
break; |
|
| 149 |
} |
|
| 150 |
boost::this_thread::sleep(boost::posix_time::milliseconds(200)); |
|
| 151 |
} |
|
| 152 |
UHD_ASSERT_THROW(found_gprmc); |
|
| 153 |
|
|
| 154 |
tok.assign(reply); |
|
| 155 |
toked.assign(tok.begin(), tok.end()); |
|
| 156 |
|
|
| 157 |
UHD_ASSERT_THROW(toked.size() == 12); //if it's not we got something weird in there |
|
| 158 |
|
|
| 159 |
now = ptime( date( |
|
| 160 |
greg_year(boost::lexical_cast<int>(toked[9].substr(4, 2)) + 2000), //just trust me on this one |
|
| 161 |
greg_month(boost::lexical_cast<int>(toked[9].substr(2, 2))), |
|
| 162 |
greg_day(boost::lexical_cast<int>(toked[9].substr(0, 2))) |
|
| 163 |
), |
|
| 164 |
hours( boost::lexical_cast<int>(toked[1].substr(0, 2))) |
|
| 165 |
+ minutes(boost::lexical_cast<int>(toked[1].substr(2, 2))) |
|
| 166 |
+ seconds(boost::lexical_cast<int>(toked[1].substr(4, 2))) |
|
| 167 |
); |
|
| 168 |
break; |
|
| 169 |
case GPS_TYPE_NONE: |
|
| 170 |
default: |
|
| 171 |
throw uhd::runtime_error("get_time(): Unsupported GPS or no GPS detected\n");
|
|
| 172 |
break; |
|
| 171 |
if(reply.substr(0, 6) == type) |
|
| 172 |
return reply; |
|
| 173 |
boost::this_thread::sleep(boost::posix_time::milliseconds(GPS_TIMEOUT_DELAY_MS)); |
|
| 173 | 174 |
} |
| 174 |
return now; |
|
| 175 |
UHD_MSG(error) << "get_nmea(): no " << type << " message found"; |
|
| 176 |
return std::string(); |
|
| 175 | 177 |
} |
| 176 | 178 |
|
| 177 | 179 |
time_t get_epoch_time(void) {
|
| ... | ... | |
| 182 | 184 |
return (gps_type != GPS_TYPE_NONE); |
| 183 | 185 |
} |
| 184 | 186 |
|
| 187 |
//return a list of supported sensors |
|
| 188 |
std::vector<std::string> get_sensors(void) {
|
|
| 189 |
std::vector<std::string> ret; |
|
| 190 |
ret.push_back("gps_gpgga");
|
|
| 191 |
ret.push_back("gps_gprmc");
|
|
| 192 |
ret.push_back("gps_gpgsa");
|
|
| 193 |
ret.push_back("gps_time");
|
|
| 194 |
return ret; |
|
| 195 |
} |
|
| 196 |
|
|
| 197 |
uhd::sensor_value_t get_sensor(std::string key) {
|
|
| 198 |
if(key == "gps_gpgga" |
|
| 199 |
or key == "gps_gprmc" |
|
| 200 |
or key == "gps_gpgsa") {
|
|
| 201 |
return sensor_value_t( |
|
| 202 |
boost::to_upper_copy(key), |
|
| 203 |
get_nmea(boost::to_upper_copy(key.substr(4,8))), |
|
| 204 |
""); |
|
| 205 |
} |
|
| 206 |
else if(key == "gps_time") {
|
|
| 207 |
return sensor_value_t("GPS epoch time", int(get_epoch_time()), "seconds");
|
|
| 208 |
} |
|
| 209 |
else {
|
|
| 210 |
UHD_THROW_PROP_GET_ERROR(); |
|
| 211 |
} |
|
| 212 |
} |
|
| 213 |
|
|
| 185 | 214 |
private: |
| 186 | 215 |
gps_send_fn_t _send; |
| 187 | 216 |
gps_recv_fn_t _recv; |
Also available in: Unified diff