PX4 Firmware
PX4 Autopilot Software http://px4.io
simulator.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Mark Charlebois. All rights reserved.
4  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file simulator.cpp
37  *
38  * This module interfaces via MAVLink to a software in the loop simulator (SITL)
39  * such as jMAVSim or Gazebo.
40  */
41 
42 #include <px4_platform_common/log.h>
43 #include <px4_platform_common/tasks.h>
44 #include <px4_platform_common/time.h>
45 #include <pthread.h>
46 #include <poll.h>
47 #include <systemlib/err.h>
48 #include <errno.h>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <sys/types.h>
53 #include <drivers/drv_board_led.h>
54 
55 #include "simulator.h"
56 
57 using namespace simulator;
58 
59 static px4_task_t g_sim_task = -1;
60 
62 
64 {
65  return _instance;
66 }
67 
68 bool Simulator::getGPSSample(uint8_t *buf, int len)
69 {
70  return _gps.copyData(buf, len);
71 }
72 
74 {
75  _gps.writeData(buf);
76 }
77 
79 {
80  // check for parameter updates
81  if (_parameter_update_sub.updated() || force) {
82  // clear update
83  parameter_update_s pupdate;
84  _parameter_update_sub.copy(&pupdate);
85 
86  // update parameters from storage
87  updateParams();
88  }
89 }
90 
91 int Simulator::start(int argc, char *argv[])
92 {
93  _instance = new Simulator();
94 
95  if (_instance) {
96  drv_led_start();
97 
98  if (argc == 4 && strcmp(argv[2], "-u") == 0) {
99  _instance->set_ip(InternetProtocol::UDP);
100  _instance->set_port(atoi(argv[3]));
101  }
102 
103  if (argc == 4 && strcmp(argv[2], "-c") == 0) {
104  _instance->set_ip(InternetProtocol::TCP);
105  _instance->set_port(atoi(argv[3]));
106  }
107 
108 #ifndef __PX4_QURT
109  // Update sensor data
110  _instance->poll_for_MAVLink_messages();
111 #endif
112 
113  return 0;
114 
115  } else {
116  PX4_WARN("Simulator creation failed");
117  return 1;
118  }
119 }
120 
122 {
123  _ip = ip;
124 }
125 
126 void Simulator::set_port(unsigned port)
127 {
128  _port = port;
129 }
130 
131 static void usage()
132 {
133  PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");
134  PX4_WARN("Start simulator: simulator start");
135  PX4_WARN("Connect using UDP: simulator start -u udp_port");
136  PX4_WARN("Connect using TCP: simulator start -c tcp_port");
137 }
138 
140 extern int simulator_main(int argc, char *argv[]);
142 
143 extern "C" {
144 
145  int simulator_main(int argc, char *argv[])
146  {
147  if (argc > 1 && strcmp(argv[1], "start") == 0) {
148 
149  if (g_sim_task >= 0) {
150  PX4_WARN("Simulator already started");
151  return 0;
152  }
153 
154  g_sim_task = px4_task_spawn_cmd("simulator",
155  SCHED_DEFAULT,
156  SCHED_PRIORITY_DEFAULT,
157  1500,
159  argv);
160 
161  while (true) {
162  if (Simulator::getInstance()) {
163  break;
164 
165  } else {
166  system_sleep(1);
167  }
168  }
169 
170  } else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
171  if (g_sim_task < 0) {
172  PX4_WARN("Simulator not running");
173 
174  } else {
175  px4_task_delete(g_sim_task);
176  g_sim_task = -1;
177  }
178 
179  } else {
180  usage();
181  return 1;
182  }
183 
184  return 0;
185  }
186 
187 }
static int start(int argc, char *argv[])
Definition: simulator.cpp:91
__BEGIN_DECLS int simulator_main(int argc, char *argv[])
Definition: simulator.cpp:145
#define __END_DECLS
Definition: visibility.h:59
#define system_sleep
Definition: visibility.h:109
InternetProtocol
Definition: simulator.h:161
LED driver API to control the onboard LED(s) via ioctl() interface.
This module interfaces via MAVLink to a software in the loop simulator (SITL) such as jMAVSim or Gaze...
void write_gps_data(void *buf)
Definition: simulator.cpp:73
void set_ip(InternetProtocol ip)
Definition: simulator.cpp:121
static void usage()
Definition: simulator.cpp:131
static Simulator * _instance
Definition: simulator.h:207
bool getGPSSample(uint8_t *buf, int len)
Definition: simulator.cpp:68
void set_port(unsigned port)
Definition: simulator.cpp:126
#define __BEGIN_DECLS
Definition: visibility.h:58
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
static Simulator * getInstance()
Definition: simulator.cpp:63
static px4_task_t g_sim_task
Definition: simulator.cpp:59
__BEGIN_DECLS __EXPORT void drv_led_start(void)
Definition: led.cpp:118
void parameters_update(bool force)
Definition: simulator.cpp:78