diff --git a/MavLinkCom/MavLinkTest/main.cpp b/MavLinkCom/MavLinkTest/main.cpp index eb4112867f..db8098e15b 100644 --- a/MavLinkCom/MavLinkTest/main.cpp +++ b/MavLinkCom/MavLinkTest/main.cpp @@ -5,6 +5,7 @@ #include "Utils.hpp" #include "FileSystem.hpp" +#include "MavLinkTcpServer.hpp" #include "MavLinkConnection.hpp" #include "MavLinkVehicle.hpp" #include "MavLinkMessages.hpp" @@ -101,21 +102,35 @@ std::string defaultLocalAddress{ "127.0.0.1" }; // The remote app is connected to Pixhawk, and is also "serving" UDP packets, this tells us what remote // connection to create to talke to that server. bool offboard = false; +bool udp = false; +bool tcp = false; PortAddress offboardEndPoint; -#define DEFAULT_OFFBOARD_PORT 14560 +#define DEFAULT_OFFBOARD_UDP_PORT 14560 +#define DEFAULT_OFFBOARD_TCP_PORT 4560 + +// SITL setup info +const uint8_t sim_sysid = 142; +const int sim_compid = 42; +#define DEFAULT_SITL_PORT 14580 +bool sitl = false; +PortAddress sitlEndPoint; + +// The local ethernet interface to use (default localhost). +PortAddress localEndPoint; // this is used if you want to connect MavLinkTest to the serial port of the Pixhawk directly bool serial = false; std::string comPort; int baudRate = 115200; -// server mode is when you want another app to connect to Pixhawk and publish data back to this process. +// server mode on UDP is when you want another app to connect to Pixhawk and publish data back to this process. // this server will be listening for UDP packets, this is mutually exclusive with 'offboard' as this // server will become the primary "droneConnection". For example, jMAVSim can talk to this server -// using their the -qgc option. +// using their the -qgc option. Server mode on TCP means mavlinktest will do an "accept" socket which is +// what PX4 is waiting for when it is running in TCP mode. Here the serverEndPoint is different from the +// offboardEndPoint. The serverEndPoint specifies which local address to use in case your computer has +// multiple network interfaces. bool server = false; -PortAddress serverEndPoint; -#define DEFAULT_SERVER_PORT 14550 bool connectLogViewer = false; PortAddress logViewerEndPoint; @@ -149,7 +164,9 @@ std::stringstream initScript; std::shared_ptr droneConnection; std::shared_ptr logConnection; std::shared_ptr mavLinkVehicle; +std::shared_ptr hilNode; // for simulation. +std::vector usedPorts; void ConvertLogFileToJson(std::string logFile) { @@ -737,9 +754,12 @@ void PrintUsage() { printf("Usage: PX4 options\n"); printf("Connects to PX4 either over udp or serial COM port\n"); printf("Options: \n"); - printf(" -udp:ipaddr[:port]] - connect to remote drone via this udp address, the remote app is the UDP server (default port 14550)\n"); - printf(" -server:ipaddr[:port] - start mavlink server on this local port (so jMAVSim can connect to it using -udp)\n"); + printf(" -udp[:ipaddr[:port]] - connect to remote drone at this udp address (default port localhost:14550)\n"); + printf(" -tcp[:ipaddr[:port]] - connect to remote drone at this tcp address (default port localhost:4560)\n"); + printf(" -local[:ipaddr] - connect to remote drone via this local address (default localhost)\n"); + printf(" -server - start mavlink server on specified -udp or -tcp local address\n"); printf(" -serial[:comPortName][, baudrate]] - open serial port\n"); + printf(" -sitl[:ipaddr[:port]] - when talking to a SITL PX4 provide socket port number here for control channel (default localhost:14580)\n"); printf(" -logviewer:ipaddr[:port] - for sending mavlink information to Log Viewer\n"); printf(" -proxy:ipaddr[:port] - send all mavlink messages to and from remote node\n"); printf(" -local:ipaddr - specify local NIC address (default 127.0.0.1)\n"); @@ -776,8 +796,9 @@ bool ParseCommandLine(int argc, const char* argv[]) std::string lower = Utils::toLower(parts[0]); if (lower == "udp") { + udp = true; offboard = true; - offboardEndPoint.port = DEFAULT_OFFBOARD_PORT; + offboardEndPoint.port = DEFAULT_OFFBOARD_UDP_PORT; if (parts.size() > 1) { offboardEndPoint.addr = parts[1]; @@ -787,16 +808,46 @@ bool ParseCommandLine(int argc, const char* argv[]) } } } + else if (lower == "tcp") + { + tcp = true; + offboard = true; + offboardEndPoint.port = DEFAULT_OFFBOARD_TCP_PORT; + if (parts.size() > 1) + { + offboardEndPoint.addr = parts[1]; + if (parts.size() > 2) + { + offboardEndPoint.port = atoi(parts[2].c_str()); + } + } + } + else if (lower == "local") + { + localEndPoint.port = 0; // any port. + if (parts.size() > 1) + { + localEndPoint.addr = parts[1]; + if (parts.size() > 2) + { + localEndPoint.port = atoi(parts[2].c_str()); + } + } + } else if (lower == "server") { server = true; - serverEndPoint.port = DEFAULT_SERVER_PORT; + } + else if (lower == "sitl") + { + sitl = true; + sitlEndPoint.port = 0; // any port. if (parts.size() > 1) { - serverEndPoint.addr = parts[1]; + sitlEndPoint.addr = parts[1]; if (parts.size() > 2) { - serverEndPoint.port = atoi(parts[2].c_str()); + sitlEndPoint.port = atoi(parts[2].c_str()); } } } @@ -1001,7 +1052,7 @@ std::shared_ptr connectProxy(const PortAddress& endPoint, std { printf("Connecting to UDP Proxy address %s:%d\n", endPoint.addr.c_str(), endPoint.port); - std::shared_ptr proxyConnection = MavLinkConnection::connectRemoteUdp(name, defaultLocalAddress, endPoint.addr, endPoint.port); + std::shared_ptr proxyConnection = MavLinkConnection::connectRemoteUdp(name, localEndPoint.addr, endPoint.addr, endPoint.port); // forward all PX4 messages to the remote proxy and all messages from remote proxy to PX4. droneConnection->join(proxyConnection); @@ -1039,18 +1090,47 @@ std::shared_ptr connectSerial() std::shared_ptr connectOffboard() { - if (offboardEndPoint.addr == "") { - offboardEndPoint.addr = defaultLocalAddress; - } printf("Connecting to offboard drone at address %s:%d\n", offboardEndPoint.addr.c_str(), offboardEndPoint.port); return MavLinkConnection::connectRemoteUdp("drone", defaultLocalAddress, offboardEndPoint.addr, offboardEndPoint.port); } -std::shared_ptr connectServer(const PortAddress& endPoint, std::string name) +std::shared_ptr connectServer(std::string name) { - printf("Connecting to UDP Server address %s:%d\n", endPoint.addr.c_str(), endPoint.port); + if (localEndPoint.addr == "") { + localEndPoint.addr = defaultLocalAddress; + } + if (localEndPoint.port == 0) { + localEndPoint.port = offboardEndPoint.port; + } + + std::shared_ptr serverConnection; + + if (tcp) + { + if (localEndPoint.port == 0) { + localEndPoint.port = DEFAULT_OFFBOARD_TCP_PORT; + } + printf("Waiting for drone to connect to server at address %s:%d...\n", localEndPoint.addr.c_str(), localEndPoint.port); + MavLinkTcpServer server(localEndPoint.addr, localEndPoint.port); + serverConnection = server.acceptTcp(name); + } + else + { + if (localEndPoint.port == 0) { + localEndPoint.port = DEFAULT_OFFBOARD_UDP_PORT; + } + printf("Starting UDP Server on address %s:%d\n", localEndPoint.addr.c_str(), localEndPoint.port); + serverConnection = MavLinkConnection::connectLocalUdp(name, localEndPoint.addr, localEndPoint.port); + } - std::shared_ptr serverConnection = MavLinkConnection::connectLocalUdp(name, endPoint.addr, endPoint.port); + if (droneConnection != nullptr) { + // then we have a serial connection as the primary droneConnection, so publish all PX4 messages out to the server + droneConnection->join(serverConnection); + } + else { + // no local serial connection, so this is the primary droneConnection. + droneConnection = serverConnection; + } return serverConnection; } @@ -1090,22 +1170,23 @@ bool connect() printf("Cannot connect to local -serial pixhawk and -udp drone at the same time \n"); return false; } - if (!offboard && !serial && !server) { - printf("Must specify one of -serial, -udp or -server otherwise we don't have a drone connection\n"); + if (!offboard && !serial) { + printf("Must specify one of -serial, -udp or -tcp otherwise we don't have a drone connection\n"); return false; } - if (offboard && server) - { - printf("Cannot have offboard and server, must pick one of this as the primary drone connection\n"); - return false; + + if (offboardEndPoint.addr == "") { + offboardEndPoint.addr = defaultLocalAddress; } - std::vector usedPorts; + if (localEndPoint.addr == "") { + localEndPoint.addr = defaultLocalAddress; + } if (serial) { droneConnection = connectSerial(); } - else if (offboard) + else if (!server) { droneConnection = connectOffboard(); usedPorts.push_back(offboardEndPoint); @@ -1113,23 +1194,34 @@ bool connect() if (server) { - if (serverEndPoint.addr == "") { - serverEndPoint.addr = defaultLocalAddress; - } + std::shared_ptr serverConnection = connectServer("server"); + usedPorts.push_back(localEndPoint); + } - std::shared_ptr serverConnection = connectServer(serverEndPoint, "server"); - usedPorts.push_back(serverEndPoint); + return true; +} - if (droneConnection != nullptr) { - // then we have a serial connection as the primary droneConnection, so publish all PX4 messages out to the server - droneConnection->join(serverConnection); +void connectSitl(std::shared_ptr mavLinkVehicle) { + + if (sitlEndPoint.port != 0) { + sitlEndPoint.port = DEFAULT_SITL_PORT; + } + + // need a retry loop here because we don't know how quickly px4 will start accepting these connections... + for (int retries = 60; retries >= 0; retries--) { + try { + auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs", + localEndPoint.addr, sitlEndPoint.addr, sitlEndPoint.port); + mavLinkVehicle->connect(gcsConnection); } - else { - // no local serial connection, so this is the primary droneConnection. - droneConnection = serverConnection; + catch (std::exception&) { + std::this_thread::sleep_for(std::chrono::seconds(1)); } } +} +bool setupDrone() +{ if (droneConnection == nullptr) { // failed to connect @@ -1145,12 +1237,26 @@ bool connect() if (outLogFile != nullptr) { droneConnection->startLoggingSendMessage(outLogFile); } - mavLinkVehicle->connect(droneConnection); - if (!server) { - // local connection, then we own sending the heartbeat. - mavLinkVehicle->startHeartbeat(); + if (sitl) { + // then we need 2 mavlink channels, one for sending/receiving HIL_* messages and the other + // for controlling the drone. + hilNode = std::make_shared(sim_sysid, sim_compid); + hilNode->connect(droneConnection); + hilNode->startHeartbeat(); + + printf("TODO: for SITL to work, mavlinktest needs to simulate HIL_SENSOR messages...\n"); + + // this is the control channel. + mavLinkVehicle = std::make_shared(LocalSystemId, LocalComponentId); + connectSitl(mavLinkVehicle); } + else { + mavLinkVehicle = std::make_shared(LocalSystemId, LocalComponentId); + mavLinkVehicle->connect(droneConnection); + } + + mavLinkVehicle->startHeartbeat(); if (connectLogViewer) { @@ -1169,6 +1275,7 @@ bool connect() for (auto ptr = proxyEndPoints.begin(), end = proxyEndPoints.end(); ptr != end; ptr++) { + bool ok = true; PortAddress proxyEndPoint = *ptr; for (auto ep = usedPorts.begin(), endep = usedPorts.end(); ep != endep; ep++) { @@ -1176,14 +1283,15 @@ bool connect() if (used.addr == proxyEndPoint.addr && used.port == proxyEndPoint.port) { printf("Cannot proxy to address that is already used: %s:%d\n", used.addr.c_str(), used.port); - return false; + ok = false; } } - usedPorts.push_back(proxyEndPoint); - connectProxy(proxyEndPoint, "proxy"); + if (ok) { + usedPorts.push_back(proxyEndPoint); + connectProxy(proxyEndPoint, "proxy"); + } } - return true; } @@ -1216,7 +1324,6 @@ void handleStatus(const MavLinkStatustext& statustext) { int console(std::stringstream& script) { std::string line; - mavLinkVehicle = std::make_shared(LocalSystemId, LocalComponentId); std::shared_ptr logViewer = nullptr; Command* currentCommand = nullptr; OrbitCommand* orbit = new OrbitCommand(); @@ -1251,10 +1358,6 @@ int console(std::stringstream& script) { cmdTable.push_back(new BatteryCommand()); cmdTable.push_back(new WaitForAltitudeCommand()); - if (!connect()) { - return 1; - } - droneConnection->subscribe([=](std::shared_ptr connection, const MavLinkMessage& message) { MavLinkStatustext statustext; @@ -1287,7 +1390,7 @@ int console(std::stringstream& script) { default: break; } - }); + }); if (logConnection != nullptr) { logViewer = std::make_shared(LocalLogViewerSystemId, LocalComponentId); @@ -1354,12 +1457,12 @@ int console(std::stringstream& script) { std::getline(script, line); } else { + printf("mavlink> "); std::getline(std::cin, line); } line = mavlink_utils::Utils::trim(line, ' '); - if (line.length() > 0) { if (line.length() == 0) @@ -1404,7 +1507,7 @@ int console(std::stringstream& script) { auto str = std::string(Command::kCommandLogPrefix) + line; MavLinkStatustext st; strncpy(st.text, str.c_str(), 50); - MavLinkMessage m; + MavLinkMessage m; st.encode(m); droneConnection->prepareForSending(m); std::lock_guard lock(logLock); @@ -1435,11 +1538,8 @@ int console(std::stringstream& script) { { printf("Unknown command. Type '?' to get list of commands\n"); } - } - } - } stopTelemetry(); @@ -1491,14 +1591,21 @@ int main(int argc, const char* argv[]) } } + try { + if (unitTest) { + UnitTests test; + test.RunAll(comPort, baudRate); + return 0; + } - if (unitTest) { - UnitTests test; - test.RunAll(comPort, baudRate); - return 0; - } + if (!connect()) { + return 1; + } + + if (!setupDrone()) { + return 1; + } - try { return console(initScript); } catch (const std::exception& e)