PX4 Firmware
PX4 Autopilot Software http://px4.io
gpssim.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Roman Bapst. 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 gpssim.cpp
36  * Simulated GPS driver
37  */
38 
39 #include <sys/types.h>
40 #include <px4_platform_common/defines.h>
41 #include <px4_platform_common/getopt.h>
42 #include <inttypes.h>
43 #include <stdio.h>
44 #include <stdbool.h>
45 #include <stdlib.h>
46 #include <semaphore.h>
47 #include <string.h>
48 #include <fcntl.h>
49 #include <poll.h>
50 #include <errno.h>
51 #include <stdio.h>
52 #include <random>
53 #include <math.h>
54 #include <unistd.h>
55 #include <fcntl.h>
56 #include <px4_platform_common/px4_config.h>
57 #include <px4_platform_common/tasks.h>
58 #include <drivers/drv_hrt.h>
59 #include <drivers/device/device.h>
60 #include <uORB/uORB.h>
63 
64 #include <simulator/simulator.h>
65 
66 #include "DevMgr.hpp"
67 #include "VirtDevObj.hpp"
68 
69 using namespace DriverFramework;
70 
71 #define GPS_DRIVER_MODE_UBX_SIM
72 #define GPSSIM_DEVICE_PATH "/dev/gpssim"
73 
74 #define TIMEOUT_100MS 100000
75 #define RATE_MEASUREMENT_PERIOD 5000000
76 
77 /* class for dynamic allocation of satellite info data */
78 class GPS_Sat_Info
79 {
80 public:
81  struct satellite_info_s _data;
82 };
83 
84 
85 class GPSSIM : public VirtDevObj
86 {
87 public:
88  GPSSIM(bool fake_gps, bool enable_sat_info,
89  int fix_type, int num_sat, int noise_multiplier);
90  ~GPSSIM() override;
91 
92  int init() override;
93 
94  int devIOCTL(unsigned long cmd, unsigned long arg) override;
95 
96  void set(int fix_type, int num_sat, int noise_multiplier);
97 
98  /**
99  * Diagnostics - print some basic information about the driver.
100  */
101  void print_info();
102 
103 protected:
104  void _measure() override {}
105 
106 private:
107 
108  bool _task_should_exit; ///< flag to make the main worker task exit
109  volatile int _task; ///< worker task
110  GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
111  struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
112  orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
113  struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
114  orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
115  SyncObj _sync;
117  int _num_sat;
119 
120  std::default_random_engine _gen;
121 
122  /**
123  * Try to configure the GPS, handle outgoing communication to the GPS
124  */
125  void config();
126 
127  /**
128  * Trampoline to the worker task
129  */
130  static int task_main_trampoline(int argc, char *argv[]);
131 
132 
133  /**
134  * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
135  */
136  void task_main();
137 
138  /**
139  * Set the baudrate of the UART to the GPS
140  */
141  int set_baudrate(unsigned baud);
142 
143  /**
144  * Send a reset command to the GPS
145  */
146  void cmd_reset();
147 
148  int receive(int timeout);
149 };
150 
151 
152 /*
153  * Driver 'main' command.
154  */
155 extern "C" __EXPORT int gpssim_main(int argc, char *argv[]);
156 
157 namespace
158 {
159 
160 GPSSIM *g_dev = nullptr;
161 
162 }
163 
164 
165 GPSSIM::GPSSIM(bool fake_gps, bool enable_sat_info,
166  int fix_type, int num_sat, int noise_multiplier) :
167  VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10),
168  _task_should_exit(false),
169  _Sat_Info(nullptr),
170  _report_gps_pos{},
171  _report_gps_pos_pub(nullptr),
172  _p_report_sat_info(nullptr),
173  _report_sat_info_pub(nullptr),
174  _fix_type(fix_type),
175  _num_sat(num_sat),
176  _noise_multiplier(noise_multiplier)
177 {
178  /* we need this potentially before it could be set in task_main */
179  g_dev = this;
180  _report_gps_pos.heading = NAN;
182 
183  /* create satellite info data object if requested */
184  if (enable_sat_info) {
185  _Sat_Info = new (GPS_Sat_Info);
187  memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
188  }
189 }
190 
192 {
193  delete _Sat_Info;
194 
195  /* tell the task we want it to go away */
196  _task_should_exit = true;
197 
198  /* spin waiting for the task to stop */
199  for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
200  /* give it another 100ms */
201  px4_usleep(100000);
202  }
203 
204  /* well, kill it anyway, though this will probably crash */
205  if (_task != -1) {
206  px4_task_delete(_task);
207  }
208 
209  g_dev = nullptr;
210 }
211 
212 int
214 {
215  int ret = PX4_ERROR;
216 
217  /* do regular cdev init */
218  if (VirtDevObj::init() != OK) {
219  goto out;
220  }
221 
222  /* start the GPS driver worker task */
223  _task = px4_task_spawn_cmd("gpssim", SCHED_DEFAULT,
224  SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr);
225 
226  if (_task < 0) {
227  PX4_ERR("task start failed: %d", errno);
228  return -errno;
229  }
230 
231  ret = OK;
232 out:
233  return ret;
234 }
235 
236 int
237 GPSSIM::devIOCTL(unsigned long cmd, unsigned long arg)
238 {
239  _sync.lock();
240 
241  int ret = OK;
242 
243  switch (cmd) {
244  case SENSORIOCRESET:
245  cmd_reset();
246  break;
247 
248  default:
249  /* give it to parent if no one wants it */
250  ret = VirtDevObj::devIOCTL(cmd, arg);
251  break;
252  }
253 
254  _sync.unlock();
255 
256  return ret;
257 }
258 
259 int
260 GPSSIM::task_main_trampoline(int argc, char *argv[])
261 {
262  g_dev->task_main();
263  return 0;
264 }
265 
266 int
267 GPSSIM::receive(int timeout)
268 {
271 
272  static uint64_t timestamp_last = 0;
273 
274  if (sim->getGPSSample((uint8_t *)&gps, sizeof(gps)) &&
275  (gps.timestamp != timestamp_last || timestamp_last == 0)) {
277  _report_gps_pos.lat = gps.lat;
278  _report_gps_pos.lon = gps.lon;
279  _report_gps_pos.alt = gps.alt;
280  _report_gps_pos.eph = (float)gps.eph * 1e-2f;
281  _report_gps_pos.epv = (float)gps.epv * 1e-2f;
282  _report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
283  _report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
284  _report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
285  _report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f;
286  _report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f);
287  _report_gps_pos.fix_type = gps.fix_type;
288  _report_gps_pos.satellites_used = gps.satellites_visible;
290 
291  timestamp_last = gps.timestamp;
292 
293  // check for data set by the user
296  // use normal distribution for noise
297  std::normal_distribution<float> normal_distribution(0.0f, 1.0f);
298  _report_gps_pos.lat += (int32_t)(_noise_multiplier * normal_distribution(_gen));
299  _report_gps_pos.lon += (int32_t)(_noise_multiplier * normal_distribution(_gen));
300 
301  return 1;
302 
303  } else {
304 
305  px4_usleep(timeout);
306  return 0;
307  }
308 }
309 
310 void
312 {
313  /* loop handling received serial bytes and also configuring in between */
314  while (!_task_should_exit) {
315 
316  int recv_ret = receive(TIMEOUT_100MS);
317 
318  if (recv_ret > 0) {
319 
320  /* opportunistic publishing - else invalid data would end up on the bus */
321  if (_report_gps_pos_pub != nullptr) {
322  orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
323 
324  } else {
325  _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
326  }
327 
328  if (_p_report_sat_info) {
329  if (_report_sat_info_pub != nullptr) {
331 
332  } else {
334  }
335  }
336  }
337  }
338 
339  if (_report_gps_pos_pub) {
341  }
342 
343  if (_report_sat_info_pub) {
345  }
346 
347  PX4_INFO("exiting");
348 
349  /* tell the dtor that we are exiting */
350  _task = -1;
351 }
352 
353 
354 
355 void
357 {
358 }
359 
360 void
362 {
363  //GPS Mode
364  PX4_INFO("protocol: SIM");
365 
366  PX4_INFO("sat info: %s, noise: %d, jamming detected: %s",
367  (_p_report_sat_info != nullptr) ? "enabled" : "disabled",
369  _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
370 
371  if (_report_gps_pos.timestamp != 0) {
372  print_message(_report_gps_pos);
373  }
374 
375  px4_usleep(100000);
376 }
377 
378 void
379 GPSSIM::set(int fix_type, int num_sat, int noise_multiplier)
380 {
381  _fix_type = fix_type;
382  _num_sat = num_sat;
383  _noise_multiplier = noise_multiplier;
384  PX4_INFO("Parameters set");
385 }
386 
387 /**
388  * Local functions in support of the shell command.
389  */
390 namespace gpssim
391 {
392 
393 GPSSIM *g_dev = nullptr;
394 
395 void start(bool fake_gps, bool enable_sat_info,
396  int fix_type, int num_sat, int noise_multiplier);
397 void stop();
398 void test();
399 void reset();
400 void info();
401 void usage(const char *reason);
402 
403 /**
404  * Start the driver.
405  */
406 void
407 start(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier)
408 {
409  DevHandle h;
410 
411  /* create the driver */
412  g_dev = new GPSSIM(fake_gps, enable_sat_info, fix_type, num_sat, noise_multiplier);
413 
414  if (g_dev == nullptr) {
415  goto fail;
416  }
417 
418  if (OK != g_dev->init()) {
419  goto fail;
420  }
421 
422  /* set the poll rate to default, starts automatic data collection */
423  DevMgr::getHandle(GPSSIM_DEVICE_PATH, h);
424 
425  if (!h.isValid()) {
426  PX4_ERR("getHandle failed: %s", GPSSIM_DEVICE_PATH);
427  goto fail;
428  }
429 
430  return;
431 
432 fail:
433 
434  if (g_dev != nullptr) {
435  delete g_dev;
436  g_dev = nullptr;
437  }
438 
439  PX4_ERR("start failed");
440 }
441 
442 /**
443  * Stop the driver.
444  */
445 void
447 {
448  delete g_dev;
449  g_dev = nullptr;
450 }
451 
452 /**
453  * Perform some basic functional tests on the driver;
454  * make sure we can collect data from the sensor in polled
455  * and automatic modes.
456  */
457 void
459 {
460  PX4_INFO("PASS");
461 }
462 
463 /**
464  * Reset the driver.
465  */
466 void
468 {
469  DevHandle h;
470  DevMgr::getHandle(GPSSIM_DEVICE_PATH, h);
471 
472  if (!h.isValid()) {
473  PX4_ERR("failed ");
474  }
475 
476  if (h.ioctl(SENSORIOCRESET, 0) < 0) {
477  PX4_ERR("reset failed");
478  }
479 }
480 
481 /**
482  * Print the status of the driver.
483  */
484 void
486 {
487  if (g_dev == nullptr) {
488  PX4_ERR("gpssim not running");
489  return;
490  }
491 
492  g_dev->print_info();
493 }
494 
495 void
496 usage(const char *reason)
497 {
498  if (reason) {
499  PX4_WARN("%s", reason);
500  }
501 
502  PX4_INFO("usage:");
503  PX4_INFO("gpssim {start|stop|test|reset|status|set}");
504  PX4_INFO(" [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
505  PX4_INFO(" [-t # (for setting fix_type)][-n # (for setting # satellites)][-m # (for setting noise multiplier [scalar] for normal distribution)]");
506 }
507 
508 } // namespace
509 
510 
511 
512 int
513 gpssim_main(int argc, char *argv[])
514 {
515  // set to default
516  bool fake_gps = false;
517  bool enable_sat_info = false;
518  int fix_type = -1;
519  int num_sat = -1;
520  int noise_multiplier = 0;
521 
522  // check for optional arguments
523  int ch;
524  int myoptind = 1;
525  const char *myoptarg = nullptr;
526 
527  while ((ch = px4_getopt(argc, argv, "fst:n:m:", &myoptind, &myoptarg)) != EOF) {
528  switch (ch) {
529  case 'f':
530  fake_gps = true;
531  PX4_INFO("Using fake GPS");
532  break;
533 
534  case 's':
535  enable_sat_info = true;
536  PX4_INFO("Satellite info enabled");
537  break;
538 
539  case 't':
540  fix_type = atoi(myoptarg);
541  PX4_INFO("Setting fix_type to %d", fix_type);
542  break;
543 
544  case 'n':
545  num_sat = atoi(myoptarg);
546  PX4_INFO("Setting number of satellites to %d", num_sat);
547  break;
548 
549  case 'm':
550  noise_multiplier = atoi(myoptarg);
551  PX4_INFO("Setting noise multiplier to %d", noise_multiplier);
552  break;
553 
554  default:
555  PX4_WARN("Unknown option!");
556  }
557  }
558 
559  if (myoptind >= argc) {
560  gpssim::usage("not enough arguments supplied");
561  return 1;
562  }
563 
564  /*
565  * Start/load the driver.
566  */
567  if (!strcmp(argv[myoptind], "start")) {
568  if (g_dev != nullptr) {
569  PX4_WARN("already started");
570  return 0;
571  }
572 
573  gpssim::start(fake_gps, enable_sat_info, fix_type, num_sat, noise_multiplier);
574  return 0;
575  }
576 
577  /* The following need gpssim running. */
578  if (g_dev == nullptr) {
579  PX4_WARN("not running");
580  return 1;
581  }
582 
583  if (!strcmp(argv[myoptind], "stop")) {
584  gpssim::stop();
585  }
586 
587  /*
588  * Test the driver/device.
589  */
590  if (!strcmp(argv[myoptind], "test")) {
591  gpssim::test();
592  }
593 
594  /*
595  * Reset the driver.
596  */
597  if (!strcmp(argv[myoptind], "reset")) {
598  gpssim::reset();
599  }
600 
601  /*
602  * Print driver status.
603  */
604  if (!strcmp(argv[myoptind], "status")) {
605  gpssim::info();
606  }
607 
608  /*
609  * Set parameters
610  */
611  if (!strcmp(argv[myoptind], "set")) {
612  g_dev->set(fix_type, num_sat, noise_multiplier);
613  }
614 
615  return 0;
616 }
struct vehicle_gps_position_s _report_gps_pos
uORB topic for gps position
Definition: gpssim.cpp:111
void _measure() override
Definition: gpssim.cpp:104
int init() override
Definition: gpssim.cpp:213
void reset()
Reset the driver.
Definition: gpssim.cpp:467
API for the uORB lightweight object broker.
GPSSIM * g_dev
Definition: gpssim.cpp:393
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
static void task_main_trampoline(int argc, char *argv[])
This module interfaces via MAVLink to a software in the loop simulator (SITL) such as jMAVSim or Gaze...
GPSSIM(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier)
Definition: gpssim.cpp:165
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
static void stop()
Definition: dataman.cpp:1491
void info()
Print the status of the driver.
Definition: gpssim.cpp:485
static int task_main_trampoline(int argc, char *argv[])
Trampoline to the worker task.
Definition: gpssim.cpp:260
struct satellite_info_s _data
Definition: gps.cpp:83
int devIOCTL(unsigned long cmd, unsigned long arg) override
Definition: gpssim.cpp:237
int receive(int timeout)
Definition: gpssim.cpp:267
High-resolution timer with callouts and timekeeping.
int _noise_multiplier
Definition: gpssim.cpp:118
bool _task_should_exit
flag to make the main worker task exit
Definition: gpssim.cpp:108
#define GPSSIM_DEVICE_PATH
Definition: gpssim.cpp:72
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
GPS_Sat_Info * _Sat_Info
instance of GPS sat info data object
Definition: gpssim.cpp:110
bool getGPSSample(uint8_t *buf, int len)
Definition: simulator.cpp:68
int _fix_type
Definition: gpssim.cpp:116
void init()
Activates/configures the hardware registers.
#define TIMEOUT_100MS
Definition: gpssim.cpp:74
__EXPORT int gpssim_main(int argc, char *argv[])
Definition: gpssim.cpp:513
struct satellite_info_s * _p_report_sat_info
pointer to uORB topic for satellite info
Definition: gpssim.cpp:113
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: gpssim.cpp:458
void task_main(int argc, char *argv[])
~GPSSIM() override
Definition: gpssim.cpp:191
Local functions in support of the shell command.
Definition: gpssim.cpp:390
void start(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier)
Start the driver.
Definition: gpssim.cpp:407
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
void task_main()
Worker task: main GPS thread that configures the GPS and parses incoming data, always running...
Definition: gpssim.cpp:311
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
void usage(const char *reason)
Print the correct usage.
Definition: gpssim.cpp:496
static int start()
Definition: dataman.cpp:1452
orb_advert_t _report_gps_pos_pub
uORB pub for gps position
Definition: gpssim.cpp:112
volatile int _task
worker task
Definition: gpssim.cpp:109
orb_advert_t _report_sat_info_pub
uORB pub for satellite info
Definition: gpssim.cpp:114
#define SENSORIOCRESET
Reset the sensor to its default configuration.
Definition: drv_sensor.h:141
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
void print_info()
Diagnostics - print some basic information about the driver.
Definition: gpssim.cpp:361
int _num_sat
Definition: gpssim.cpp:117
static Simulator * getInstance()
Definition: simulator.cpp:63
#define OK
Definition: uavcan_main.cpp:71
void cmd_reset()
Send a reset command to the GPS.
Definition: gpssim.cpp:356
SyncObj _sync
Definition: gpssim.cpp:115
void stop()
Stop the driver.
Definition: gpssim.cpp:446
std::default_random_engine _gen
Definition: gpssim.cpp:120
void set(int fix_type, int num_sat, int noise_multiplier)
Definition: gpssim.cpp:379
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).