1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
|
//
// Copyright 2010 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 <http://www.gnu.org/licenses/>.
//
#include <uhd/usrp/gps_ctrl.hpp>
#include <uhd/exception.hpp>
#include <boost/cstdint.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/tokenizer.hpp>
using namespace uhd;
using namespace boost::gregorian;
using namespace boost::posix_time;
using namespace boost::algorithm;
/*!
* A GPS control for Jackson Labs devices (and other NMEA compatible GPS's)
*/
//TODO: multiple baud rate support (requires mboard_impl changes for poking UART registers)
class gps_ctrl_impl : public gps_ctrl{
public:
gps_ctrl_impl(gps_send_fn_t send, gps_recv_fn_t recv){
_send = send;
_recv = recv;
std::string reply;
bool i_heard_some_nmea = false, i_heard_something_weird = false;
gps_type = GPS_TYPE_NONE;
// set_uart_baud_rate(GPS_UART, 115200);
//first we look for a Jackson Labs Firefly (since that's what we sell with the USRP2+...)
_recv(); //get whatever junk is in the rx buffer right now, and throw it away
_send("HAAAY GUYYYYS\n"); //to elicit a response from the Firefly
//then we loop until we either timeout, or until we get a response that indicates we're a JL device
int timeout = GPS_TIMEOUT_TRIES;
while(timeout--) {
reply = safe_gps_read();
if(trim_right_copy(reply) == "Command Error") {
gps_type = GPS_TYPE_JACKSON_LABS;
break;
}
else if(reply.substr(0, 3) == "$GP") i_heard_some_nmea = true; //but keep looking for that "Command Error" response
else if(reply.length() != 0) i_heard_something_weird = true; //probably wrong baud rate
boost::this_thread::sleep(boost::posix_time::milliseconds(200));
}
if((i_heard_some_nmea) && (gps_type != GPS_TYPE_JACKSON_LABS)) gps_type = GPS_TYPE_GENERIC_NMEA;
//otherwise, we can try some other common baud rates looking to see if a GPS is connected (todo, later)
if((gps_type == GPS_TYPE_NONE) && i_heard_something_weird) {
std::cout << "GPS invalid reply \"" << reply << "\", assuming none available" << std::endl;
}
bool found_gprmc = false;
switch(gps_type) {
case GPS_TYPE_JACKSON_LABS:
std::cout << "Found a Jackson Labs GPS" << std::endl;
//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.
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
_send("SYST:COMM:SER:ECHO OFF\n");
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
_send("SYST:COMM:SER:PRO OFF\n");
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
_send("GPS:GPGGA 0\n");
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
_send("GPS:GGAST 0\n");
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
_send("GPS:GPRMC 1\n");
boost::this_thread::sleep(boost::posix_time::milliseconds(FIREFLY_STUPID_DELAY_MS));
// break;
case GPS_TYPE_GENERIC_NMEA:
if(gps_type == GPS_TYPE_GENERIC_NMEA) std::cout << "Found a generic NMEA GPS device" << std::endl;
found_gprmc = false;
//here we loop around looking for a GPRMC packet. if we don't get one, we don't have a usable GPS.
timeout = GPS_TIMEOUT_TRIES;
while(timeout--) {
reply = safe_gps_read();
if(reply.substr(0, 6) == "$GPRMC") {
found_gprmc = true;
break;
}
boost::this_thread::sleep(boost::posix_time::milliseconds(200));
}
if(!found_gprmc) {
if(gps_type == GPS_TYPE_JACKSON_LABS) std::cout << "Firefly GPS not locked or warming up." << std::endl;
else std::cout << "GPS does not output GPRMC packets. Cannot retrieve time." << std::endl;
gps_type = GPS_TYPE_NONE;
}
break;
case GPS_TYPE_NONE:
default:
break;
}
}
~gps_ctrl_impl(void){
}
//TODO: this isn't generalizeable to non-USRP2 USRPs.
std::string safe_gps_read() {
return _recv();
}
ptime get_time(void) {
std::string reply;
ptime now;
boost::tokenizer<boost::escaped_list_separator<char> > tok(reply);
std::vector<std::string> toked;
int timeout = GPS_TIMEOUT_TRIES;
bool found_gprmc = false;
switch(gps_type) {
case GPS_TYPE_JACKSON_LABS: //deprecated in favor of a single NMEA parser
case GPS_TYPE_GENERIC_NMEA:
while(timeout--) {
reply = safe_gps_read();
if(reply.substr(0, 6) == "$GPRMC") {
found_gprmc = true;
break;
}
}
UHD_ASSERT_THROW(found_gprmc);
tok.assign(reply);
toked.assign(tok.begin(), tok.end());
UHD_ASSERT_THROW(toked.size() == 11); //if it's not we got something weird in there
now = ptime( date(
greg_year(boost::lexical_cast<int>(toked[8].substr(4, 2)) + 2000), //just trust me on this one
greg_month(boost::lexical_cast<int>(toked[8].substr(2, 2))),
greg_day(boost::lexical_cast<int>(toked[8].substr(0, 2)))
),
hours( boost::lexical_cast<int>(toked[1].substr(0, 2)))
+ minutes(boost::lexical_cast<int>(toked[1].substr(2, 2)))
+ seconds(boost::lexical_cast<int>(toked[1].substr(4, 2)))
);
break;
case GPS_TYPE_NONE:
default:
throw std::runtime_error("get_time(): Unsupported GPS or no GPS detected\n");
break;
}
return now;
}
bool gps_detected(void) {
return (gps_type != GPS_TYPE_NONE);
}
private:
gps_send_fn_t _send;
gps_recv_fn_t _recv;
enum {
GPS_TYPE_JACKSON_LABS,
GPS_TYPE_GENERIC_NMEA,
GPS_TYPE_NONE
} gps_type;
static const int GPS_TIMEOUT_TRIES = 10;
static const int GPS_TIMEOUT_DELAY_MS = 200;
static const int FIREFLY_STUPID_DELAY_MS = 200;
};
/***********************************************************************
* Public make function for the GPS control
**********************************************************************/
gps_ctrl::sptr gps_ctrl::make(gps_send_fn_t send, gps_recv_fn_t recv){
return sptr(new gps_ctrl_impl(send, recv));
}
|
