PX4 Firmware
PX4 Autopilot Software http://px4.io
gps.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file gps.cpp
36  * Driver for the GPS on a serial/spi port
37  */
38 
39 #ifdef __PX4_NUTTX
40 #include <nuttx/clock.h>
41 #include <nuttx/arch.h>
42 #endif
43 
44 #ifndef __PX4_QURT
45 #include <poll.h>
46 #endif
47 
48 #include <termios.h>
49 
50 #include <lib/parameters/param.h>
51 #include <mathlib/mathlib.h>
52 #include <matrix/math.hpp>
53 #include <px4_platform_common/cli.h>
54 #include <px4_platform_common/getopt.h>
55 #include <px4_platform_common/module.h>
57 #include <uORB/Subscription.hpp>
58 #include <uORB/topics/gps_dump.h>
60 
61 #include "devices/src/ashtech.h"
63 #include "devices/src/mtk.h"
64 #include "devices/src/ubx.h"
65 
66 #ifdef __PX4_LINUX
67 #include <linux/spi/spidev.h>
68 #endif /* __PX4_LINUX */
69 
70 #define TIMEOUT_5HZ 500
71 #define RATE_MEASUREMENT_PERIOD 5000000
72 
73 typedef enum {
80 
81 /* struct for dynamic allocation of satellite info data */
82 struct GPS_Sat_Info {
84 };
85 
86 static constexpr int TASK_STACK_SIZE = 1620;
87 
88 
89 class GPS : public ModuleBase<GPS>
90 {
91 public:
92 
93  /** The GPS allows to run multiple instances */
94  enum class Instance : uint8_t {
95  Main = 0,
96  Secondary,
97 
98  Count
99  };
100 
101  GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info,
102  Instance instance, unsigned configured_baudrate);
103  ~GPS() override;
104 
105  /** @see ModuleBase */
106  static int task_spawn(int argc, char *argv[]);
107 
108  /** spawn task and select the instance */
109  static int task_spawn(int argc, char *argv[], Instance instance);
110 
111  /** @see ModuleBase */
112  static GPS *instantiate(int argc, char *argv[]);
113 
114  static GPS *instantiate(int argc, char *argv[], Instance instance);
115 
116  /** @see ModuleBase */
117  static int custom_command(int argc, char *argv[]);
118 
119  /** @see ModuleBase */
120  static int print_usage(const char *reason = nullptr);
121 
122  /**
123  * task spawn trampoline for the secondary GPS
124  */
125  static int run_trampoline_secondary(int argc, char *argv[]);
126 
127  /** @see ModuleBase::run() */
128  void run() override;
129 
130  /**
131  * Diagnostics - print some basic information about the driver.
132  */
133  int print_status() override;
134 
135  /**
136  * Schedule reset of the GPS device
137  */
138  void schedule_reset(GPSRestartType restart_type);
139 
140  /**
141  * Reset device if reset was scheduled
142  */
143  void reset_if_scheduled();
144 
145 private:
146  int _serial_fd{-1}; ///< serial interface to GPS
147  unsigned _baudrate{0}; ///< current baudrate
148  const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect)
149  char _port[20] {}; ///< device / serial port path
150 
151  bool _healthy{false}; ///< flag to signal if the GPS is ok
152  bool _mode_auto; ///< if true, auto-detect which GPS is attached
153 
154  gps_driver_mode_t _mode; ///< current mode
155 
157  GPSHelper *_helper{nullptr}; ///< instance of GPS parser
158 
159  GPS_Sat_Info *_sat_info{nullptr}; ///< instance of GPS sat info data object
160 
161  vehicle_gps_position_s _report_gps_pos{}; ///< uORB topic for gps position
162  satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info
163 
164  orb_advert_t _report_gps_pos_pub{nullptr}; ///< uORB pub for gps position
165  orb_advert_t _report_sat_info_pub{nullptr}; ///< uORB pub for satellite info
166 
167  int _gps_orb_instance{-1}; ///< uORB multi-topic instance
168  int _gps_sat_orb_instance{-1}; ///< uORB multi-topic instance for satellite info
169 
170  float _rate{0.0f}; ///< position update rate
171  float _rate_rtcm_injection{0.0f}; ///< RTCM message injection rate
172  unsigned _last_rate_rtcm_injection_count{0}; ///< counter for number of RTCM messages
173 
174  const bool _fake_gps; ///< fake gps output
175 
177 
178  uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)};
179  uORB::PublicationQueued<gps_dump_s> _dump_communication_pub{ORB_ID(gps_dump)};
180  gps_dump_s *_dump_to_device{nullptr};
181  gps_dump_s *_dump_from_device{nullptr};
182  bool _should_dump_communication{false}; ///< if true, dump communication
183 
184  static volatile bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1
185  /// and thus we wait until the first one publishes at least one message.
186 
187  static volatile GPS *_secondary_instance;
188 
189  volatile GPSRestartType _scheduled_reset{GPSRestartType::None};
190 
191  /**
192  * Publish the gps struct
193  */
194  void publish();
195 
196  /**
197  * Publish the satellite info
198  */
199  void publishSatelliteInfo();
200 
201  /**
202  * This is an abstraction for the poll on serial used.
203  *
204  * @param buf: pointer to read buffer
205  * @param buf_length: size of read buffer
206  * @param timeout: timeout in ms
207  * @return: 0 for nothing read, or poll timed out
208  * < 0 for error
209  * > 0 number of bytes read
210  */
211  int pollOrRead(uint8_t *buf, size_t buf_length, int timeout);
212 
213  /**
214  * check for new messages on the inject data topic & handle them
215  */
216  void handleInjectDataTopic();
217 
218  /**
219  * send data to the device, such as an RTCM stream
220  * @param data
221  * @param len
222  */
223  inline bool injectData(uint8_t *data, size_t len);
224 
225  /**
226  * set the Baudrate
227  * @param baud
228  * @return 0 on success, <0 on error
229  */
230  int setBaudrate(unsigned baud);
231 
232  /**
233  * callback from the driver for the platform specific stuff
234  */
235  static int callback(GPSCallbackType type, void *data1, int data2, void *user);
236 
237  /**
238  * Dump gps communication.
239  * @param data message
240  * @param len length of the message
241  * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device
242  */
243  void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device);
244 
245  void initializeCommunicationDump();
246 };
247 
248 volatile bool GPS::_is_gps_main_advertised = false;
249 volatile GPS *GPS::_secondary_instance = nullptr;
250 
251 /*
252  * Driver 'main' command.
253  */
254 extern "C" __EXPORT int gps_main(int argc, char *argv[]);
255 
256 
257 GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps,
258  bool enable_sat_info, Instance instance, unsigned configured_baudrate) :
259  _configured_baudrate(configured_baudrate),
260  _mode(mode),
261  _interface(interface),
262  _fake_gps(fake_gps),
263  _instance(instance)
264 {
265  /* store port name */
266  strncpy(_port, path, sizeof(_port) - 1);
267  /* enforce null termination */
268  _port[sizeof(_port) - 1] = '\0';
269 
270  _report_gps_pos.heading = NAN;
272 
273  /* create satellite info data object if requested */
274  if (enable_sat_info) {
275  _sat_info = new GPS_Sat_Info();
277  memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
278  }
279 
281 }
282 
284 {
285  GPS *secondary_instance = (GPS *) _secondary_instance;
286 
287  if (_instance == Instance::Main && secondary_instance) {
288  secondary_instance->request_stop();
289 
290  // wait for it to exit
291  unsigned int i = 0;
292 
293  do {
294  px4_usleep(20000); // 20 ms
295  ++i;
296  } while (_secondary_instance && i < 100);
297  }
298 
299  if (_sat_info) {
300  delete (_sat_info);
301  }
302 
303  if (_dump_to_device) {
304  delete (_dump_to_device);
305  }
306 
307  if (_dump_from_device) {
308  delete (_dump_from_device);
309  }
310 
311 }
312 
313 int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
314 {
315  GPS *gps = (GPS *)user;
316 
317  switch (type) {
319  int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
320 
321  if (num_read > 0) {
322  gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false);
323  }
324 
325  return num_read;
326  }
327 
329  gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
330 
331  return write(gps->_serial_fd, data1, (size_t)data2);
332 
334  return gps->setBaudrate(data2);
335 
337  /* not used */
338  break;
339 
341  /* not used */
342  break;
343 
345  px4_clock_settime(CLOCK_REALTIME, (timespec *)data1);
346  break;
347  }
348 
349  return 0;
350 }
351 
352 int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
353 {
355 
356 #if !defined(__PX4_QURT)
357 
358  /* For non QURT, use the usual polling. */
359 
360  //Poll only for the serial data. In the same thread we also need to handle orb messages,
361  //so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the
362  //two pollings use different underlying mechanisms (at least under posix), which makes this
363  //impossible. Instead we limit the maximum polling interval and regularly check for new orb
364  //messages.
365  //FIXME: add a unified poll() API
366  const int max_timeout = 50;
367 
368  pollfd fds[1];
369  fds[0].fd = _serial_fd;
370  fds[0].events = POLLIN;
371 
372  int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout));
373 
374  if (ret > 0) {
375  /* if we have new data from GPS, go handle it */
376  if (fds[0].revents & POLLIN) {
377  /*
378  * We are here because poll says there is some data, so this
379  * won't block even on a blocking device. But don't read immediately
380  * by 1-2 bytes, wait for some more data to save expensive read() calls.
381  * If we have all requested data available, read it without waiting.
382  * If more bytes are available, we'll go back to poll() again.
383  */
384  const unsigned character_count = 32; // minimum bytes that we want to read
385  unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate;
386  const unsigned sleeptime = character_count * 1000000 / (baudrate / 10);
387 
388 #ifdef __PX4_NUTTX
389  int err = 0;
390  int bytes_available = 0;
391  err = ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
392 
393  if (err != 0 || bytes_available < (int)character_count) {
394  px4_usleep(sleeptime);
395  }
396 
397 #else
398  px4_usleep(sleeptime);
399 #endif
400 
401  ret = ::read(_serial_fd, buf, buf_length);
402 
403  } else {
404  ret = -1;
405  }
406  }
407 
408  return ret;
409 
410 #else
411  /* For QURT, just use read for now, since this doesn't block, we need to slow it down
412  * just a bit. */
413  px4_usleep(10000);
414  return ::read(_serial_fd, buf, buf_length);
415 #endif
416 }
417 
419 {
420  bool updated = false;
421 
422  // Limit maximum number of GPS injections to 6 since usually
423  // GPS injections should consist of 1-4 packets (GPS, Glonass, Baidu, Galileo).
424  // Looking at 6 packets thus guarantees, that at least a full injection
425  // data set is evaluated.
426  const size_t max_num_injections = 6;
427  size_t num_injections = 0;
428 
429  do {
430  num_injections++;
431  updated = _orb_inject_data_sub.updated();
432 
433  if (updated) {
436 
437  /* Write the message to the gps device. Note that the message could be fragmented.
438  * But as we don't write anywhere else to the device during operation, we don't
439  * need to assemble the message first.
440  */
441  injectData(msg.data, msg.len);
442 
444  }
445  } while (updated && num_injections < max_num_injections);
446 }
447 
448 bool GPS::injectData(uint8_t *data, size_t len)
449 {
450  dumpGpsData(data, len, true);
451 
452  size_t written = ::write(_serial_fd, data, len);
453  ::fsync(_serial_fd);
454  return written == len;
455 }
456 
457 int GPS::setBaudrate(unsigned baud)
458 {
459  /* process baud rate */
460  int speed;
461 
462  switch (baud) {
463  case 9600: speed = B9600; break;
464 
465  case 19200: speed = B19200; break;
466 
467  case 38400: speed = B38400; break;
468 
469  case 57600: speed = B57600; break;
470 
471  case 115200: speed = B115200; break;
472 
473  case 230400: speed = B230400; break;
474 
475  default:
476  PX4_ERR("ERR: unknown baudrate: %d", baud);
477  return -EINVAL;
478  }
479 
480  struct termios uart_config;
481 
482  int termios_state;
483 
484  /* fill the struct for the new configuration */
485  tcgetattr(_serial_fd, &uart_config);
486 
487  /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
488 
489  //
490  // Input flags - Turn off input processing
491  //
492  // convert break to null byte, no CR to NL translation,
493  // no NL to CR translation, don't mark parity errors or breaks
494  // no input parity check, don't strip high bit off,
495  // no XON/XOFF software flow control
496  //
497  uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
498  INLCR | PARMRK | INPCK | ISTRIP | IXON);
499  //
500  // Output flags - Turn off output processing
501  //
502  // no CR to NL translation, no NL to CR-NL translation,
503  // no NL to CR translation, no column 0 CR suppression,
504  // no Ctrl-D suppression, no fill characters, no case mapping,
505  // no local output processing
506  //
507  // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
508  // ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
509  uart_config.c_oflag = 0;
510 
511  //
512  // No line processing
513  //
514  // echo off, echo newline off, canonical mode off,
515  // extended input processing off, signal chars off
516  //
517  uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
518 
519  /* no parity, one stop bit, disable flow control */
520  uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
521 
522  /* set baud rate */
523  if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
524  GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
525  return -1;
526  }
527 
528  if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
529  GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
530  return -1;
531  }
532 
533  if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
534  GPS_ERR("ERR: %d (tcsetattr)", termios_state);
535  return -1;
536  }
537 
538  return 0;
539 }
540 
542 {
543  param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
544  int32_t param_dump_comm;
545 
546  if (gps_dump_comm_ph == PARAM_INVALID || param_get(gps_dump_comm_ph, &param_dump_comm) != 0) {
547  return;
548  }
549 
550  if (param_dump_comm != 1) {
551  return; //dumping disabled
552  }
553 
555  _dump_to_device = new gps_dump_s();
556 
558  PX4_ERR("failed to allocated dump data");
559  return;
560  }
561 
562  memset(_dump_to_device, 0, sizeof(gps_dump_s));
563  memset(_dump_from_device, 0, sizeof(gps_dump_s));
564 
565  //make sure to use a large enough queue size, so that we don't lose messages. You may also want
566  //to increase the logger rate for that.
568 
570 }
571 
572 void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
573 {
575  return;
576  }
577 
578  gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device;
579 
580  while (len > 0) {
581  size_t write_len = len;
582 
583  if (write_len > sizeof(dump_data->data) - dump_data->len) {
584  write_len = sizeof(dump_data->data) - dump_data->len;
585  }
586 
587  memcpy(dump_data->data + dump_data->len, data, write_len);
588  data += write_len;
589  dump_data->len += write_len;
590  len -= write_len;
591 
592  if (dump_data->len >= sizeof(dump_data->data)) {
593  if (msg_to_gps_device) {
594  dump_data->len |= 1 << 7;
595  }
596 
597  dump_data->timestamp = hrt_absolute_time();
598  _dump_communication_pub.publish(*dump_data);
599  dump_data->len = 0;
600  }
601  }
602 }
603 
604 void
606 {
607  if (!_fake_gps) {
608  /* open the serial port */
609  _serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
610 
611  if (_serial_fd < 0) {
612  PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
613  return;
614  }
615 
616 #ifdef __PX4_LINUX
617 
619  int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
620  int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
621 
622  if (status_value < 0) {
623  PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
624  return;
625  }
626 
627  status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
628 
629  if (status_value < 0) {
630  PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
631  return;
632  }
633  }
634 
635 #endif /* __PX4_LINUX */
636  }
637 
638  param_t handle = param_find("GPS_YAW_OFFSET");
639  float heading_offset = 0.f;
640 
641  if (handle != PARAM_INVALID) {
642  param_get(handle, &heading_offset);
643  heading_offset = matrix::wrap_pi(math::radians(heading_offset));
644  }
645 
646  int32_t gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration
647  handle = param_find("GPS_UBX_DYNMODEL");
648 
649  if (handle != PARAM_INVALID) {
650  param_get(handle, &gps_ubx_dynmodel);
651  }
652 
654 
655  uint64_t last_rate_measurement = hrt_absolute_time();
656  unsigned last_rate_count = 0;
657 
658  /* loop handling received serial bytes and also configuring in between */
659  while (!should_exit()) {
660 
661  if (_fake_gps) {
663  _report_gps_pos.lat = (int32_t)47.378301e7f;
664  _report_gps_pos.lon = (int32_t)8.538777e7f;
665  _report_gps_pos.alt = (int32_t)1200e3f;
670  _report_gps_pos.eph = 0.8f;
671  _report_gps_pos.epv = 1.2f;
672  _report_gps_pos.hdop = 0.9f;
673  _report_gps_pos.vdop = 0.9f;
674  _report_gps_pos.vel_n_m_s = 0.0f;
675  _report_gps_pos.vel_e_m_s = 0.0f;
676  _report_gps_pos.vel_d_m_s = 0.0f;
677  _report_gps_pos.vel_m_s = 0.0f;
678  _report_gps_pos.cog_rad = 0.0f;
681  _report_gps_pos.heading = NAN;
683 
684  /* no time and satellite information simulated */
685 
686 
687  publish();
688 
689  px4_usleep(200000);
690 
691  } else {
692 
693  if (_helper != nullptr) {
694  delete (_helper);
695  _helper = nullptr;
696  }
697 
698  switch (_mode) {
701 
702  /* FALLTHROUGH */
703  case GPS_DRIVER_MODE_UBX:
705  gps_ubx_dynmodel);
706  break;
707 
708  case GPS_DRIVER_MODE_MTK:
710  break;
711 
714  break;
715 
718  break;
719 
720  default:
721  break;
722  }
723 
725 
727 
728  /* reset report */
729  memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
730  _report_gps_pos.heading = NAN;
731  _report_gps_pos.heading_offset = heading_offset;
732 
733  if (_mode == GPS_DRIVER_MODE_UBX) {
734 
735  /* GPS is obviously detected successfully, reset statistics */
737  }
738 
739  int helper_ret;
740 
741  while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) {
742 
743  if (helper_ret & 1) {
744  publish();
745 
746  last_rate_count++;
747  }
748 
749  if (_p_report_sat_info && (helper_ret & 2)) {
751  }
752 
754 
755  /* measure update rate every 5 seconds */
756  if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
757  float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
758  _rate = last_rate_count / dt;
760  last_rate_measurement = hrt_absolute_time();
761  last_rate_count = 0;
765  }
766 
767  if (!_healthy) {
768  // Helpful for debugging, but too verbose for normal ops
769 // const char *mode_str = "unknown";
770 //
771 // switch (_mode) {
772 // case GPS_DRIVER_MODE_UBX:
773 // mode_str = "UBX";
774 // break;
775 //
776 // case GPS_DRIVER_MODE_MTK:
777 // mode_str = "MTK";
778 // break;
779 //
780 // case GPS_DRIVER_MODE_ASHTECH:
781 // mode_str = "ASHTECH";
782 // break;
783 //
784 // case GPS_DRIVER_MODE_EMLIDREACH:
785 // mode_str = "EMLID REACH";
786 // break;
787 //
788 // default:
789 // break;
790 // }
791 //
792 // PX4_WARN("module found: %s", mode_str);
793  _healthy = true;
794  }
795  }
796 
797  if (_healthy) {
798  _healthy = false;
799  _rate = 0.0f;
800  _rate_rtcm_injection = 0.0f;
801  }
802  }
803 
804  if (_mode_auto) {
805  switch (_mode) {
806  case GPS_DRIVER_MODE_UBX:
808  break;
809 
810  case GPS_DRIVER_MODE_MTK:
812  break;
813 
816  break;
817 
820  px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
821  break;
822 
823  default:
824  break;
825  }
826 
827  } else {
828  px4_usleep(500000);
829  }
830 
831  }
832  }
833 
834  PX4_INFO("exiting");
835 
836  if (_serial_fd >= 0) {
837  ::close(_serial_fd);
838  _serial_fd = -1;
839  }
840 
842 }
843 
844 int
846 {
847  switch (_instance) {
848  case Instance::Main:
849  PX4_INFO("Main GPS");
850  break;
851 
852  case Instance::Secondary:
853  PX4_INFO("");
854  PX4_INFO("Secondary GPS");
855  break;
856 
857  default:
858  break;
859  }
860 
861  //GPS Mode
862  if (_fake_gps) {
863  PX4_INFO("protocol: SIMULATED");
864 
865  } else {
866  switch (_mode) {
867  case GPS_DRIVER_MODE_UBX:
868  PX4_INFO("protocol: UBX");
869  break;
870 
871  case GPS_DRIVER_MODE_MTK:
872  PX4_INFO("protocol: MTK");
873  break;
874 
876  PX4_INFO("protocol: ASHTECH");
877  break;
878 
880  PX4_INFO("protocol: EMLIDREACH");
881  break;
882 
883  default:
884  break;
885  }
886  }
887 
888  PX4_INFO("status: %s, port: %s, baudrate: %d", _healthy ? "OK" : "NOT OK", _port, _baudrate);
889  PX4_INFO("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
890 
891  if (_report_gps_pos.timestamp != 0) {
892  if (_helper) {
893  PX4_INFO("rate position: \t\t%6.2f Hz", (double)_helper->getPositionUpdateRate());
894  PX4_INFO("rate velocity: \t\t%6.2f Hz", (double)_helper->getVelocityUpdateRate());
895  }
896 
897  if (!_fake_gps) {
898  PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
899  PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
900  }
901 
902  print_message(_report_gps_pos);
903  }
904 
906  GPS *secondary_instance = (GPS *)_secondary_instance;
907  secondary_instance->print_status();
908  }
909 
910  return 0;
911 }
912 
913 void
915 {
916  _scheduled_reset = restart_type;
917 
919  GPS *secondary_instance = (GPS *)_secondary_instance;
920  secondary_instance->schedule_reset(restart_type);
921  }
922 }
923 
924 void
926 {
927  GPSRestartType restart_type = _scheduled_reset;
928 
929  if (restart_type != GPSRestartType::None) {
931  int res = _helper->reset(restart_type);
932 
933  if (res == -1) {
934  PX4_INFO("Reset is not supported on this device.");
935 
936  } else if (res < 0) {
937  PX4_INFO("Reset failed.");
938 
939  } else {
940  PX4_INFO("Reset succeeded.");
941  }
942  }
943 }
944 
945 void
947 {
951  // Heading/yaw data can be updated at a lower rate than the other navigation data.
952  // The uORB message definition requires this data to be set to a NAN if no new valid data is available.
953  _report_gps_pos.heading = NAN;
955  }
956 }
957 
958 void
960 {
961  if (_instance == Instance::Main) {
964 
965  } else {
966  //we don't publish satellite info for the secondary gps
967  }
968 }
969 
970 int
971 GPS::custom_command(int argc, char *argv[])
972 {
973  // Check if the driver is running.
974  if (!is_running()) {
975  PX4_INFO("not running");
976  return PX4_ERROR;
977  }
978 
979  GPS *_instance = get_instance();
980 
981  bool res = false;
982 
983  if (argc == 2 && !strcmp(argv[0], "reset")) {
984 
985  if (!strcmp(argv[1], "hot")) {
986  res = true;
988 
989  } else if (!strcmp(argv[1], "cold")) {
990  res = true;
992 
993  } else if (!strcmp(argv[1], "warm")) {
994  res = true;
996  }
997  }
998 
999  if (res) {
1000  PX4_INFO("Resetting GPS - %s", argv[1]);
1001  return 0;
1002  }
1003 
1004  return (res) ? 0 : print_usage("unknown command");
1005 }
1006 
1007 int GPS::print_usage(const char *reason)
1008 {
1009  if (reason) {
1010  PX4_WARN("%s\n", reason);
1011  }
1012 
1013  PRINT_MODULE_DESCRIPTION(
1014  R"DESCR_STR(
1015 ### Description
1016 GPS driver module that handles the communication with the device and publishes the position via uORB.
1017 It supports multiple protocols (device vendors) and by default automatically selects the correct one.
1018 
1019 The module supports a secondary GPS device, specified via `-e` parameter. The position will be published
1020 on the second uORB topic instance, but it's currently not used by the rest of the system (however the
1021 data will be logged, so that it can be used for comparisons).
1022 
1023 ### Implementation
1024 There is a thread for each device polling for data. The GPS protocol classes are implemented with callbacks
1025 so that they can be used in other projects as well (eg. QGroundControl uses them too).
1026 
1027 ### Examples
1028 For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position):
1029 $ gps stop
1030 $ gps start -f
1031 
1032 Starting 2 GPS devices (the main GPS on /dev/ttyS3 and the secondary on /dev/ttyS4):
1033 $ gps start -d /dev/ttyS3 -e /dev/ttyS4
1034 
1035 Initiate warm restart of GPS device
1036 $ gps reset warm
1037 )DESCR_STR");
1038 
1039  PRINT_MODULE_USAGE_NAME("gps", "driver");
1040  PRINT_MODULE_USAGE_COMMAND("start");
1041  PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "GPS device", true);
1042  PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:<param_name>)", true);
1043  PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Optional secondary GPS device", true);
1044  PRINT_MODULE_USAGE_PARAM_INT('g', 0, 0, 3000000, "Baudrate (secondary GPS, can also be p:<param_name>)", true);
1045 
1046  PRINT_MODULE_USAGE_PARAM_FLAG('f', "Fake a GPS signal (useful for testing)", true);
1047  PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true);
1048 
1049  PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true);
1050  PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml", "GPS Protocol (default=auto select)", true);
1051 
1052  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
1053  PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device");
1054  PRINT_MODULE_USAGE_ARG("cold|warm|hot", "Specify reset type", false);
1055 
1056  return 0;
1057 }
1058 
1059 int GPS::task_spawn(int argc, char *argv[])
1060 {
1061  return task_spawn(argc, argv, Instance::Main);
1062 }
1063 
1064 int GPS::task_spawn(int argc, char *argv[], Instance instance)
1065 {
1066  px4_main_t entry_point;
1067  if (instance == Instance::Main) {
1068  entry_point = (px4_main_t)&run_trampoline;
1069  } else {
1070  entry_point = (px4_main_t)&run_trampoline_secondary;
1071  }
1072 
1073  int task_id = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
1074  SCHED_PRIORITY_SLOW_DRIVER, TASK_STACK_SIZE,
1075  entry_point, (char *const *)argv);
1076 
1077  if (task_id < 0) {
1078  task_id = -1;
1079  return -errno;
1080  }
1081 
1082  if (instance == Instance::Main) {
1083  _task_id = task_id;
1084  }
1085 
1086  return 0;
1087 }
1088 
1089 int GPS::run_trampoline_secondary(int argc, char *argv[])
1090 {
1091 
1092 #ifdef __PX4_NUTTX
1093  // on NuttX task_create() adds the task name as first argument
1094  argc -= 1;
1095  argv += 1;
1096 #endif
1097 
1098  GPS *gps = instantiate(argc, argv, Instance::Secondary);
1099  if (gps) {
1101  gps->run();
1102 
1103  _secondary_instance = nullptr;
1104  delete gps;
1105  }
1106  return 0;
1107 }
1108 GPS *GPS::instantiate(int argc, char *argv[])
1109 {
1110  return instantiate(argc, argv, Instance::Main);
1111 }
1112 
1113 GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
1114 {
1115  const char *device_name = "/dev/ttyS3";
1116  const char *device_name_secondary = nullptr;
1117  int baudrate_main = 0;
1118  int baudrate_secondary = 0;
1119  bool fake_gps = false;
1120  bool enable_sat_info = false;
1123 
1124  bool error_flag = false;
1125  int myoptind = 1;
1126  int ch;
1127  const char *myoptarg = nullptr;
1128 
1129  while ((ch = px4_getopt(argc, argv, "b:d:e:fg:si:p:", &myoptind, &myoptarg)) != EOF) {
1130  switch (ch) {
1131  case 'b':
1132  if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) {
1133  PX4_ERR("baudrate parsing failed");
1134  error_flag = true;
1135  }
1136  break;
1137  case 'g':
1138  if (px4_get_parameter_value(myoptarg, baudrate_secondary) != 0) {
1139  PX4_ERR("baudrate parsing failed");
1140  error_flag = true;
1141  }
1142  break;
1143 
1144  case 'd':
1145  device_name = myoptarg;
1146  break;
1147 
1148  case 'e':
1149  device_name_secondary = myoptarg;
1150  break;
1151 
1152  case 'f':
1153  fake_gps = true;
1154  break;
1155 
1156  case 's':
1157  enable_sat_info = true;
1158  break;
1159 
1160  case 'i':
1161  if (!strcmp(myoptarg, "spi")) {
1162  interface = GPSHelper::Interface::SPI;
1163 
1164  } else if (!strcmp(myoptarg, "uart")) {
1165  interface = GPSHelper::Interface::UART;
1166 
1167  } else {
1168  PX4_ERR("unknown interface: %s", myoptarg);
1169  error_flag = true;
1170  }
1171  break;
1172 
1173  case 'p':
1174  if (!strcmp(myoptarg, "ubx")) {
1175  mode = GPS_DRIVER_MODE_UBX;
1176 
1177  } else if (!strcmp(myoptarg, "mtk")) {
1178  mode = GPS_DRIVER_MODE_MTK;
1179 
1180  } else if (!strcmp(myoptarg, "ash")) {
1181  mode = GPS_DRIVER_MODE_ASHTECH;
1182 
1183  } else if (!strcmp(myoptarg, "eml")) {
1185 
1186  } else {
1187  PX4_ERR("unknown interface: %s", myoptarg);
1188  error_flag = true;
1189  }
1190  break;
1191 
1192  case '?':
1193  error_flag = true;
1194  break;
1195 
1196  default:
1197  PX4_WARN("unrecognized flag");
1198  error_flag = true;
1199  break;
1200  }
1201  }
1202 
1203  if (error_flag) {
1204  return nullptr;
1205  }
1206 
1207  GPS *gps;
1208  if (instance == Instance::Main) {
1209  gps = new GPS(device_name, mode, interface, fake_gps, enable_sat_info, instance, baudrate_main);
1210 
1211  if (gps && device_name_secondary) {
1212  task_spawn(argc, argv, Instance::Secondary);
1213  // wait until running
1214  int i = 0;
1215 
1216  do {
1217  /* wait up to 1s */
1218  px4_usleep(2500);
1219 
1220  } while (!_secondary_instance && ++i < 400);
1221 
1222  if (i == 400) {
1223  PX4_ERR("Timed out while waiting for thread to start");
1224  }
1225  }
1226  } else { // secondary instance
1227  gps = new GPS(device_name_secondary, mode, interface, fake_gps, enable_sat_info, instance, baudrate_secondary);
1228  }
1229 
1230  return gps;
1231 }
1232 
1233 int
1234 gps_main(int argc, char *argv[])
1235 {
1236  return GPS::main(argc, argv);
1237 }
float getPositionUpdateRate()
Definition: gps_helper.h:190
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
bool _should_dump_communication
if true, dump communication
Definition: gps.cpp:182
gps_dump_s * _dump_to_device
Definition: gps.cpp:180
static volatile GPS * _secondary_instance
and thus we wait until the first one publishes at least one message.
Definition: gps.cpp:187
GPSHelper::Interface _interface
interface
Definition: gps.cpp:156
void initializeCommunicationDump()
Definition: gps.cpp:541
virtual int configure(unsigned &baud, OutputMode output_mode)=0
configure the device
GPSRestartType
Definition: gps_helper.h:101
Read data from device.
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
Definition: gps.cpp:89
void handleInjectDataTopic()
check for new messages on the inject data topic & handle them
Definition: gps.cpp:418
bool injectData(uint8_t *data, size_t len)
send data to the device, such as an RTCM stream
Definition: gps.cpp:448
Instance
The GPS allows to run multiple instances.
Definition: gps.cpp:94
const bool _fake_gps
fake gps output
Definition: gps.cpp:174
int _serial_fd
serial interface to GPS
Definition: gps.cpp:146
#define RATE_MEASUREMENT_PERIOD
Definition: gps.cpp:71
int main(int argc, char **argv)
Definition: main.cpp:3
uint64_t timestamp
Definition: gps_dump.h:54
orb_advert_t _report_sat_info_pub
uORB pub for satellite info
Definition: gps.cpp:165
satellite_info_s * _p_report_sat_info
pointer to uORB topic for satellite info
Definition: gps.cpp:162
Definition: I2C.hpp:51
static Mode _mode
Definition: motor_ramp.cpp:81
uint8_t len
Definition: gps_dump.h:55
bool publish(const T &data)
Publish the struct.
static int callback(GPSCallbackType type, void *data1, int data2, void *user)
callback from the driver for the platform specific stuff
Definition: gps.cpp:313
void print_status()
Definition: Commander.cpp:517
int pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
This is an abstraction for the poll on serial used.
Definition: gps.cpp:352
static GPS * instantiate(int argc, char *argv[])
Definition: gps.cpp:1108
static constexpr int TASK_STACK_SIZE
Definition: gps.cpp:86
static void print_usage()
virtual int reset(GPSRestartType restart_type)
Reset GPS device.
Definition: gps_helper.h:188
struct satellite_info_s _data
Definition: gps.cpp:83
static bool is_running()
Definition: dataman.cpp:415
LidarLite * instance
Definition: ll40ls.cpp:65
const Instance _instance
Definition: gps.cpp:176
bool _healthy
flag to signal if the GPS is ok
Definition: gps.cpp:151
GPS_Sat_Info * _sat_info
instance of GPS sat info data object
Definition: gps.cpp:159
void resetUpdateRates()
Definition: gps_helper.cpp:55
uint8_t data[79]
Definition: gps_dump.h:56
virtual int receive(unsigned timeout)=0
receive & handle new data from the device
float _rate
position update rate
Definition: gps.cpp:170
void reset_if_scheduled()
Reset device if reset was scheduled.
Definition: gps.cpp:925
Global flash based parameter store.
gps_driver_mode_t
Definition: gps.cpp:73
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
char _port[20]
device / serial port path
Definition: gps.cpp:149
bool _mode_auto
if true, auto-detect which GPS is attached
Definition: gps.cpp:152
static void read(bootloader_app_shared_t *pshared)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
static int run_trampoline_secondary(int argc, char *argv[])
task spawn trampoline for the secondary GPS
Definition: gps.cpp:1089
float _rate_rtcm_injection
RTCM message injection rate.
Definition: gps.cpp:171
set Baudrate data1: ignored data2: baudrate return: 0 on success
volatile GPSRestartType _scheduled_reset
Definition: gps.cpp:189
#define GPS_ERR(...)
Definition: definitions.h:49
static int task_spawn(int argc, char *argv[])
Definition: gps.cpp:1059
void storeUpdateRates()
Definition: gps_helper.cpp:63
orb_advert_t _report_gps_pos_pub
uORB pub for gps position
Definition: gps.cpp:164
uint8_t * data
Definition: dataman.cpp:149
int _gps_orb_instance
uORB multi-topic instance
Definition: gps.cpp:167
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
U-Blox protocol definition.
Got an RTCM message from the device.
void publishSatelliteInfo()
Publish the satellite info.
Definition: gps.cpp:959
In cold start mode, the receiver has no information from the last position at startup.
bool updated()
Check if there is a new update.
GPSHelper * _helper
instance of GPS parser
Definition: gps.cpp:157
void publish()
Publish the gps struct.
Definition: gps.cpp:946
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
In warm start mode, the receiver has approximate information for time, position, and coarse satellite...
uORB::PublicationQueued< gps_dump_s > _dump_communication_pub
Definition: gps.cpp:179
can be used to set the current clock accurately data1: pointer to a timespec struct data2: ignored re...
#define err(eval,...)
Definition: err.h:83
unsigned _baudrate
current baudrate
Definition: gps.cpp:147
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
Type wrap_pi(Type x)
Wrap value in range [-π, π)
unsigned _last_rate_rtcm_injection_count
counter for number of RTCM messages
Definition: gps.cpp:172
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
const char * device_name
GPSCallbackType
Definition: gps_helper.h:49
static void write(bootloader_app_shared_t *pshared)
__EXPORT int gps_main(int argc, char *argv[])
Definition: gps.cpp:1234
void schedule_reset(GPSRestartType restart_type)
Schedule reset of the GPS device.
Definition: gps.cpp:914
~GPS() override
Definition: gps.cpp:283
static int print_usage(const char *reason=nullptr)
Definition: gps.cpp:1007
static int custom_command(int argc, char *argv[])
Definition: gps.cpp:971
static volatile bool _is_gps_main_advertised
for the second gps we want to make sure that it gets instance 1
Definition: gps.cpp:184
const unsigned _configured_baudrate
configured baudrate (0=auto-detect)
Definition: gps.cpp:148
float dt
Definition: px4io.c:73
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
uint8_t data[182]
GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info, Instance instance, unsigned configured_baudrate)
Definition: gps.cpp:257
In hot start mode, the receiver was powered down only for a short time (4 hours or less)...
int setBaudrate(unsigned baud)
set the Baudrate
Definition: gps.cpp:457
Driver class for Emlid Reach Populates caller provided vehicle_gps_position_s Some ERB messages are c...
Definition: emlid_reach.h:139
normal GPS output
gps_driver_mode_t _mode
current mode
Definition: gps.cpp:154
int print_status() override
Diagnostics - print some basic information about the driver.
Definition: gps.cpp:845
#define TIMEOUT_5HZ
Definition: gps.cpp:70
int _gps_sat_orb_instance
uORB multi-topic instance for satellite info
Definition: gps.cpp:168
mode
Definition: vtol_type.h:76
bool copy(void *dst)
Copy the struct.
uORB::Subscription _orb_inject_data_sub
Definition: gps.cpp:178
void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
Dump gps communication.
Definition: gps.cpp:572
vehicle_gps_position_s _report_gps_pos
uORB topic for gps position
Definition: gps.cpp:161
message about current survey-in status data1: points to a SurveyInStatus struct data2: ignored return...
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177
float getVelocityUpdateRate()
Definition: gps_helper.h:191
void run() override
Definition: gps.cpp:605
Write data to device data1: data to be written data2: number of bytes to write return: num written by...
gps_dump_s * _dump_from_device
Definition: gps.cpp:181