From 92e5d2d50a9416d9c52fdd4a0e3581b697ae3f09 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Fri, 28 Apr 2017 00:22:27 +0000 Subject: [PATCH] Add a bit more checking of message traffic to DroneShell/DroneServer. --- AirLib/include/rpc/RpcLibClient.hpp | 1 + AirLib/src/controllers/MavLinkDroneController.cpp | 14 +++++++++++++- AirLib/src/rpc/RpcLibClient.cpp | 4 ++++ AirLib/src/rpc/RpcLibServer.cpp | 1 + DroneServer/main.cpp | 4 +++- DroneShell/src/main.cpp | 9 +++++++-- 6 files changed, 29 insertions(+), 4 deletions(-) diff --git a/AirLib/include/rpc/RpcLibClient.hpp b/AirLib/include/rpc/RpcLibClient.hpp index ee4206166f..d959bd0ca7 100644 --- a/AirLib/include/rpc/RpcLibClient.hpp +++ b/AirLib/include/rpc/RpcLibClient.hpp @@ -16,6 +16,7 @@ namespace msr { namespace airlib { class RpcLibClient { public: RpcLibClient(const string& ip_address = "127.0.0.1", uint16_t port = 41451); + bool ping(); bool armDisarm(bool arm); void setOffboardMode(bool is_set); void setSimulationMode(bool is_set); diff --git a/AirLib/src/controllers/MavLinkDroneController.cpp b/AirLib/src/controllers/MavLinkDroneController.cpp index 7873321c9d..1ad3e7652a 100644 --- a/AirLib/src/controllers/MavLinkDroneController.cpp +++ b/AirLib/src/controllers/MavLinkDroneController.cpp @@ -102,6 +102,7 @@ struct MavLinkDroneController::impl { bool is_offboard_mode_; bool is_simulation_mode_; PidController thrust_controller_; + int hil_message_check_; int sitl_message_check_; void initialize(const ConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation) @@ -257,6 +258,8 @@ struct MavLinkDroneController::impl { void connect() { + hil_message_check_ = 0; + sitl_message_check_ = 0; createMavConnection(connection_info_); initializeMavSubscriptions(); } @@ -294,7 +297,6 @@ struct MavLinkDroneController::impl { if (connection_info_.sitl_ip_address != "" && connection_info_.sitl_ip_port != 0 && connection_info_.sitl_ip_port != port) { // bugbug: the PX4 SITL mode app cannot receive commands to control the drone over the same mavlink connection // as the HIL_SENSOR messages, we must establish a separate mavlink channel for that so that DroneShell works. - sitl_message_check_ = 0; auto sitlconnection = MavLinkConnection::connectRemoteUdp("sitl", connection_info_.local_host_ip, connection_info_.sitl_ip_address, connection_info_.sitl_ip_port); mav_vehicle_->connect(sitlconnection); } @@ -522,6 +524,7 @@ struct MavLinkDroneController::impl { Utils::setValue(rotor_controls_, 0.0f); was_reset_ = false; debug_pose_ = Pose::nanPose(); + hil_message_check_ = 0; sitl_message_check_ = 0; } @@ -976,12 +979,21 @@ struct MavLinkDroneController::impl { } MavLinkTelemetry data; connection_->getTelemetry(data); + if (data.messagesReceived == 0) { + hil_message_check_++; + if (hil_message_check_ == 100) { + addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again"); + } + } else { + hil_message_check_ = 0; + } // listen to the other mavlink connection also auto mavcon = mav_vehicle_->getConnection(); if (mavcon != connection_) { MavLinkTelemetry sitl; mavcon->getTelemetry(sitl); + data.handlerMicroseconds += sitl.handlerMicroseconds; data.messagesHandled += sitl.messagesHandled; data.messagesReceived += sitl.messagesReceived; diff --git a/AirLib/src/rpc/RpcLibClient.cpp b/AirLib/src/rpc/RpcLibClient.cpp index 868c815270..62ecba3440 100644 --- a/AirLib/src/rpc/RpcLibClient.cpp +++ b/AirLib/src/rpc/RpcLibClient.cpp @@ -48,6 +48,10 @@ RpcLibClient::RpcLibClient(const string& ip_address, uint16_t port) RpcLibClient::~RpcLibClient() {} +bool RpcLibClient::ping() +{ + return pimpl_->client.call("ping").as(); +} bool RpcLibClient::armDisarm(bool arm) { return pimpl_->client.call("armDisarm", arm).as(); diff --git a/AirLib/src/rpc/RpcLibServer.cpp b/AirLib/src/rpc/RpcLibServer.cpp index 32e7d993f5..a1d73f5a9e 100644 --- a/AirLib/src/rpc/RpcLibServer.cpp +++ b/AirLib/src/rpc/RpcLibServer.cpp @@ -41,6 +41,7 @@ RpcLibServer::RpcLibServer(DroneControllerCancelable* drone, string server_addre : drone_(drone) { pimpl_.reset(new impl(server_address, port)); + pimpl_->server.bind("ping", [&]() -> bool { return true; }); pimpl_->server.bind("armDisarm", [&](bool arm) -> bool { return drone_->armDisarm(arm); }); pimpl_->server.bind("setOffboardMode", [&](bool is_set) -> void { drone_->setOffboardMode(is_set); }); pimpl_->server.bind("setSimulationMode", [&](bool is_set) -> void { drone_->setSimulationMode(is_set); }); diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index 137ed65e2d..210b779bad 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -97,10 +97,12 @@ int main(int argc, const char* argv[]) for (const auto& message : messages) { std::cout << message << std::endl; } - } + } constexpr static std::chrono::milliseconds MessageCheckDurationMillis(100); std::this_thread::sleep_for(MessageCheckDurationMillis); + + mav_drone.reportTelemetry(100); } return 0; diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 97c1ac4302..f0ddf7ed1d 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -1183,6 +1183,7 @@ int main(int argc, const char *argv[]) { } }); + SimpleShell shell("==||=> "); shell.showMessage(R"( @@ -1191,13 +1192,17 @@ int main(int argc, const char *argv[]) { Microsoft Research (c) 2016. )"); + // make sure we can talk to the DroneServer + std::cout << "Contacting DroneServer..." << std::flush; + command_context.client.ping(); + std::cout << "DroneServer is responding." << std::endl; - std::cout << "Waiting for drone to report a valid GPS location..."; + std::cout << "Waiting for drone to report a valid GPS location..." << std::flush; const float pause_time = 1; auto gps = command_context.client.getGpsLocation(); while (gps.latitude == 0 && gps.longitude == 0 && gps.altitude == 0) { - std::cout << "."; + std::cout << "." << std::flush; std::this_thread::sleep_for(std::chrono::duration(pause_time)); gps = command_context.client.getGpsLocation(); }