Statistics
| Branch: | Tag: | Revision:

root / host / lib / usrp / gps_ctrl.cpp @ db5eb382

History | View | Annotate | Download (8.95 KB)

1 f09d9820 Nick Foster
//
2 95b966a5 Josh Blum
// Copyright 2010-2011 Ettus Research LLC
3 f09d9820 Nick Foster
//
4
// This program is free software: you can redistribute it and/or modify
5
// it under the terms of the GNU General Public License as published by
6
// the Free Software Foundation, either version 3 of the License, or
7
// (at your option) any later version.
8
//
9
// This program is distributed in the hope that it will be useful,
10
// but WITHOUT ANY WARRANTY; without even the implied warranty of
11
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12
// GNU General Public License for more details.
13
//
14
// You should have received a copy of the GNU General Public License
15
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
//
17
18 330a014d Nick Foster
#include <uhd/usrp/gps_ctrl.hpp>
19 09be0518 Josh Blum
#include <uhd/utils/msg.hpp>
20 16f08844 Josh Blum
#include <uhd/exception.hpp>
21 7af605b2 Nick Foster
#include <uhd/types/sensors.hpp>
22
#include <boost/algorithm/string.hpp>
23 ab63a540 Nick Foster
#include <boost/assign/list_of.hpp>
24 f09d9820 Nick Foster
#include <boost/cstdint.hpp>
25
#include <boost/date_time/posix_time/posix_time.hpp>
26 16f08844 Josh Blum
#include <boost/thread/thread.hpp>
27 727d8711 Nick Foster
#include <boost/tokenizer.hpp>
28 8009a340 Nick Foster
#include <boost/format.hpp>
29 f09d9820 Nick Foster
30
using namespace uhd;
31
using namespace boost::gregorian;
32
using namespace boost::posix_time;
33 727d8711 Nick Foster
using namespace boost::algorithm;
34 ab63a540 Nick Foster
using namespace boost::this_thread;
35 f09d9820 Nick Foster
36
/*!
37 330a014d Nick Foster
 * A GPS control for Jackson Labs devices (and other NMEA compatible GPS's)
38 f09d9820 Nick Foster
 */
39
40 330a014d Nick Foster
class gps_ctrl_impl : public gps_ctrl{
41 f09d9820 Nick Foster
public:
42 25494489 Josh Blum
  gps_ctrl_impl(uart_iface::sptr uart){
43
    _uart = uart;
44 f09d9820 Nick Foster
45
    std::string reply;
46 40faee2e Nick Foster
    bool i_heard_some_nmea = false, i_heard_something_weird = false;
47
    gps_type = GPS_TYPE_NONE;
48 7af605b2 Nick Foster
    
49
    //first we look for a Jackson Labs Firefly (since that's what we provide...)
50 25494489 Josh Blum
    _flush(); //get whatever junk is in the rx buffer right now, and throw it away
51 330a014d Nick Foster
    _send("HAAAY GUYYYYS\n"); //to elicit a response from the Firefly
52 40faee2e Nick Foster
53 25494489 Josh Blum
    //wait for _send(...) to return
54 cbd7ff3a Nick Foster
    sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
55
56 40faee2e Nick Foster
    //then we loop until we either timeout, or until we get a response that indicates we're a JL device
57 0d9421b7 Josh Blum
    const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
58
    while(boost::get_system_time() < comm_timeout) {
59 ab63a540 Nick Foster
      reply = _recv();
60
      if(reply.find("Command Error") != std::string::npos) {
61 40faee2e Nick Foster
        gps_type = GPS_TYPE_JACKSON_LABS;
62
        break;
63 bf25ec24 Nick Foster
      } 
64 40faee2e Nick Foster
      else if(reply.substr(0, 3) == "$GP") i_heard_some_nmea = true; //but keep looking for that "Command Error" response
65
      else if(reply.length() != 0) i_heard_something_weird = true; //probably wrong baud rate
66 0d9421b7 Josh Blum
      sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
67 f09d9820 Nick Foster
    }
68
69 40faee2e Nick Foster
    if((i_heard_some_nmea) && (gps_type != GPS_TYPE_JACKSON_LABS)) gps_type = GPS_TYPE_GENERIC_NMEA;
70
71
    if((gps_type == GPS_TYPE_NONE) && i_heard_something_weird) {
72 09be0518 Josh Blum
      UHD_MSG(error) << "GPS invalid reply \"" << reply << "\", assuming none available" << std::endl;
73 40faee2e Nick Foster
    }
74
75 f09d9820 Nick Foster
    switch(gps_type) {
76
    case GPS_TYPE_JACKSON_LABS:
77 09be0518 Josh Blum
      UHD_MSG(status) << "Found a Jackson Labs GPS" << std::endl;
78 ab63a540 Nick Foster
      init_firefly();
79 8009a340 Nick Foster
      break;
80 f09d9820 Nick Foster
81
    case GPS_TYPE_GENERIC_NMEA:
82 09be0518 Josh Blum
      if(gps_type == GPS_TYPE_GENERIC_NMEA) UHD_MSG(status) << "Found a generic NMEA GPS device" << std::endl;
83 40faee2e Nick Foster
      break;
84
85 f09d9820 Nick Foster
    case GPS_TYPE_NONE:
86
    default:
87
      break;
88 40faee2e Nick Foster
89 f09d9820 Nick Foster
    }
90
  }
91
92 330a014d Nick Foster
  ~gps_ctrl_impl(void){
93 ab63a540 Nick Foster
    /* NOP */
94 f09d9820 Nick Foster
  }
95
96 ab63a540 Nick Foster
  //return a list of supported sensors
97
  std::vector<std::string> get_sensors(void) {
98
    std::vector<std::string> ret = boost::assign::list_of
99
        ("gps_gpgga")
100
        ("gps_gprmc")
101
        ("gps_gpgsa")
102
        ("gps_time")
103
        ("gps_locked");
104
    return ret;
105 40faee2e Nick Foster
  }
106
107 ab63a540 Nick Foster
  uhd::sensor_value_t get_sensor(std::string key) {
108
    if(key == "gps_gpgga"
109
    or key == "gps_gprmc"
110
    or key == "gps_gpgsa") {
111
        return sensor_value_t(
112
                 boost::to_upper_copy(key),
113
                 get_nmea(boost::to_upper_copy(key.substr(4,8))),
114
                 "");
115 7af605b2 Nick Foster
    }
116 ab63a540 Nick Foster
    else if(key == "gps_time") {
117
        return sensor_value_t("GPS epoch time", int(get_epoch_time()), "seconds");
118
    }
119
    else if(key == "gps_locked") {
120
        return sensor_value_t("GPS lock status", locked(), "locked", "unlocked");
121
    }
122
    else {
123 902818f5 Josh Blum
        throw uhd::value_error("gps ctrl get_sensor unknown key: " + key);
124 7af605b2 Nick Foster
    }
125
  }
126
127 ab63a540 Nick Foster
private:
128
  void init_firefly(void) {
129
    //issue some setup stuff so it spits out the appropriate data
130
    //none of these should issue replies so we don't bother looking for them
131
    //we have to sleep between commands because the JL device, despite not acking, takes considerable time to process each command.
132
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
133
    _send("SYST:COMM:SER:ECHO OFF\n");
134
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
135
    _send("SYST:COMM:SER:PRO OFF\n");
136
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
137
    _send("GPS:GPGGA 1\n");
138
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
139
    _send("GPS:GGAST 0\n");
140
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
141
    _send("GPS:GPRMC 1\n");
142
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
143
    _send("GPS:GPGSA 1\n");
144
     sleep(milliseconds(FIREFLY_STUPID_DELAY_MS));
145
  }
146
 
147
  //retrieve a raw NMEA sentence
148
  std::string get_nmea(std::string msgtype) {
149
    msgtype.insert(0, "$");
150 7af605b2 Nick Foster
    std::string reply;
151
    if(not gps_detected()) {
152
        UHD_MSG(error) << "get_nmea(): unsupported GPS or no GPS detected";
153
        return std::string();
154
    }
155 8009a340 Nick Foster
156 25494489 Josh Blum
    _flush(); //flush all input before waiting for a message
157 8009a340 Nick Foster
158 0d9421b7 Josh Blum
    const boost::system_time comm_timeout = boost::get_system_time() + milliseconds(GPS_COMM_TIMEOUT_MS);
159
    while(boost::get_system_time() < comm_timeout) {
160 ab63a540 Nick Foster
        reply = _recv();
161
        if(reply.substr(0, 6) == msgtype)
162 7af605b2 Nick Foster
          return reply;
163 ab63a540 Nick Foster
        boost::this_thread::sleep(milliseconds(GPS_TIMEOUT_DELAY_MS));
164 f09d9820 Nick Foster
    }
165 8009a340 Nick Foster
    throw uhd::value_error(str(boost::format("get_nmea(): no %s message found") % msgtype));
166 7af605b2 Nick Foster
    return std::string();
167 f09d9820 Nick Foster
  }
168 ab63a540 Nick Foster
169
  //helper function to retrieve a field from an NMEA sentence
170
  std::string get_token(std::string sentence, size_t offset) {
171
    boost::tokenizer<boost::escaped_list_separator<char> > tok(sentence);
172
    std::vector<std::string> toked;
173 8009a340 Nick Foster
    tok.assign(sentence); //this can throw
174 ab63a540 Nick Foster
    toked.assign(tok.begin(), tok.end());
175
176
    if(toked.size() <= offset) {
177 8009a340 Nick Foster
        throw uhd::value_error(str(boost::format("Invalid response \"%s\"") % sentence));
178 ab63a540 Nick Foster
    }
179
    return toked[offset];
180
  }
181
182
  ptime get_time(void) {
183 8009a340 Nick Foster
    int error_cnt = 0;
184
    ptime gps_time;
185
    while(error_cnt < 3) {
186
        try {
187
            std::string reply = get_nmea("GPRMC");
188 ab63a540 Nick Foster
189 8009a340 Nick Foster
            std::string datestr = get_token(reply, 9);
190
            std::string timestr = get_token(reply, 1);
191 ab63a540 Nick Foster
192 8009a340 Nick Foster
            if(datestr.size() == 0 or timestr.size() == 0) {
193
                throw uhd::value_error(str(boost::format("Invalid response \"%s\"") % reply));
194
            }
195
            
196
            //just trust me on this one
197
            gps_time = ptime( date( 
198
                             greg_year(boost::lexical_cast<int>(datestr.substr(4, 2)) + 2000),
199
                             greg_month(boost::lexical_cast<int>(datestr.substr(2, 2))), 
200
                             greg_day(boost::lexical_cast<int>(datestr.substr(0, 2))) 
201
                           ),
202
                          hours(  boost::lexical_cast<int>(timestr.substr(0, 2)))
203
                        + minutes(boost::lexical_cast<int>(timestr.substr(2, 2)))
204
                        + seconds(boost::lexical_cast<int>(timestr.substr(4, 2)))
205
                     );
206
            return gps_time;
207
            
208
        } catch(std::exception &e) {
209
            UHD_MSG(warning) << "get_time: " << e.what();
210
            error_cnt++;
211
        }
212 ab63a540 Nick Foster
    }
213 8009a340 Nick Foster
    throw uhd::value_error("Timeout after no valid message found");
214
    
215
    return gps_time; //keep gcc from complaining
216 ab63a540 Nick Foster
  }
217 a4aa89ed Nick Foster
  
218
  time_t get_epoch_time(void) {
219 ab63a540 Nick Foster
      return (get_time() - from_time_t(0)).total_seconds();
220 a4aa89ed Nick Foster
  }
221 f09d9820 Nick Foster
222
  bool gps_detected(void) {
223
    return (gps_type != GPS_TYPE_NONE);
224
  }
225
226 6f6364f7 Nick Foster
  bool locked(void) {
227 8009a340 Nick Foster
    int error_cnt = 0;
228
    while(error_cnt < 3) {
229
        try {
230
            std::string reply = get_nmea("GPGGA");
231
            if(reply.size() <= 1) return false;
232 6f6364f7 Nick Foster
233 8009a340 Nick Foster
            return (get_token(reply, 6) != "0");
234
        } catch(std::exception &e) {
235
            UHD_MSG(warning) << "locked: " << e.what();
236
            error_cnt++;
237
        }
238
    }
239
    throw uhd::value_error("Timeout after no valid message found");
240
    return false;
241 7af605b2 Nick Foster
  }
242
243 25494489 Josh Blum
  uart_iface::sptr _uart;
244
245
  void _flush(void){
246
    while (not _uart->read_uart(0.0).empty()){
247
        //NOP
248
    }
249
  }
250
251
  std::string _recv(void){
252
      return _uart->read_uart(GPS_TIMEOUT_DELAY_MS/1000.);
253
  }
254
255
  void _send(const std::string &buf){
256
      return _uart->write_uart(buf);
257
  }
258 f09d9820 Nick Foster
259
  enum {
260
    GPS_TYPE_JACKSON_LABS,
261
    GPS_TYPE_GENERIC_NMEA,
262
    GPS_TYPE_NONE
263
  } gps_type;
264
265 0d9421b7 Josh Blum
  static const int GPS_COMM_TIMEOUT_MS = 1500;
266 330a014d Nick Foster
  static const int GPS_TIMEOUT_DELAY_MS = 200;
267 40faee2e Nick Foster
  static const int FIREFLY_STUPID_DELAY_MS = 200;
268 f09d9820 Nick Foster
};
269
270
/***********************************************************************
271
 * Public make function for the GPS control
272
 **********************************************************************/
273 25494489 Josh Blum
gps_ctrl::sptr gps_ctrl::make(uart_iface::sptr uart){
274
    return sptr(new gps_ctrl_impl(uart));
275 f09d9820 Nick Foster
}