diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index ee6b876ea0..19fea1a602 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -220,7 +220,21 @@ dataman start # only start the simulator if not in replay mode, as both control the lockstep time if ! replay tryapplyparams then - simulator start -c $simulator_tcp_port + # Check if PX4_SIM_HOSTNAME environment variable is empty + # If empty check if PX4_SIM_HOST_ADDR environment variable is empty + # If both are empty use localhost for simulator + if [ -z "${PX4_SIM_HOSTNAME}" ]; then + if [ -z "${PX4_SIM_HOST_ADDR}" ]; then + echo "PX4 SIM HOST: localhost" + simulator start -c $simulator_tcp_port + else + echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" + simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port + fi + else + echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" + simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port + fi fi load_mon start battery_simulator start diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 38843afa42..cd9bb940cd 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -69,6 +69,7 @@ int Simulator::start(int argc, char *argv[]) _instance = new Simulator(); if (_instance) { + if (argc == 5 && strcmp(argv[3], "-u") == 0) { _instance->set_ip(InternetProtocol::UDP); _instance->set_port(atoi(argv[4])); @@ -80,11 +81,21 @@ int Simulator::start(int argc, char *argv[]) } if (argc == 6 && strcmp(argv[3], "-t") == 0) { + PX4_INFO("Simulator using TCP on remote host %s port %s", argv[4], argv[5]); + PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]); _instance->set_ip(InternetProtocol::TCP); _instance->set_tcp_remote_ipaddr(argv[4]); _instance->set_port(atoi(argv[5])); } + if (argc == 6 && strcmp(argv[3], "-h") == 0) { + PX4_INFO("Simulator using TCP on remote host %s port %s", argv[4], argv[5]); + PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]); + _instance->set_ip(InternetProtocol::TCP); + _instance->set_hostname(argv[4]); + _instance->set_port(atoi(argv[5])); + } + _instance->run(); return 0; @@ -102,6 +113,7 @@ static void usage() PX4_INFO("Connect using UDP: simulator start -u udp_port"); PX4_INFO("Connect using TCP: simulator start -c tcp_port"); PX4_INFO("Connect to a remote server using TCP: simulator start -t ip_addr tcp_port"); + PX4_INFO("Connect to a remote server via hostname using TCP: simulator start -h hostname tcp_port"); } __BEGIN_DECLS diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index aeaa080b74..7de8eb6a1d 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -123,6 +123,7 @@ public: void set_ip(InternetProtocol ip) { _ip = ip; } void set_port(unsigned port) { _port = port; } + void set_hostname(std::string hostname) { _hostname = hostname; } void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; } #if defined(ENABLE_LOCKSTEP_SCHEDULER) @@ -206,6 +207,8 @@ private: InternetProtocol _ip{InternetProtocol::UDP}; + std::string _hostname{""}; + char *_tcp_remote_ipaddr{nullptr}; double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ff509a7539..ebb51e9e64 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -45,7 +45,9 @@ #include #include +#include #include +#include #include #include #include @@ -731,8 +733,19 @@ void Simulator::run() if (_tcp_remote_ipaddr != nullptr) { _myaddr.sin_addr.s_addr = inet_addr(_tcp_remote_ipaddr); + + } else if (!_hostname.empty()) { + /* resolve hostname */ + struct hostent *host; + host = gethostbyname(_hostname.c_str()); + memcpy(&_myaddr.sin_addr, host->h_addr_list[0], host->h_length); + + char ip[30]; + strcpy(ip, (char *)inet_ntoa((struct in_addr)_myaddr.sin_addr)); + PX4_INFO("Resolved host '%s' to address: %s", _hostname.c_str(), ip); } + if (_ip == InternetProtocol::UDP) { if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {