fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 04:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulator: added TCP, general cleanup, lockstep

Browse Source 
This adds the option to connect over UDP or TCP for mavlink to the
SITL simulator.

Also, this includes a bunch of general cleanup and refactoring of the
simulator interface code. For instance sending of mavlink messages was
put into separate functions and unneccessary comments were removed.

Also, this now sets the timestamp sent by the SITL simulator in the
HIL_SENSOR message in order to enable lockstep.
This commit is contained in:
Julian Oes
2018-11-13 15:18:23 +01:00
parent cf39fdd37b
commit 6d7fb232dd
3 changed files with 181 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.cpp
+42 -46
View File
@@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,7 +34,9 @@
/**
* @file simulator.cpp
* A device simulator
*
* This module interfaces via MAVLink to a software in the loop simulator (SITL)
* such as jMAVSim or Gazebo.
*/
#include <px4_log.h>
@@ -141,47 +144,54 @@ void Simulator::parameters_update(bool force)
int Simulator::start(int argc, char *argv[])
{
int ret = 0;
int udp_port = 14560;
_instance = new Simulator();
if (_instance) {
drv_led_start();
InternetProtocol ip = InternetProtocol::UDP;
unsigned port = 14560;
if (argc == 5 && strcmp(argv[3], "-u") == 0) {
udp_port = atoi(argv[4]);
ip = InternetProtocol::UDP;
port = atoi(argv[4]);
}
if (argc == 5 && strcmp(argv[3], "-t") == 0) {
ip = InternetProtocol::TCP;
port = atoi(argv[4]);
}
if (argv[2][1] == 's') {
_instance->initializeSensorData();
#ifndef __PX4_QURT
// Update sensor data
_instance->pollForMAVLinkMessages(false, udp_port);
_instance->pollForMAVLinkMessages(false, ip, port);
#endif
} else if (argv[2][1] == 'p') {
// Update sensor data
_instance->pollForMAVLinkMessages(true, udp_port);
_instance->pollForMAVLinkMessages(true, ip, port);
} else {
_instance->initializeSensorData();
_instance->_initialized = true;
}
return 0;
} else {
PX4_WARN("Simulator creation failed");
ret = 1;
return 1;
}
return ret;
}
static void usage()
{
PX4_WARN("Usage: simulator {start -[spt] [-u udp_port] |stop}");
PX4_WARN("Usage: simulator {start -[sp] [-u udp_port / -t tcp_port] |stop}");
PX4_WARN("Simulate raw sensors: simulator start -s");
PX4_WARN("Publish sensors combined: simulator start -p");
PX4_WARN("Dummy unit test data: simulator start -t");
PX4_WARN("Connect using UDP: simulator start -u udp_port");
PX4_WARN("Connect using TCP: simulator start -t tcp_port");
}
__BEGIN_DECLS
@@ -192,41 +202,27 @@ extern "C" {
int simulator_main(int argc, char *argv[])
{
int ret = 0;
if (argc == 5 && strcmp(argv[1], "start") == 0) {
if (argc > 2 && strcmp(argv[1], "start") == 0) {
if (strcmp(argv[2], "-s") == 0 ||
strcmp(argv[2], "-p") == 0 ||
strcmp(argv[2], "-t") == 0) {
if (g_sim_task >= 0) {
PX4_WARN("Simulator already started");
return 0;
}
if (g_sim_task >= 0) {
warnx("Simulator already started");
return 0;
g_sim_task = px4_task_spawn_cmd("simulator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1500,
Simulator::start,
argv);
while (true) {
if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
break;
} else {
system_sleep(1);
}
// enable lockstep support
px4_enable_sim_lockstep();
g_sim_task = px4_task_spawn_cmd("simulator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
Simulator::start,
argv);
// now wait for the command to complete
while (!px4_exit_requested()) {
if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
break;
} else {
px4_usleep(100000);
}
}
} else {
usage();
ret = -EINVAL;
}
} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
@@ -240,10 +236,10 @@ extern "C" {
} else {
usage();
ret = -EINVAL;
return 1;
}
return ret;
return 0;
}
}