39 #include <sys/types.h> 40 #include <px4_platform_common/defines.h> 41 #include <px4_platform_common/getopt.h> 46 #include <semaphore.h> 56 #include <px4_platform_common/px4_config.h> 57 #include <px4_platform_common/tasks.h> 67 #include "VirtDevObj.hpp" 71 #define GPS_DRIVER_MODE_UBX_SIM 72 #define GPSSIM_DEVICE_PATH "/dev/gpssim" 74 #define TIMEOUT_100MS 100000 75 #define RATE_MEASUREMENT_PERIOD 5000000 88 GPSSIM(
bool fake_gps,
bool enable_sat_info,
89 int fix_type,
int num_sat,
int noise_multiplier);
94 int devIOCTL(
unsigned long cmd,
unsigned long arg)
override;
96 void set(
int fix_type,
int num_sat,
int noise_multiplier);
120 std::default_random_engine
_gen;
141 int set_baudrate(
unsigned baud);
148 int receive(
int timeout);
166 int fix_type,
int num_sat,
int noise_multiplier) :
184 if (enable_sat_info) {
199 for (
unsigned i = 0; (i < 10) && (
_task != -1); i++) {
206 px4_task_delete(
_task);
223 _task = px4_task_spawn_cmd(
"gpssim", SCHED_DEFAULT,
227 PX4_ERR(
"task start failed: %d", errno);
250 ret = VirtDevObj::devIOCTL(cmd, arg);
272 static uint64_t timestamp_last = 0;
275 (gps.timestamp != timestamp_last || timestamp_last == 0)) {
291 timestamp_last = gps.timestamp;
297 std::normal_distribution<float> normal_distribution(0.0f, 1.0f);
364 PX4_INFO(
"protocol: SIM");
366 PX4_INFO(
"sat info: %s, noise: %d, jamming detected: %s",
384 PX4_INFO(
"Parameters set");
395 void start(
bool fake_gps,
bool enable_sat_info,
396 int fix_type,
int num_sat,
int noise_multiplier);
401 void usage(
const char *reason);
407 start(
bool fake_gps,
bool enable_sat_info,
int fix_type,
int num_sat,
int noise_multiplier)
412 g_dev =
new GPSSIM(fake_gps, enable_sat_info, fix_type, num_sat, noise_multiplier);
414 if (g_dev ==
nullptr) {
418 if (
OK != g_dev->
init()) {
434 if (g_dev !=
nullptr) {
439 PX4_ERR(
"start failed");
477 PX4_ERR(
"reset failed");
487 if (g_dev ==
nullptr) {
488 PX4_ERR(
"gpssim not running");
499 PX4_WARN(
"%s", reason);
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)]");
516 bool fake_gps =
false;
517 bool enable_sat_info =
false;
520 int noise_multiplier = 0;
525 const char *myoptarg =
nullptr;
527 while ((ch = px4_getopt(argc, argv,
"fst:n:m:", &myoptind, &myoptarg)) != EOF) {
531 PX4_INFO(
"Using fake GPS");
535 enable_sat_info =
true;
536 PX4_INFO(
"Satellite info enabled");
540 fix_type = atoi(myoptarg);
541 PX4_INFO(
"Setting fix_type to %d", fix_type);
545 num_sat = atoi(myoptarg);
546 PX4_INFO(
"Setting number of satellites to %d", num_sat);
550 noise_multiplier = atoi(myoptarg);
551 PX4_INFO(
"Setting noise multiplier to %d", noise_multiplier);
555 PX4_WARN(
"Unknown option!");
559 if (myoptind >= argc) {
567 if (!strcmp(argv[myoptind],
"start")) {
568 if (
g_dev !=
nullptr) {
569 PX4_WARN(
"already started");
573 gpssim::start(fake_gps, enable_sat_info, fix_type, num_sat, noise_multiplier);
578 if (
g_dev ==
nullptr) {
579 PX4_WARN(
"not running");
583 if (!strcmp(argv[myoptind],
"stop")) {
590 if (!strcmp(argv[myoptind],
"test")) {
597 if (!strcmp(argv[myoptind],
"reset")) {
604 if (!strcmp(argv[myoptind],
"status")) {
611 if (!strcmp(argv[myoptind],
"set")) {
612 g_dev->
set(fix_type, num_sat, noise_multiplier);
volatile bool _task_should_exit
struct vehicle_gps_position_s _report_gps_pos
uORB topic for gps position
void reset()
Reset the driver.
API for the uORB lightweight object broker.
void usage(const char *reason)
Print the correct usage.
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)
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
void info()
Print the status of the driver.
static int task_main_trampoline(int argc, char *argv[])
Trampoline to the worker task.
struct satellite_info_s _data
int devIOCTL(unsigned long cmd, unsigned long arg) override
High-resolution timer with callouts and timekeeping.
bool _task_should_exit
flag to make the main worker task exit
#define GPSSIM_DEVICE_PATH
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
GPS_Sat_Info * _Sat_Info
instance of GPS sat info data object
bool getGPSSample(uint8_t *buf, int len)
void init()
Activates/configures the hardware registers.
__EXPORT int gpssim_main(int argc, char *argv[])
struct satellite_info_s * _p_report_sat_info
pointer to uORB topic for satellite info
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
void task_main(int argc, char *argv[])
Local functions in support of the shell command.
void start(bool fake_gps, bool enable_sat_info, int fix_type, int num_sat, int noise_multiplier)
Start the driver.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
void task_main()
Worker task: main GPS thread that configures the GPS and parses incoming data, always running...
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
void usage(const char *reason)
Print the correct usage.
int32_t jamming_indicator
orb_advert_t _report_gps_pos_pub
uORB pub for gps position
volatile int _task
worker task
orb_advert_t _report_sat_info_pub
uORB pub for satellite info
#define SENSORIOCRESET
Reset the sensor to its default configuration.
int orb_unadvertise(orb_advert_t handle)
void print_info()
Diagnostics - print some basic information about the driver.
static Simulator * getInstance()
void cmd_reset()
Send a reset command to the GPS.
void stop()
Stop the driver.
std::default_random_engine _gen
void set(int fix_type, int num_sat, int noise_multiplier)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).