Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
4 :
5 : #include "AP_HAL_SITL.h"
6 : #include "AP_HAL_SITL_Namespace.h"
7 : #include "HAL_SITL_Class.h"
8 : #include "UARTDriver.h"
9 : #include <AP_HAL/utility/getopt_cpp.h>
10 : #include <AP_HAL_SITL/Storage.h>
11 : #include <AP_Param/AP_Param.h>
12 : #include <AP_JSON/AP_JSON.h>
13 : #include <AP_Filesystem/AP_Filesystem.h>
14 :
15 : #include <SITL/SIM_Multicopter.h>
16 : #include <SITL/SIM_Helicopter.h>
17 : #include <SITL/SIM_SingleCopter.h>
18 : #include <SITL/SIM_Plane.h>
19 : #include <SITL/SIM_Glider.h>
20 : #include <SITL/SIM_QuadPlane.h>
21 : #include <SITL/SIM_Rover.h>
22 : #include <SITL/SIM_BalanceBot.h>
23 : #include <SITL/SIM_Sailboat.h>
24 : #include <SITL/SIM_MotorBoat.h>
25 : #include <SITL/SIM_CRRCSim.h>
26 : #include <SITL/SIM_Gazebo.h>
27 : #include <SITL/SIM_last_letter.h>
28 : #include <SITL/SIM_JSBSim.h>
29 : #include <SITL/SIM_Tracker.h>
30 : #include <SITL/SIM_Balloon.h>
31 : #include <SITL/SIM_FlightAxis.h>
32 : #include <SITL/SIM_Calibration.h>
33 : #include <SITL/SIM_XPlane.h>
34 : #include <SITL/SIM_Submarine.h>
35 : #include <SITL/SIM_SilentWings.h>
36 : #include <SITL/SIM_Morse.h>
37 : #include <SITL/SIM_AirSim.h>
38 : #include <SITL/SIM_Scrimmage.h>
39 : #include <SITL/SIM_Webots.h>
40 : #include <SITL/SIM_Webots_Python.h>
41 : #include <SITL/SIM_JSON.h>
42 : #include <SITL/SIM_Blimp.h>
43 : #include <SITL/SIM_NoVehicle.h>
44 : #include <SITL/SIM_StratoBlimp.h>
45 :
46 : #include <AP_Filesystem/AP_Filesystem.h>
47 :
48 : #include <AP_Vehicle/AP_Vehicle_Type.h>
49 :
50 : #include <signal.h>
51 : #include <stdio.h>
52 : #include <time.h>
53 : #include <sys/time.h>
54 :
55 : #define FORCE_VERSION_H_INCLUDE
56 : #include "ap_version.h"
57 :
58 : extern HAL_SITL& hal;
59 :
60 : using namespace HALSITL;
61 : using namespace SITL;
62 :
63 : // catch floating point exceptions
64 0 : static void _sig_fpe(int signum)
65 : {
66 0 : fprintf(stderr, "ERROR: Floating point exception - aborting\n");
67 0 : AP_HAL::dump_stack_trace();
68 0 : AP_HAL::dump_core_file();
69 0 : abort();
70 : }
71 :
72 : // catch segfault
73 0 : static void _sig_segv(int signum)
74 : {
75 0 : fprintf(stderr, "ERROR: segmentation fault - aborting\n");
76 0 : AP_HAL::dump_stack_trace();
77 0 : AP_HAL::dump_core_file();
78 0 : abort();
79 : }
80 :
81 0 : void SITL_State::_usage(void)
82 : {
83 0 : printf("Options:\n"
84 : "\t--help|-h display this help information\n"
85 : "\t--wipe|-w wipe eeprom\n"
86 : "\t--unhide-groups|-u parameter enumeration ignores AP_PARAM_FLAG_ENABLE\n"
87 : "\t--speedup|-s SPEEDUP set simulation speedup\n"
88 : "\t--rate|-r RATE set SITL framerate\n"
89 : "\t--console|-C use console instead of TCP ports\n"
90 : "\t--instance|-I N set instance of SITL (adds 10*instance to all port numbers)\n"
91 : "\t--home|-O HOME set start location (lat,lng,alt,yaw) or location name\n"
92 : "\t--model|-M MODEL set simulation model\n"
93 : "\t--config string set additional simulation config string\n"
94 : "\t--fg|-F ADDRESS set Flight Gear view address, defaults to 127.0.0.1\n"
95 : "\t--enable-fgview enable Flight Gear view\n"
96 : "\t--gimbal enable simulated MAVLink gimbal\n"
97 : "\t--autotest-dir DIR set directory for additional files\n"
98 : "\t--defaults path set path to defaults file\n"
99 : "\t--list-models list embedded vehicleinfo.json models and exit\n"
100 : "\t--sim-periph-lockstep do not advance the simulation until all simulated peripherals have consumed our state\n"
101 : "\t--serial0 device set device string for SERIAL0\n"
102 : "\t--serial1 device set device string for SERIAL1\n"
103 : "\t--serial2 device set device string for SERIAL2\n"
104 : "\t--serial3 device set device string for SERIAL3\n"
105 : "\t--serial4 device set device string for SERIAL4\n"
106 : "\t--serial5 device set device string for SERIAL5\n"
107 : "\t--serial6 device set device string for SERIAL6\n"
108 : "\t--serial7 device set device string for SERIAL7\n"
109 : "\t--serial8 device set device string for SERIAL8\n"
110 : "\t--serial9 device set device string for SERIAL9\n"
111 : "\t--uartA device alias for --serial0 (do not use)\n"
112 : "\t--net-device NAME:PORT attach simulated device NAME to TCP port PORT rather than to a serial port\n"
113 : "\t--base-port PORT set port num for base port(default 5670) must be before -I option\n"
114 : "\t--rc-in-port PORT|uds:PATH set UDP port or Unix datagram path for rc in\n"
115 : "\t--sim-address ADDR set address string for simulator\n"
116 : "\t--sim-port-in PORT set port num for simulator in\n"
117 : "\t--sim-port-out PORT set port num for simulator out\n"
118 : "\t--irlock-port PORT set port num for irlock\n"
119 : "\t--start-time TIMESTR set simulation start time in UNIX timestamp\n"
120 : "\t--sysid ID set MAV_SYSID\n"
121 : "\t--slave number set the number of JSON slaves\n"
122 : "\t--use_sim_time <true|false> use ROS2 simulation clock for DDS topics. Defaults to false\n"
123 : );
124 0 : }
125 :
126 : static const struct {
127 : const char *name;
128 : Aircraft *(*constructor)(const char *frame_str);
129 : } model_constructors[] = {
130 : { "quadplane", QuadPlane::create },
131 : #if AP_SIM_XPLANE_ENABLED
132 : { "xplane", XPlane::create },
133 : #endif // AP_SIM_XPLANE_ENABLED
134 : { "firefly", QuadPlane::create },
135 : { "+", MultiCopter::create },
136 : { "quad", MultiCopter::create },
137 : { "copter", MultiCopter::create },
138 : { "x", MultiCopter::create },
139 : { "bfxrev", MultiCopter::create },
140 : { "bfx", MultiCopter::create },
141 : { "djix", MultiCopter::create },
142 : { "cwx", MultiCopter::create },
143 : { "hexa", MultiCopter::create },
144 : { "hexax", MultiCopter::create },
145 : { "hexa-cwx", MultiCopter::create },
146 : { "hexa-dji", MultiCopter::create },
147 : { "octa", MultiCopter::create },
148 : { "octa-cwx", MultiCopter::create },
149 : { "octa-dji", MultiCopter::create },
150 : { "octa-quad-cwx", MultiCopter::create },
151 : { "dotriaconta_octaquad_x", MultiCopter::create },
152 : { "dodeca-hexa", MultiCopter::create },
153 : { "hexadeca-octa", MultiCopter::create },
154 : { "hexadeca-octa-cwx", MultiCopter::create },
155 : { "tri", MultiCopter::create },
156 : { "y6", MultiCopter::create },
157 : { "deca", MultiCopter::create },
158 : { "deca-cwx", MultiCopter::create },
159 : // heli-quad must precede heli as matching is partial:
160 : { "heli-quad", MultiCopter::create },
161 : { "heli", Helicopter::create },
162 : { "heli-dual", Helicopter::create },
163 : { "heli-compound", Helicopter::create },
164 : { "heli-blade360", Helicopter::create },
165 : { "singlecopter", SingleCopter::create },
166 : { "coaxcopter", SingleCopter::create },
167 : { "rover", SimRover::create },
168 : { "balancebot", BalanceBot::create },
169 : { "sailboat", Sailboat::create },
170 : { "motorboat", MotorBoat::create },
171 : #if AP_SIM_CRRCSIM_ENABLED
172 : { "crrcsim", CRRCSim::create },
173 : #endif // AP_SIM_CRRCSIM_ENABLED
174 : #if AP_SIM_JSBSIM_ENABLED
175 : { "jsbsim", JSBSim::create },
176 : #endif // AP_SIM_JSBSIM_ENABLED
177 : #if AP_SIM_FLIGHTAXIS_ENABLED
178 : { "flightaxis", FlightAxis::create },
179 : #endif // AP_SIM_FLIGHTAXIS_ENABLED
180 : #if AP_SIM_GAZEBO_ENABLED
181 : { "gazebo", Gazebo::create },
182 : #endif // AP_SIM_GAZEBO_ENABLED
183 : #if AP_SIM_LAST_LETTER_ENABLED
184 : { "last_letter", last_letter::create },
185 : #endif // AP_SIM_LAST_LETTER_ENABLED
186 : { "tracker", Tracker::create },
187 : { "balloon", Balloon::create },
188 : { "glider", Glider::create },
189 : { "plane", Plane::create },
190 : { "calibration", Calibration::create },
191 : { "vectored", Submarine::create },
192 : { "vectored_6dof", Submarine::create },
193 : #if AP_SIM_SILENTWINGS_ENABLED
194 : { "silentwings", SilentWings::create },
195 : #endif // AP_SIM_SILENTWINGS_ENABLED
196 : #if AP_SIM_MORSE_ENABLED
197 : { "morse", Morse::create },
198 : #endif // AP_SIM_MORSE_ENABLED
199 : #if AP_SIM_AIRSIM_ENABLED
200 : { "airsim", AirSim::create},
201 : #endif // AP_SIM_AIRSIM_ENABLED
202 : #if AP_SIM_SCRIMMAGE_ENABLED
203 : { "scrimmage", Scrimmage::create },
204 : #endif // AP_SIM_SCRIMMAGE_ENABLED
205 : { "webots-python", WebotsPython::create },
206 : #if AP_SIM_WEBOTS_ENABLED
207 : { "webots", Webots::create },
208 : #endif // AP_SIM_WEBOTS_ENABLED
209 : #if AP_SIM_JSON_ENABLED
210 : { "JSON", JSON::create },
211 : #endif // AP_SIM_JSON_ENABLED
212 : { "blimp", Blimp::create },
213 : { "novehicle", NoVehicle::create },
214 : #if AP_SIM_STRATOBLIMP_ENABLED
215 : { "stratoblimp", StratoBlimp::create },
216 : #endif
217 : };
218 :
219 2094 : void SITL_State::_set_signal_handlers(void) const
220 : {
221 2094 : struct sigaction sa_fpe = {};
222 2094 : sigemptyset(&sa_fpe.sa_mask);
223 2094 : sa_fpe.sa_handler = _sig_fpe;
224 2094 : sigaction(SIGFPE, &sa_fpe, nullptr);
225 :
226 2094 : struct sigaction sa_pipe = {};
227 2094 : sigemptyset(&sa_pipe.sa_mask);
228 2094 : sa_pipe.sa_handler = SIG_IGN; /* No-op SIGPIPE handler */
229 2094 : sigaction(SIGPIPE, &sa_pipe, nullptr);
230 :
231 2094 : struct sigaction sa_segv = {};
232 2094 : sigemptyset(&sa_segv.sa_mask);
233 2094 : sa_segv.sa_handler = _sig_segv;
234 2094 : sigaction(SIGSEGV, &sa_segv, nullptr);
235 :
236 2094 : }
237 :
238 2094 : void SITL_State::_parse_command_line(int argc, char * const argv[])
239 : {
240 : int opt;
241 2094 : float speedup = 1.0f;
242 2094 : float sim_rate_hz = 0;
243 2094 : _instance = 0;
244 : // default to CMAC
245 2094 : const char *home_str = nullptr;
246 2094 : const char *model_str = nullptr;
247 2094 : const char *vehicle_str = AP_BUILD_TARGET_NAME;
248 2094 : _use_fg_view = false;
249 2094 : char *autotest_dir = nullptr;
250 2094 : _fg_address = "127.0.0.1";
251 2094 : const char* config = "";
252 :
253 2094 : const int BASE_PORT = 5760;
254 2094 : const int RCIN_PORT = 5501;
255 2094 : const int FG_VIEW_PORT = 5503;
256 2094 : _base_port = BASE_PORT;
257 2094 : _rcin_port = RCIN_PORT;
258 2094 : _rcin_path = nullptr;
259 2094 : _fg_view_port = FG_VIEW_PORT;
260 :
261 2094 : const int SIM_IN_PORT = 9003;
262 2094 : const int SIM_OUT_PORT = 9002;
263 2094 : const int IRLOCK_PORT = 9005;
264 2094 : const char * simulator_address = "127.0.0.1";
265 2094 : uint16_t simulator_port_in = SIM_IN_PORT;
266 2094 : uint16_t simulator_port_out = SIM_OUT_PORT;
267 2094 : _irlock_port = IRLOCK_PORT;
268 2094 : struct AP_Param::defaults_table_struct temp_cmdline_param{};
269 :
270 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
271 : // NAME:TCPPORT strings from --net-device options:
272 : const char *net_device_strings[4];
273 2094 : uint8_t num_net_device_strings = 0;
274 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
275 :
276 : // Set default start time to the real system time.
277 : // This will be overwritten if argument provided.
278 : static struct timeval first_tv;
279 2094 : gettimeofday(&first_tv, nullptr);
280 2094 : time_t start_time_UTC = first_tv.tv_sec;
281 2094 : const bool is_example = APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN);
282 :
283 : enum long_options {
284 : CMDLINE_GIMBAL = 1,
285 : CMDLINE_FGVIEW,
286 : CMDLINE_AUTOTESTDIR,
287 : CMDLINE_DEFAULTS,
288 : CMDLINE_UARTA, // must be in A-J order and numbered consecutively
289 : CMDLINE_UARTB,
290 : CMDLINE_UARTC,
291 : CMDLINE_UARTD,
292 : CMDLINE_UARTE,
293 : CMDLINE_UARTF,
294 : CMDLINE_UARTG,
295 : CMDLINE_UARTH,
296 : CMDLINE_UARTI,
297 : CMDLINE_UARTJ,
298 : CMDLINE_SERIAL0, // must be in 0-9 order and numbered consecutively
299 : CMDLINE_SERIAL1,
300 : CMDLINE_SERIAL2,
301 : CMDLINE_SERIAL3,
302 : CMDLINE_SERIAL4,
303 : CMDLINE_SERIAL5,
304 : CMDLINE_SERIAL6,
305 : CMDLINE_SERIAL7,
306 : CMDLINE_SERIAL8,
307 : CMDLINE_SERIAL9,
308 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
309 : CMDLINE_NET_DEVICE,
310 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
311 : CMDLINE_BASE_PORT,
312 : CMDLINE_RCIN_PORT,
313 : CMDLINE_SIM_ADDRESS,
314 : CMDLINE_SIM_PORT_IN,
315 : CMDLINE_SIM_PORT_OUT,
316 : CMDLINE_IRLOCK_PORT,
317 : CMDLINE_START_TIME,
318 : CMDLINE_SYSID,
319 : CMDLINE_SLAVE,
320 : CMDLINE_LIST_MODELS,
321 : CMDLINE_SIM_PERIPH_LOCKSTEP,
322 : #if STORAGE_USE_FLASH
323 : CMDLINE_SET_STORAGE_FLASH_ENABLED,
324 : #endif
325 : #if STORAGE_USE_POSIX
326 : CMDLINE_SET_STORAGE_POSIX_ENABLED,
327 : #endif
328 : #if STORAGE_USE_FRAM
329 : CMDLINE_SET_STORAGE_FRAM_ENABLED,
330 : #endif
331 : #if AP_DDS_ENABLED
332 : CMDLINE_DDS_USE_SIM_TIME,
333 : #endif
334 : };
335 :
336 2094 : const struct GetOptLong::option options[] = {
337 : {"help", false, 0, 'h'},
338 : {"wipe", false, 0, 'w'},
339 : {"unhide-groups", false, 0, 'u'},
340 : {"speedup", true, 0, 's'},
341 : {"rate", true, 0, 'r'},
342 : {"console", false, 0, 'C'},
343 : {"instance", true, 0, 'I'},
344 : {"synthetic-clock", false, 0, 'S'}, // kept to warn that it's always enabled
345 : {"home", true, 0, 'O'},
346 : {"model", true, 0, 'M'},
347 : {"config", true, 0, 'c'},
348 : {"fg", true, 0, 'F'},
349 : {"gimbal", false, 0, CMDLINE_GIMBAL},
350 : {"enable-fgview", false, 0, CMDLINE_FGVIEW},
351 : {"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR},
352 : {"defaults", true, 0, CMDLINE_DEFAULTS},
353 : // {"uartA", true, 0, CMDLINE_UARTA}, // alias for serial0
354 : {"uartB", true, 0, CMDLINE_UARTB},
355 : {"uartC", true, 0, CMDLINE_UARTC},
356 : {"uartD", true, 0, CMDLINE_UARTD},
357 : {"uartE", true, 0, CMDLINE_UARTE},
358 : {"uartF", true, 0, CMDLINE_UARTF},
359 : {"uartG", true, 0, CMDLINE_UARTG},
360 : {"uartH", true, 0, CMDLINE_UARTH},
361 : {"uartI", true, 0, CMDLINE_UARTI},
362 : {"uartJ", true, 0, CMDLINE_UARTJ},
363 : {"serial0", true, 0, CMDLINE_SERIAL0},
364 : {"uartA", true, 0, CMDLINE_SERIAL0}, // for MissionPlanner compatibility
365 : {"serial1", true, 0, CMDLINE_SERIAL1},
366 : {"serial2", true, 0, CMDLINE_SERIAL2},
367 : {"serial3", true, 0, CMDLINE_SERIAL3},
368 : {"serial4", true, 0, CMDLINE_SERIAL4},
369 : {"serial5", true, 0, CMDLINE_SERIAL5},
370 : {"serial6", true, 0, CMDLINE_SERIAL6},
371 : {"serial7", true, 0, CMDLINE_SERIAL7},
372 : {"serial8", true, 0, CMDLINE_SERIAL8},
373 : {"serial9", true, 0, CMDLINE_SERIAL9},
374 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
375 : {"net-device", true, 0, CMDLINE_NET_DEVICE},
376 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
377 : {"base-port", true, 0, CMDLINE_BASE_PORT},
378 : {"rc-in-port", true, 0, CMDLINE_RCIN_PORT},
379 : {"sim-address", true, 0, CMDLINE_SIM_ADDRESS},
380 : {"sim-port-in", true, 0, CMDLINE_SIM_PORT_IN},
381 : {"sim-port-out", true, 0, CMDLINE_SIM_PORT_OUT},
382 : {"irlock-port", true, 0, CMDLINE_IRLOCK_PORT},
383 : {"start-time", true, 0, CMDLINE_START_TIME},
384 : {"sysid", true, 0, CMDLINE_SYSID},
385 : {"slave", true, 0, CMDLINE_SLAVE},
386 : {"list-models", false, 0, CMDLINE_LIST_MODELS},
387 : {"sim-periph-lockstep", false, 0, CMDLINE_SIM_PERIPH_LOCKSTEP},
388 : #if STORAGE_USE_FLASH
389 : {"set-storage-flash-enabled", true, 0, CMDLINE_SET_STORAGE_FLASH_ENABLED},
390 : #endif
391 : #if STORAGE_USE_POSIX
392 : {"set-storage-posix-enabled", true, 0, CMDLINE_SET_STORAGE_POSIX_ENABLED},
393 : #endif
394 : #if STORAGE_USE_FRAM
395 : {"set-storage-fram-enabled", true, 0, CMDLINE_SET_STORAGE_FRAM_ENABLED},
396 : #endif
397 : #if AP_DDS_ENABLED
398 : {"use_sim_time", true, 0, CMDLINE_DDS_USE_SIM_TIME},
399 : #endif
400 : {"vehicle", true, 0, 'v'},
401 : {0, false, 0, 0}
402 : };
403 :
404 : if (is_example) {
405 0 : model_str = "novehicle";
406 0 : HALSITL::UARTDriver::_console = true;
407 : }
408 :
409 : // storage defaults are set here:
410 2094 : bool storage_posix_enabled = true;
411 2094 : bool storage_flash_enabled = false;
412 2094 : bool storage_fram_enabled = false;
413 2094 : bool erase_all_storage = false;
414 :
415 2094 : if (asprintf(&autotest_dir, AP_BUILD_ROOT "/Tools/autotest") <= 0) {
416 0 : AP_HAL::panic("out of memory");
417 : }
418 2094 : _set_signal_handlers();
419 :
420 2094 : setvbuf(stdout, (char *)0, _IONBF, 0);
421 2094 : setvbuf(stderr, (char *)0, _IONBF, 0);
422 :
423 2094 : bool wiping_storage = false;
424 :
425 : GetOptLong gopt(argc, argv, "hwus:r:CI:P:SO:M:F:c:v:",
426 2094 : options);
427 11144 : while (!is_example && (opt = gopt.getoption()) != -1) {
428 9050 : switch (opt) {
429 294 : case 'w':
430 294 : erase_all_storage = true;
431 294 : break;
432 7 : case 'u':
433 7 : AP_Param::set_hide_disabled_groups(false);
434 7 : break;
435 0 : case 's':
436 0 : speedup = strtof(gopt.optarg, nullptr);
437 0 : temp_cmdline_param = {"SIM_SPEEDUP", speedup};
438 0 : cmdline_param.push(temp_cmdline_param);
439 0 : printf("Setting SIM_SPEEDUP=%f\n", speedup);
440 0 : break;
441 0 : case 'r':
442 0 : sim_rate_hz = strtof(gopt.optarg, nullptr);
443 0 : temp_cmdline_param = {"SIM_RATE_HZ", sim_rate_hz};
444 0 : cmdline_param.push(temp_cmdline_param);
445 0 : printf("Setting SIM_RATE_HZ=%f\n", sim_rate_hz);
446 0 : break;
447 0 : case 'C':
448 0 : HALSITL::UARTDriver::_console = true;
449 0 : break;
450 0 : case 'I': {
451 0 : _instance = atoi(gopt.optarg);
452 0 : if (_base_port == BASE_PORT) {
453 0 : _base_port += _instance * 10;
454 : }
455 0 : if (_rcin_path == nullptr && _rcin_port == RCIN_PORT) {
456 0 : _rcin_port += _instance * 10;
457 : }
458 0 : if (_fg_view_port == FG_VIEW_PORT) {
459 0 : _fg_view_port += _instance * 10;
460 : }
461 0 : if (simulator_port_in == SIM_IN_PORT) {
462 0 : simulator_port_in += _instance * 10;
463 : }
464 0 : if (simulator_port_out == SIM_OUT_PORT) {
465 0 : simulator_port_out += _instance * 10;
466 : }
467 0 : if (_irlock_port == IRLOCK_PORT) {
468 0 : _irlock_port += _instance * 10;
469 : }
470 : }
471 0 : break;
472 0 : case 'S':
473 0 : printf("Ignoring obsolete command-line parameter '-S'/'--synthetic-clock'. Synthetic clock mode is now always enabled.\n");
474 0 : break;
475 2139 : case 'O':
476 2139 : home_str = gopt.optarg;
477 2139 : break;
478 2105 : case 'M':
479 2105 : model_str = gopt.optarg;
480 2105 : break;
481 0 : case 'c':
482 0 : config = gopt.optarg;
483 0 : break;
484 0 : case 'F':
485 0 : _fg_address = gopt.optarg;
486 0 : break;
487 0 : case 'v':
488 0 : vehicle_str = gopt.optarg;
489 0 : break;
490 : #if AP_SIM_SOLOGIMBAL_ENABLED
491 1 : case CMDLINE_GIMBAL:
492 1 : enable_gimbal = true;
493 1 : break;
494 : #endif
495 0 : case CMDLINE_FGVIEW:
496 0 : _use_fg_view = true;
497 0 : break;
498 0 : case CMDLINE_AUTOTESTDIR:
499 0 : autotest_dir = strdup(gopt.optarg);
500 0 : break;
501 2094 : case CMDLINE_DEFAULTS:
502 2094 : defaults_path = strdup(gopt.optarg);
503 2094 : break;
504 0 : case CMDLINE_UARTA:
505 : case CMDLINE_UARTB:
506 : case CMDLINE_UARTC:
507 : case CMDLINE_UARTD:
508 : case CMDLINE_UARTE:
509 : case CMDLINE_UARTF:
510 : case CMDLINE_UARTG:
511 : case CMDLINE_UARTH:
512 : case CMDLINE_UARTI:
513 : case CMDLINE_UARTJ: {
514 0 : int uart_idx = opt - CMDLINE_UARTA;
515 : // ordering captures the historical use of uartB as SERIAL3
516 : static const uint8_t mapping[] = { 0, 3, 1, 2, 4, 5, 6, 7, 8, 9 };
517 0 : int serial_idx = mapping[uart_idx];
518 0 : char uart_letter = (char)(uart_idx)+'A';
519 0 : printf("ERROR: Removed option --uart%c supplied. "
520 : "Use --serial%d instead.\n", uart_letter, serial_idx);
521 0 : exit(1);
522 : }
523 2390 : case CMDLINE_SERIAL0:
524 : case CMDLINE_SERIAL1:
525 : case CMDLINE_SERIAL2:
526 : case CMDLINE_SERIAL3:
527 : case CMDLINE_SERIAL4:
528 : case CMDLINE_SERIAL5:
529 : case CMDLINE_SERIAL6:
530 : case CMDLINE_SERIAL7:
531 : case CMDLINE_SERIAL8:
532 : case CMDLINE_SERIAL9:
533 2390 : _serial_path[opt - CMDLINE_SERIAL0] = gopt.optarg;
534 2390 : break;
535 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
536 1 : case CMDLINE_NET_DEVICE:
537 : // the simulated device is created below, once the vehicle
538 : // model it may attach itself to exists
539 1 : if (num_net_device_strings >= ARRAY_SIZE(net_device_strings)) {
540 0 : printf("Too many --net-device options\n");
541 0 : exit(1);
542 : }
543 1 : net_device_strings[num_net_device_strings++] = gopt.optarg;
544 1 : break;
545 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
546 0 : case CMDLINE_BASE_PORT:
547 0 : _base_port = atoi(gopt.optarg);
548 0 : break;
549 1 : case CMDLINE_RCIN_PORT:
550 1 : if (strncmp(gopt.optarg, "uds:", 4) == 0) {
551 0 : _rcin_path = &gopt.optarg[4];
552 0 : if (_rcin_path[0] == '\0') {
553 0 : printf("--rc-in-port requires a path after uds:\n");
554 0 : exit(1);
555 : }
556 : } else {
557 1 : _rcin_path = nullptr;
558 1 : _rcin_port = atoi(gopt.optarg);
559 : }
560 1 : break;
561 0 : case CMDLINE_SIM_ADDRESS:
562 0 : simulator_address = gopt.optarg;
563 0 : break;
564 0 : case CMDLINE_SIM_PORT_IN:
565 0 : simulator_port_in = atoi(gopt.optarg);
566 0 : break;
567 0 : case CMDLINE_SIM_PORT_OUT:
568 0 : simulator_port_out = atoi(gopt.optarg);
569 0 : break;
570 0 : case CMDLINE_IRLOCK_PORT:
571 0 : _irlock_port = atoi(gopt.optarg);
572 0 : break;
573 0 : case CMDLINE_START_TIME:
574 0 : start_time_UTC = atoi(gopt.optarg);
575 0 : break;
576 0 : case CMDLINE_SYSID: {
577 0 : const int32_t sysid = atoi(gopt.optarg);
578 0 : if (sysid < 1 || sysid > 255) {
579 0 : fprintf(stderr, "You must specify a SYSID greater than 0 and less than 256\n");
580 0 : exit(1);
581 : }
582 0 : temp_cmdline_param = {"MAV_SYSID", static_cast<float>(sysid)};
583 0 : cmdline_param.push(temp_cmdline_param);
584 0 : printf("Setting MAV_SYSID=%d\n", sysid);
585 0 : break;
586 : }
587 : #if STORAGE_USE_POSIX
588 6 : case CMDLINE_SET_STORAGE_POSIX_ENABLED:
589 6 : storage_posix_enabled = atoi(gopt.optarg);
590 6 : break;
591 : #endif
592 : #if STORAGE_USE_FLASH
593 3 : case CMDLINE_SET_STORAGE_FLASH_ENABLED:
594 3 : storage_flash_enabled = atoi(gopt.optarg);
595 3 : break;
596 : #endif
597 : #if STORAGE_USE_FRAM
598 3 : case CMDLINE_SET_STORAGE_FRAM_ENABLED:
599 3 : storage_fram_enabled = atoi(gopt.optarg);
600 3 : break;
601 : #endif
602 : #if AP_DDS_ENABLED
603 : case CMDLINE_DDS_USE_SIM_TIME:
604 : if (strcasecmp(gopt.optarg, "true") == 0) {
605 : _use_dds_sim_time = true;
606 : } else {
607 : _use_dds_sim_time = false;
608 : }
609 : break;
610 : #endif
611 0 : case 'h':
612 0 : _usage();
613 0 : exit(0);
614 0 : case CMDLINE_SLAVE: {
615 : #if AP_SIM_JSON_MASTER_ENABLED
616 0 : const int32_t slaves = atoi(gopt.optarg);
617 0 : if (slaves > 0) {
618 0 : ride_along.init(slaves);
619 : }
620 : #endif // AP_SIM_JSON_MASTER_ENABLED
621 0 : break;
622 : }
623 0 : case CMDLINE_LIST_MODELS:
624 0 : list_models_and_exit();
625 0 : break;
626 6 : case CMDLINE_SIM_PERIPH_LOCKSTEP:
627 6 : _periph_lockstep = true;
628 6 : break;
629 0 : default:
630 0 : _usage();
631 0 : exit(1);
632 : }
633 : }
634 :
635 : // if no explicit --defaults was given, try resolving via the embedded
636 : // vehicleinfo.json. This makes the binary self-sufficient: a user
637 : // running `arduplane --model=quadplane-tilt` from a directory without
638 : // the source tree gets the right parameter defaults from ROMFS.
639 2094 : resolve_defaults_from_romfs(model_str, vehicle_str);
640 :
641 2094 : if (!model_str) {
642 0 : printf("You must specify a vehicle model. Options are:\n");
643 0 : for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) {
644 0 : printf(" %s\n", model_constructors[i].name);
645 : }
646 : // spit this out again as the original message probably just
647 : // scrolled off the screen:
648 0 : printf("You must specify a vehicle model.\n");
649 0 : exit(1);
650 : }
651 :
652 2094 : _model_str = model_str;
653 :
654 2094 : if (AP::sitl() != nullptr) { // some examples don't instantiate this object
655 2094 : AP::sitl()->init();
656 : }
657 :
658 46102 : for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) {
659 46102 : if (strncasecmp(model_constructors[i].name, model_str, strlen(model_constructors[i].name)) == 0) {
660 : // printf("Creating model %f,%f,%f,%f at speed %.1f\n", opos.lat, opos.lng, opos.alt, opos.hdg, speedup);
661 2094 : sitl_model = model_constructors[i].constructor(model_str);
662 2094 : if (home_str != nullptr) {
663 2094 : Location home;
664 : float home_yaw;
665 2094 : if (strchr(home_str,',') == nullptr) {
666 8 : if (!lookup_location(home_str, home, home_yaw)) {
667 0 : ::printf("Failed to find location (%s). Should be in locations.txt or LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str);
668 0 : exit(1);
669 : }
670 2086 : } else if (!parse_home(home_str, home, home_yaw)) {
671 0 : ::printf("Failed to parse home string (%s). Should be LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str);
672 0 : exit(1);
673 : }
674 2094 : sitl_model->set_start_location(home, home_yaw);
675 : }
676 2094 : sitl_model->set_interface_ports(simulator_address, simulator_port_in, simulator_port_out);
677 2094 : sitl_model->set_speedup(speedup);
678 2094 : sitl_model->set_instance(_instance);
679 2094 : sitl_model->set_autotest_dir(autotest_dir);
680 2094 : sitl_model->set_config(config);
681 2094 : sitl_model->launch_external_sim();
682 2094 : break;
683 : }
684 : }
685 2094 : if (sitl_model == nullptr) {
686 0 : printf("Vehicle model (%s) not found\n", model_str);
687 0 : exit(1);
688 : }
689 :
690 : #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
691 : // create devices which the autopilot connects to over the network
692 : // rather than over a serial port. This is done here as some
693 : // devices attach themselves to the vehicle model:
694 2095 : for (uint8_t i=0; i<num_net_device_strings; i++) {
695 1 : create_net_serial_sim(net_device_strings[i]);
696 : }
697 : #endif // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
698 :
699 2094 : if (storage_posix_enabled && storage_flash_enabled) {
700 : // this will change in the future!
701 0 : printf("Only one of flash or posix storage may be selected");
702 0 : exit(1);
703 : }
704 :
705 2094 : if (AP::sitl()) {
706 : // Set SITL start time.
707 2094 : AP::sitl()->start_time_UTC = start_time_UTC;
708 : }
709 :
710 2094 : hal.set_storage_posix_enabled(storage_posix_enabled);
711 2094 : hal.set_storage_flash_enabled(storage_flash_enabled);
712 2094 : hal.set_storage_fram_enabled(storage_fram_enabled);
713 :
714 2094 : if (erase_all_storage) {
715 294 : AP_Param::erase_all();
716 294 : unlink("flash.dat");
717 294 : hal.set_wipe_storage(wiping_storage);
718 : }
719 :
720 2094 : fprintf(stdout, "Starting sketch '%s'\n", vehicle_str);
721 :
722 2094 : if (strcmp(vehicle_str, "ArduCopter") == 0) {
723 1001 : _vehicle = ArduCopter;
724 1093 : } else if (strcmp(vehicle_str, "Rover") == 0) {
725 285 : _vehicle = Rover;
726 808 : } else if (strcmp(vehicle_str, "ArduSub") == 0) {
727 83 : _vehicle = ArduSub;
728 725 : } else if (strcmp(vehicle_str, "Blimp") == 0) {
729 0 : _vehicle = Blimp;
730 : } else {
731 725 : _vehicle = ArduPlane;
732 : }
733 :
734 2094 : _sitl_setup();
735 2094 : }
736 :
737 : /*
738 : parse a home string into a location and yaw
739 : */
740 2094 : bool SITL_State::parse_home(const char *home_str, Location &loc, float &yaw_degrees)
741 : {
742 2094 : char *saveptr = nullptr;
743 2094 : char *s = strdup(home_str);
744 2094 : if (!s) {
745 0 : free(s);
746 0 : ::printf("No home string supplied\n");
747 0 : return false;
748 : }
749 2094 : char *lat_s = strtok_r(s, ",", &saveptr);
750 2094 : if (!lat_s) {
751 0 : free(s);
752 0 : ::printf("Failed to parse latitude\n");
753 0 : return false;
754 : }
755 2094 : char *lon_s = strtok_r(nullptr, ",", &saveptr);
756 2094 : if (!lon_s) {
757 0 : free(s);
758 0 : ::printf("Failed to parse longitude\n");
759 0 : return false;
760 : }
761 2094 : char *alt_s = strtok_r(nullptr, ",", &saveptr);
762 2094 : if (!alt_s) {
763 0 : free(s);
764 0 : ::printf("Failed to parse altitude\n");
765 0 : return false;
766 : }
767 2094 : char *yaw_s = strtok_r(nullptr, ",", &saveptr);
768 2094 : if (!yaw_s) {
769 0 : free(s);
770 0 : ::printf("Failed to parse yaw\n");
771 0 : return false;
772 : }
773 :
774 2094 : loc = {};
775 2094 : loc.lat = static_cast<int32_t>(strtod(lat_s, nullptr) * 1.0e7);
776 2094 : loc.lng = static_cast<int32_t>(strtod(lon_s, nullptr) * 1.0e7);
777 2094 : loc.alt = static_cast<int32_t>(strtod(alt_s, nullptr) * 1.0e2);
778 :
779 2094 : if (loc.lat == 0 && loc.lng == 0) {
780 : // default to CMAC instead of middle of the ocean. This makes
781 : // SITL in MissionPlanner a bit more useful
782 0 : loc.lat = -35.363261*1e7;
783 0 : loc.lng = 149.165230*1e7;
784 0 : loc.alt = 584*100;
785 : }
786 :
787 2094 : yaw_degrees = strtof(yaw_s, nullptr);
788 2094 : free(s);
789 :
790 2094 : return true;
791 : }
792 :
793 : /*
794 : lookup a location in locations.txt in ROMFS
795 : */
796 8 : bool SITL_State::lookup_location(const char *home_str, Location &loc, float &yaw_degrees)
797 : {
798 8 : const char *locations = "@ROMFS/locations.txt";
799 8 : FileData *fd = AP::FS().load_file(locations);
800 8 : if (fd == nullptr) {
801 0 : ::printf("Missing %s\n", locations);
802 0 : return false;
803 : }
804 8 : char *str = strndup((const char *)fd->data, fd->length);
805 8 : if (!str) {
806 0 : delete fd;
807 0 : return false;
808 : }
809 8 : size_t len = strlen(home_str);
810 8 : char *saveptr = nullptr;
811 8 : for (char *s = strtok_r(str, "\r\n", &saveptr);
812 746 : s;
813 738 : s=strtok_r(nullptr, "\r\n", &saveptr)) {
814 746 : if (strncasecmp(s, home_str, len) == 0 && s[len]=='=') {
815 8 : bool ok = parse_home(&s[len+1], loc, yaw_degrees);
816 8 : free(str);
817 8 : delete fd;
818 8 : return ok;
819 : }
820 : }
821 0 : free(str);
822 0 : delete fd;
823 0 : ::printf("Failed to find location '%s'\n", home_str);
824 0 : return false;
825 : }
826 :
827 : /*
828 : append one parm filename to the comma-separated @ROMFS/... list
829 : */
830 3199 : static void append_romfs_path(std::string &joined, const std::string &name)
831 : {
832 3199 : if (!joined.empty()) {
833 1149 : joined += ",";
834 : }
835 3199 : joined += "@ROMFS/";
836 3199 : joined += name;
837 3199 : }
838 :
839 : /*
840 : given a frame object from vehicleinfo.json, append its
841 : default_params_filename entries (string or array) to joined.
842 : */
843 2094 : static void append_frame_defaults(std::string &joined, const AP_JSON::value &frame)
844 : {
845 2094 : const AP_JSON::value &dpf = frame.get("default_params_filename");
846 2094 : if (dpf.is<AP_JSON::null>()) {
847 44 : return;
848 : }
849 2050 : if (dpf.is<std::string>()) {
850 1429 : append_romfs_path(joined, dpf.get<std::string>());
851 1429 : return;
852 : }
853 621 : if (dpf.is<AP_JSON::value::array>()) {
854 621 : const AP_JSON::value::array &arr = dpf.get<AP_JSON::value::array>();
855 2391 : for (const auto &v : arr) {
856 1770 : if (v.is<std::string>()) {
857 1770 : append_romfs_path(joined, v.get<std::string>());
858 : }
859 : }
860 : }
861 : }
862 :
863 : /*
864 : search a single vehicle entry for a frame matching model_str. Returns
865 : true and fills `joined` if found.
866 :
867 : A frame matches when either its JSON key equals model_str (e.g. "X",
868 : "octa-quad") or its explicit "model" field equals model_str (e.g.
869 : "Callisto" defines model "octa-quad:@ROMFS/models/Callisto.json").
870 : Without the second check, frames with a custom model string would
871 : not load their per-frame defaults.
872 : */
873 2202 : static bool resolve_frame_in_vehicle(const AP_JSON::value &vehicle,
874 : const char *model_str,
875 : std::string &joined)
876 : {
877 2202 : if (!vehicle.is<AP_JSON::value::object>()) {
878 0 : return false;
879 : }
880 2202 : const AP_JSON::value &frames = vehicle.get("frames");
881 2202 : if (!frames.is<AP_JSON::value::object>()) {
882 0 : return false;
883 : }
884 2202 : if (frames.contains(std::string(model_str))) {
885 2065 : append_frame_defaults(joined, frames.get(std::string(model_str)));
886 2065 : return true;
887 : }
888 137 : const AP_JSON::value::object &frames_obj = frames.get<AP_JSON::value::object>();
889 1474 : for (const auto &kv : frames_obj) {
890 1366 : if (!kv.second.is<AP_JSON::value::object>()) {
891 0 : continue;
892 : }
893 1366 : const AP_JSON::value &model_val = kv.second.get("model");
894 1462 : if (model_val.is<std::string>() &&
895 96 : model_val.get<std::string>() == model_str) {
896 29 : append_frame_defaults(joined, kv.second);
897 29 : return true;
898 : }
899 : }
900 108 : return false;
901 : }
902 :
903 : // Search own vehicle first, then all vehicles, for a single candidate string.
904 2106 : static bool search_once(const AP_JSON::value *root, const char *vehicle_str,
905 : const std::string &candidate, std::string &out)
906 : {
907 2106 : if (vehicle_str != nullptr && root->contains(std::string(vehicle_str))) {
908 2106 : if (resolve_frame_in_vehicle(root->get(std::string(vehicle_str)),
909 : candidate.c_str(), out)) {
910 2094 : return true;
911 : }
912 : }
913 12 : if (root->is<AP_JSON::value::object>()) {
914 12 : const AP_JSON::value::object &top = root->get<AP_JSON::value::object>();
915 120 : for (const auto &kv : top) {
916 108 : if (vehicle_str != nullptr && kv.first == vehicle_str) {
917 12 : continue; // already tried
918 : }
919 96 : if (resolve_frame_in_vehicle(kv.second, candidate.c_str(), out)) {
920 0 : return true;
921 : }
922 : }
923 : }
924 12 : return false;
925 : }
926 :
927 : /*
928 : look up the model in @ROMFS/vehicleinfo.json and prepend the matching
929 : @ROMFS/<path>,@ROMFS/<path>... list to defaults_path. AP_Param reads
930 : the comma-separated list left-to-right with later files overriding
931 : earlier ones, so any user-supplied --defaults paths layer on top of
932 : the embedded per-frame defaults.
933 :
934 : We first look under the binary's own vehicle (AP_BUILD_TARGET_NAME)
935 : and fall back to scanning every top-level vehicle. The scan covers
936 : the case where a heli binary (AP_BUILD_TARGET_NAME=ArduCopter) is
937 : asked for a frame that lives under "Helicopter" in the JSON.
938 :
939 : If the full model string is not found, trailing dash-separated suffixes
940 : are stripped and the lookup is retried (e.g. "plane-catapult" falls back
941 : to "plane"). This mirrors the model-constructor prefix matching so that
942 : physics variants automatically inherit the base frame's defaults.
943 : */
944 2094 : void SITL_State::resolve_defaults_from_romfs(const char *model_str, const char *vehicle_str)
945 : {
946 2094 : if (model_str == nullptr) {
947 0 : return;
948 : }
949 :
950 2094 : AP_JSON::value *root = AP_JSON::load_json("@ROMFS/vehicleinfo.json");
951 2094 : if (root == nullptr) {
952 0 : return; // no embedded data — graceful no-op
953 : }
954 :
955 2094 : std::string joined;
956 2094 : bool found = false;
957 2094 : std::string candidate = model_str;
958 :
959 2106 : while (!found) {
960 2106 : found = search_once(root, vehicle_str, candidate, joined);
961 2106 : if (found) {
962 2094 : break;
963 : }
964 12 : const size_t dash = candidate.rfind('-');
965 12 : if (dash == std::string::npos) {
966 0 : break;
967 : }
968 12 : candidate = candidate.substr(0, dash);
969 : }
970 :
971 2094 : if (found && !joined.empty()) {
972 2050 : if (defaults_path != nullptr) {
973 : // prepend ROMFS list so explicit --defaults files override
974 2050 : std::string combined = joined + "," + defaults_path;
975 2050 : defaults_path = strdup(combined.c_str());
976 2050 : } else {
977 0 : defaults_path = strdup(joined.c_str());
978 : }
979 : }
980 :
981 2094 : delete root;
982 2094 : }
983 :
984 : /*
985 : Implementation of --list-models: dump the embedded vehicleinfo.json
986 : to stdout so a GCS can enumerate which models the binary supports.
987 : */
988 0 : void SITL_State::list_models_and_exit(void)
989 : {
990 0 : FileData *fd = AP::FS().load_file("@ROMFS/vehicleinfo.json");
991 0 : if (fd == nullptr) {
992 0 : ::fprintf(stderr, "vehicleinfo.json not embedded in this binary\n");
993 0 : exit(1);
994 : }
995 0 : fwrite(fd->data, 1, fd->length, stdout);
996 0 : delete fd;
997 0 : exit(0);
998 : }
999 :
1000 : #endif
|
