Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

simulator: fix TCP on Cygwin-Windows and updated two world files for Gazebo #11101

Merged
merged 2 commits into from
Dec 27, 2018
Merged

simulator: fix TCP on Cygwin-Windows and updated two world files for Gazebo #11101

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
30ccb78
simulator: fix TCP on Cygwin-Windows
julianoes Dec 26, 2018
9ef390c
sitl_gazebo: update submodule
julianoes Dec 26, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Tools/sitl_gazebo
Open in desktop
Submodule sitl_gazebo updated from c90e8a to e94574
25 changes: 16 additions & 9 deletions src/modules/simulator/simulator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,29 +149,26 @@ int Simulator::start(int argc, char *argv[])
if (_instance) {
drv_led_start();

InternetProtocol ip = InternetProtocol::UDP;
unsigned port = 14560;

if (argc == 5 && strcmp(argv[3], "-u") == 0) {
ip = InternetProtocol::UDP;
port = atoi(argv[4]);
_instance->set_ip(InternetProtocol::UDP);
_instance->set_port(atoi(argv[4]));
}

if (argc == 5 && strcmp(argv[3], "-c") == 0) {
ip = InternetProtocol::TCP;
port = atoi(argv[4]);
_instance->set_ip(InternetProtocol::TCP);
_instance->set_port(atoi(argv[4]));
}

if (argv[2][1] == 's') {
_instance->initializeSensorData();
#ifndef __PX4_QURT
// Update sensor data
_instance->pollForMAVLinkMessages(false, ip, port);
_instance->pollForMAVLinkMessages(false);
#endif

} else if (argv[2][1] == 'p') {
// Update sensor data
_instance->pollForMAVLinkMessages(true, ip, port);
_instance->pollForMAVLinkMessages(true);

} else {
_instance->initializeSensorData();
Expand All @@ -186,6 +183,16 @@ int Simulator::start(int argc, char *argv[])
}
}

void Simulator::set_ip(InternetProtocol ip)
{
_ip = ip;
}

void Simulator::set_port(unsigned port)
{
_port = port;
}

static void usage()
{
PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");
Expand Down
18 changes: 12 additions & 6 deletions src/modules/simulator/simulator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,11 @@ class Simulator : public ModuleParams
sample(float a, float b, float c) : x(a), y(b), z(c) {}
};

enum class InternetProtocol {
TCP,
UDP
};

static int start(int argc, char *argv[]);

bool getRawAccelReport(uint8_t *buf, int len);
Expand All @@ -230,6 +235,9 @@ class Simulator : public ModuleParams

bool isInitialized() { return _initialized; }

void set_ip(InternetProtocol ip);
void set_port(unsigned port);

private:
Simulator() : ModuleParams(nullptr),
_accel(1),
Expand Down Expand Up @@ -336,6 +344,9 @@ class Simulator : public ModuleParams

int _param_sub;

unsigned _port = 14560;
InternetProtocol _ip = InternetProtocol::UDP;

bool _initialized;
double _realtime_factor; ///< How fast the simulation runs in comparison to real system time
hrt_abstime _last_sim_timestamp;
Expand Down Expand Up @@ -385,17 +396,12 @@ class Simulator : public ModuleParams

)

enum class InternetProtocol {
TCP,
UDP
};

void poll_topics();
void handle_message(mavlink_message_t *msg, bool publish);
void send_controls();
void send_heartbeat();
void request_hil_state_quaternion();
void pollForMAVLinkMessages(bool publish, InternetProtocol ip, int port);
void pollForMAVLinkMessages(bool publish);

void pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index);
void send_mavlink_message(const mavlink_message_t &aMsg);
Expand Down
25 changes: 16 additions & 9 deletions src/modules/simulator/simulator_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -511,7 +511,14 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)

bufLen = mavlink_msg_to_send_buffer(buf, &aMsg);

ssize_t len = sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
ssize_t len;

if (_ip == InternetProtocol::UDP) {
len = ::sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));

} else {
len = ::send(_fd, buf, bufLen, 0);
}

if (len <= 0) {
PX4_WARN("Failed sending mavlink message: %s", strerror(errno));
Expand Down Expand Up @@ -643,7 +650,7 @@ void Simulator::initializeSensorData()
write_airspeed_data(&airspeed);
}

void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int port)
void Simulator::pollForMAVLinkMessages(bool publish)
{
#ifdef __PX4_DARWIN
pthread_setname_np("sim_rcv");
Expand All @@ -654,21 +661,21 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
struct sockaddr_in _myaddr {};
_myaddr.sin_family = AF_INET;
_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
_myaddr.sin_port = htons(port);
_myaddr.sin_port = htons(_port);

if (ip == InternetProtocol::UDP) {
if (_ip == InternetProtocol::UDP) {

if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
PX4_ERR("Creating UDP socket failed: %s", strerror(errno));
return;
}

if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
PX4_ERR("bind for UDP port %i failed (%i)", port, errno);
PX4_ERR("bind for UDP port %i failed (%i)", _port, errno);
return;
}

PX4_INFO("Waiting for simulator to connect on UDP port %u", port);
PX4_INFO("Waiting for simulator to connect on UDP port %u", _port);

while (true) {
// Once we receive something, we're most probably good and can carry on.
Expand All @@ -683,11 +690,11 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
}
}

PX4_INFO("Simulator connected on UDP port %u.", port);
PX4_INFO("Simulator connected on UDP port %u.", _port);

} else {

PX4_INFO("Waiting for simulator to connect on TCP port %u", port);
PX4_INFO("Waiting for simulator to connect on TCP port %u", _port);

while (true) {
if ((_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
Expand All @@ -714,7 +721,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
}
}

PX4_INFO("Simulator connected on TCP port %u.", port);
PX4_INFO("Simulator connected on TCP port %u.", _port);

}

Expand Down