more fixes in DroneServer as well as MavLinkDroneController

modi0012 · Mar 17, 2017 · e3d1ca7 · e3d1ca7
1 parent d53029a
commit e3d1ca7
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 7 deletions.
diff --git a/AirLib/include/controllers/DroneControllerCancelable.hpp b/AirLib/include/controllers/DroneControllerCancelable.hpp
@@ -183,9 +183,15 @@ class DroneControllerCancelable : CancelableBase {
 
     std::string getServerDebugInfo()
     {
+        //for now this method just allows to see if server was started
         return std::to_string(Utils::getTimeSinceEpochMillis());
     }
 
+    void getStatusMessages(std::vector<std::string>& messages)
+    {
+        controller_->getStatusMessages(messages);
+    }
+
 
     //request image
     void setImageTypeForCamera(int camera_id, DroneControllerBase::ImageType type)

diff --git a/AirLib/src/controllers/MavLinkDroneController.cpp b/AirLib/src/controllers/MavLinkDroneController.cpp
@@ -325,7 +325,7 @@ struct MavLinkDroneController::impl {
 	{
 		is_armed_ = armed;
 		if (!armed) {
-			// motors are not armed!
+			//reset motor controls
 			for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
 				rotor_controls_[i] = 0;
 			}
@@ -401,6 +401,9 @@ struct MavLinkDroneController::impl {
 
     void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt)
     {
+        if (!is_simulation_mode_)
+            throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
+
         mavlinkcom::MavLinkHilSensor hil_sensor;
         hil_sensor.time_usec = static_cast<uint64_t>(Utils::getTimeSinceEpochMillis() * 1000);
         hil_sensor.xacc = acceleration.x();
@@ -436,6 +439,9 @@ struct MavLinkDroneController::impl {
     void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog,
         float eph, float epv, int fix_type, unsigned int satellites_visible)
     {
+        if (!is_simulation_mode_)
+            throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
+
         mavlinkcom::MavLinkHilGps hil_gps;
         hil_gps.time_usec = static_cast<uint64_t>(Utils::getTimeSinceEpochMillis() * 1000);
         hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
@@ -465,6 +471,9 @@ struct MavLinkDroneController::impl {
 
     real_T getVertexControlSignal(unsigned int rotor_index)
     {
+        if (!is_simulation_mode_)
+            throw std::logic_error("Attempt to read simulated motor controls while not in simulation mode");
+
         std::lock_guard<std::mutex> guard(hil_controls_mutex_);
         return rotor_controls_[rotor_index];
     }
@@ -481,7 +490,6 @@ struct MavLinkDroneController::impl {
         current_state = mavlinkcom::VehicleState();
         target_height_ = 0;
         is_offboard_mode_ = false;
-        is_simulation_mode_ = false;
         thrust_controller_ = PidController();
         Utils::setValue(rotor_controls_, 0.0f);
         was_reset_ = false;
@@ -568,8 +576,8 @@ struct MavLinkDroneController::impl {
 
     void start()
     {
-        close();
-        resetState();
+        close(); //just in case if connections were open
+        resetState(); //reset all variables we might have changed during last session
 
         connect();
         connectToLogViewer();
@@ -648,6 +656,10 @@ struct MavLinkDroneController::impl {
 
     void setHILMode()
     {
+        if (!is_simulation_mode_)
+            throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
+
+
         if (mav_vehicle_ == nullptr) {
             return;
         }

diff --git a/AirLib/src/rpc/RpcLibServer.cpp b/AirLib/src/rpc/RpcLibServer.cpp
@@ -113,7 +113,7 @@ void RpcLibServer::start(bool block)
     if (block)
         pimpl_->server.run();
     else
-        pimpl_->server.async_run(2);   //2 threads
+        pimpl_->server.async_run(4);   //4 threads
 }
 
 void RpcLibServer::stop()

diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp
@@ -77,7 +77,7 @@ int main(int argc, const char* argv[])
     }
 
     MavLinkDroneController mav_drone;
-    mav_drone.initialize(connection_info, nullptr, is_simulation);   //TODO: need to review how is_simulation flag might affect here
+    mav_drone.initialize(connection_info, nullptr, is_simulation);
     mav_drone.start();
 
     DroneControllerCancelable server_wrapper(&mav_drone);
@@ -87,7 +87,25 @@ int main(int argc, const char* argv[])
     server_wrapper.setImageForCamera(3, DroneControllerBase::ImageType::Depth, v);
     server_wrapper.setImageForCamera(4, DroneControllerBase::ImageType::Scene, std::vector<msr::airlib::uint8_t>{6, 5, 4, 3, 2});
 
+    //start server in async mode
+    server.start(false);
+
     std::cout << "Server connected to MavLink endpoint at " << connection_info.local_host_ip << ":" << connection_info.ip_port << std::endl;
-    server.start(true);
+    std::cout << "Hit Ctrl+C to terminate." << std::endl;
+
+    std::vector<std::string> messages;
+    while (true) {
+        //check messages
+        server_wrapper.getStatusMessages(messages);
+        if (messages.size() > 1) {
+            for (const auto& message : messages) {
+                std::cout << message << std::endl;
+            }
+        }
+
+        constexpr static std::chrono::milliseconds MessageCheckDurationMillis(100);
+        std::this_thread::sleep_for(MessageCheckDurationMillis);
+    }
+
     return 0;
 }