diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 51f897af8b..638596329f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -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(); @@ -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}"); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 8289e7c4ec..941b4afa1e 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -212,6 +212,11 @@ public: 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); @@ -230,6 +235,9 @@ public: bool isInitialized() { return _initialized; } + void set_ip(InternetProtocol ip); + void set_port(unsigned port); + private: Simulator() : ModuleParams(nullptr), _accel(1), @@ -336,6 +344,9 @@ private: 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; @@ -385,17 +396,12 @@ private: ) - 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); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c172611757..79d9a8a67f 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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)); @@ -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"); @@ -654,9 +661,9 @@ 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)); @@ -664,11 +671,11 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po } 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. @@ -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) { @@ -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); }