Add a bit more checking of message traffic to DroneShell/DroneServer.

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);

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;

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>();
+}
 bool RpcLibClient::armDisarm(bool arm)
 {
     return pimpl_->client.call("armDisarm", arm).as<bool>();

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); });

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;

DroneShell/src/main.cpp

@@ -1183,6 +1183,7 @@ int main(int argc, const char *argv[]) {
         }
     });
 
+
     SimpleShell<CommandContext> 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<double>(pause_time)); 
 		gps = command_context.client.getGpsLocation();
 	}