Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-14 08:55:59 +08:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Change interface port to a generic solution for external simulator

Browse Source
This commit is contained in:
Pierre Kancir
2017-03-18 19:58:11 +11:00
committed by Andrew Tridgell
parent 6e0319d513
commit 8054405521
6 changed files with 60 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_HAL_SITL/SITL_State.h
+3 -3
View File
@@ -171,9 +171,9 @@ private:
bool _use_fg_view;
const char *_fdm_address;
const char *_gazebo_address;
int _gazebo_port_in;
int _gazebo_port_out;
const char *_simulator_address;
int _simulator_port_in;
int _simulator_port_out;
// delay buffer variables
static const uint8_t mag_buffer_length = 250;
libraries/AP_HAL_SITL/SITL_cmdline.cpp
+25 -27
View File
@@ -58,9 +58,9 @@ void SITL_State::_usage(void)
"\t--uartC device set device string for UARTC\n"
"\t--uartD device set device string for UARTD\n"
"\t--uartE device set device string for UARTE\n"
"\t--gazebo-address ADDR set address string for gazebo\n"
"\t--gazebo-port-in PORT set port num for gazebo in\n"
"\t--gazebo-port-out PORT set port num for gazebo out\n"
"\t--sim-address ADDR set address string for simulator\n"
"\t--sim-port-in PORT set port num for simulator in\n"
"\t--sim-port-out PORT set port num for simulator out\n"
"\t--irlock-port PORT set port num for irlock\n"
"\t--rc-in-port PORT set port num for rc in\n"
"\t--rc-out-port PORT set port num for rc out\n"
@@ -140,8 +140,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
const int RCOUT_PORT = 5502;
const int RCIN_PORT = 5501;
const int FG_VIEW_PORT = 5503;
const int GAZEBO_IN_PORT = 9003;
const int GAZEBO_OUT_PORT = 9002;
const int SIM_IN_PORT = 9003;
const int SIM_OUT_PORT = 9002;
const int IRLOCK_PORT = 9005;
_base_port = BASE_PORT;
_rcout_port = RCOUT_PORT;
@@ -150,9 +150,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_fdm_address = "127.0.0.1";
_client_address = nullptr;
_use_fg_view = true;
_gazebo_address = "127.0.0.1";
_gazebo_port_in = GAZEBO_IN_PORT;
_gazebo_port_out = GAZEBO_OUT_PORT;
_simulator_address = "127.0.0.1";
_simulator_port_in = SIM_IN_PORT;
_simulator_port_out = SIM_OUT_PORT;
_irlock_port = IRLOCK_PORT;
_instance = 0;
@@ -168,9 +168,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
CMDLINE_UARTF,
CMDLINE_RTSCTS,
CMDLINE_FGVIEW,
CMDLINE_GAZEBO_ADDRESS,
CMDLINE_GAZEBO_PORT_IN,
CMDLINE_GAZEBO_PORT_OUT,
CMDLINE_SIM_ADDRESS,
CMDLINE_SIM_PORT_IN,
CMDLINE_SIM_PORT_OUT,
CMDLINE_BASE_PORT,
CMDLINE_IRLOCK_PORT,
CMDLINE_RCIN_PORT,
@@ -201,9 +201,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{"defaults", true, 0, CMDLINE_DEFAULTS},
{"rtscts", false, 0, CMDLINE_RTSCTS},
{"disable-fgview", false, 0, CMDLINE_FGVIEW},
{"gazebo-address", true, 0, CMDLINE_GAZEBO_ADDRESS},
{"gazebo-port-in", true, 0, CMDLINE_GAZEBO_PORT_IN},
{"gazebo-port-out", true, 0, CMDLINE_GAZEBO_PORT_OUT},
{"sim-address", true, 0, CMDLINE_SIM_ADDRESS},
{"sim-port-in", true, 0, CMDLINE_SIM_PORT_IN},
{"sim-port-out", true, 0, CMDLINE_SIM_PORT_OUT},
{"base-port", true, 0, CMDLINE_BASE_PORT},
{"irlock-port", true, 0, CMDLINE_IRLOCK_PORT},
{"rc-in-port", true, 0, CMDLINE_RCIN_PORT},
@@ -243,11 +243,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
if (_fg_view_port == FG_VIEW_PORT) {
_fg_view_port += _instance * 10;
}
if (_gazebo_port_in == GAZEBO_IN_PORT) {
_gazebo_port_in += _instance * 10;
if (_simulator_port_in == SIM_IN_PORT) {
_simulator_port_in += _instance * 10;
}
if (_gazebo_port_out == GAZEBO_OUT_PORT) {
_gazebo_port_out += _instance * 10;
if (_simulator_port_out == SIM_OUT_PORT) {
_simulator_port_out += _instance * 10;
}
if (_irlock_port == IRLOCK_PORT) {
_irlock_port += _instance * 10;
@@ -299,14 +299,14 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
case CMDLINE_FGVIEW:
_use_fg_view = false;
break;
case CMDLINE_GAZEBO_ADDRESS:
_gazebo_address = gopt.optarg;
case CMDLINE_SIM_ADDRESS:
_simulator_address = gopt.optarg;
break;
case CMDLINE_GAZEBO_PORT_IN:
_gazebo_port_in = atoi(gopt.optarg);
case CMDLINE_SIM_PORT_IN:
_simulator_port_in = atoi(gopt.optarg);
break;
case CMDLINE_GAZEBO_PORT_OUT:
_gazebo_port_out = atoi(gopt.optarg);
case CMDLINE_SIM_PORT_OUT:
_simulator_port_out = atoi(gopt.optarg);
break;
case CMDLINE_IRLOCK_PORT:
_irlock_port = atoi(gopt.optarg);
@@ -334,9 +334,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) {
if (strncasecmp(model_constructors[i].name, model_str, strlen(model_constructors[i].name)) == 0) {
sitl_model = model_constructors[i].constructor(home_str, model_str);
if (strncasecmp("gazebo", model_str, strlen("gazebo")) == 0) {
sitl_model->set_interface_ports(_gazebo_address, _gazebo_port_in, _gazebo_port_out);
}
sitl_model->set_interface_ports(_simulator_address, _simulator_port_in, _simulator_port_out);
sitl_model->set_speedup(speedup);
sitl_model->set_instance(_instance);
sitl_model->set_autotest_dir(autotest_dir);
libraries/SITL/SIM_Aircraft.cpp
-26
View File
@@ -55,8 +55,6 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
rate_hz(1200.0f),
autotest_dir(nullptr),
frame(frame_str),
socket_in(true),
socket_out(true),
#ifdef __CYGWIN__
min_sleep_time(20000)
#else
@@ -412,30 +410,6 @@ void Aircraft::set_speedup(float speedup)
setup_frame_time(rate_hz, speedup);
}
/*
Create and set in/out socket
*/
void Aircraft::set_interface_ports(const char* address, const int port_in, const int port_out)
{
if (!socket_in.bind("127.0.0.1", port_in)) {
fprintf(stderr, "SITL: socket in bind failed on sim in : %d - %s\n", port_in, strerror(errno));
fprintf(stderr, "Abording launch...\n");
exit(1);
}
printf("Bind %s:%d for SITL in\n", "127.0.0.1", port_in);
socket_in.reuseaddress();
socket_in.set_blocking(false);
if (!socket_out.connect(address, port_out)) {
fprintf(stderr, "SITL: socket out bind failed on sim out : %d - %s\n", port_out, strerror(errno));
fprintf(stderr, "Abording launch...\n");
exit(1);
}
printf("Bind %s:%d for SITL out\n", address, port_out);
socket_out.reuseaddress();
socket_out.set_blocking(false);
}
/*
update the simulation attitude and relative position
*/
libraries/SITL/SIM_Aircraft.h
+2 -5
View File
@@ -68,8 +68,8 @@ public:
autotest_dir = _autotest_dir;
}
/* Create and set in/out socket */
void set_interface_ports(const char* address, const int port_in, const int port_out);
/* Create and set in/out socket for extenal simulator */
virtual void set_interface_ports(const char* address, const int port_in, const int port_out) {};
/*
step the FDM by one time step
@@ -164,9 +164,6 @@ protected:
bool use_time_sync = true;
float last_speedup = -1;
SocketAPM socket_in;
SocketAPM socket_out;
enum {
GROUND_BEHAVIOR_NONE = 0,
GROUND_BEHAVIOR_NO_MOVEMENT,
libraries/SITL/SIM_Gazebo.cpp
+24 -4
View File
@@ -29,7 +29,8 @@ namespace SITL {
Gazebo::Gazebo(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str),
last_timestamp(0)
last_timestamp(0),
socket_sitl{true}
{
// try to bind to a specific port so that if we restart ArduPilot
// Gazebo keeps sending us packets. Not strictly necessary but
@@ -38,6 +39,25 @@ Gazebo::Gazebo(const char *home_str, const char *frame_str) :
}
/*
Create and set in/out socket
*/
void Gazebo::set_interface_ports(const char* address, const int port_in, const int port_out)
{
if (!socket_sitl.bind("0.0.0.0", port_in)) {
fprintf(stderr, "SITL: socket in bind failed on sim in : %d - %s\n", port_in, strerror(errno));
fprintf(stderr, "Abording launch...\n");
exit(1);
}
printf("Bind %s:%d for SITL in\n", "127.0.0.1", port_in);
socket_sitl.reuseaddress();
socket_sitl.set_blocking(false);
_gazebo_address = address;
_gazebo_port = port_out;
printf("Setting Gazebo interface to %s:%d \n", _gazebo_address, _gazebo_port);
}
/*
decode and send servos
*/
@@ -50,7 +70,7 @@ void Gazebo::send_servos(const struct sitl_input &input)
{
pkt.motor_speed[i] = (input.servos[i]-1000) / 1000.0f;
}
socket_out.send(&pkt, sizeof(pkt));
socket_sitl.sendto(&pkt, sizeof(pkt), _gazebo_address, _gazebo_port);
}
/*
@@ -65,7 +85,7 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
we re-send the servo packet every 0.1 seconds until we get a
reply. This allows us to cope with some packet loss to the FDM
*/
while (socket_in.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
while (socket_sitl.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
send_servos(input);
}
@@ -123,7 +143,7 @@ void Gazebo::drain_sockets()
ssize_t received;
errno = 0;
do {
received = socket_in.recv(buf, buflen, 0);
received = socket_sitl.recv(buf, buflen, 0);
if (received < 0) {
if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
fprintf(stderr, "error recv on socket in: %s \n",
libraries/SITL/SIM_Gazebo.h
+6
View File
@@ -37,6 +37,9 @@ public:
return new Gazebo(home_str, frame_str);
}
/* Create and set in/out socket for Gazebo simulator */
void set_interface_ports(const char* address, const int port_in, const int port_out);
private:
/*
packet sent to Gazebo
@@ -64,6 +67,9 @@ private:
double last_timestamp;
SocketAPM socket_sitl;
const char *_gazebo_address = "127.0.0.1";
int _gazebo_port = 9002;
};
} // namespace SITL