add MavLinkLog interface that can be overridden.

add Log class that can redirect Utils::logMessage output. Fix various bugs.
kjhatis · Apr 7, 2017 · 3667cc5 · 3667cc5
1 parent bd8429d
commit 3667cc5
Show file tree

Hide file tree

Showing 21 changed files with 299 additions and 91 deletions.
diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj
@@ -26,6 +26,7 @@
     <ClInclude Include="include\common\common_utils\ExceptionUtils.hpp" />
     <ClInclude Include="include\common\common_utils\FileSystem.hpp" />
     <ClInclude Include="include\common\common_utils\json.hpp" />
+    <ClInclude Include="include\common\common_utils\Log.hpp" />
     <ClInclude Include="include\common\common_utils\MedianFilter.hpp" />
     <ClInclude Include="include\common\common_utils\OnlineStats.hpp" />
     <ClInclude Include="include\common\common_utils\optional.hpp" />
@@ -124,6 +125,7 @@
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\controllers\DroneControllerBase.cpp" />
+    <ClCompile Include="src\controllers\Log.cpp" />
     <ClCompile Include="src\controllers\MavLinkDroneController.cpp" />
     <ClCompile Include="src\safety\ObstacleMap.cpp" />
     <ClCompile Include="src\rpc\RpcLibClient.cpp" />

diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters
@@ -321,6 +321,9 @@
     <ClInclude Include="include\vehicles\configs\Px4HILQuadX.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\common\common_utils\Log.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\controllers\DroneControllerBase.cpp">
@@ -347,5 +350,8 @@
     <ClCompile Include="src\controllers\FileSystem.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="src\controllers\Log.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>
diff --git a/AirLib/include/common/common_utils/Log.hpp b/AirLib/include/common/common_utils/Log.hpp
@@ -0,0 +1,42 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef common_utils_Log_hpp
+#define common_utils_Log_hpp
+
+#include <cstdarg>
+#include <stdio.h>
+
+namespace common_utils {
+
+    // This simple logging interface can be used to redirect debug printf statements to your own app's environment.
+    class Log
+    {
+        static Log* log_;
+    public:
+
+        static Log* getLog() {
+            return log_;
+        }
+
+        static void setLog(Log* log) {
+            log_ = log;
+        }
+
+        virtual void logMessage(const char* message) {
+            printf(message);
+            printf("\n");
+            fflush(stdout);
+        }
+
+        virtual void logError(const char* message) {
+            fprintf(stderr, message);
+            printf("\n");
+            fflush(stderr);
+        }
+
+    };
+}
+
+#endif
+
diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp
@@ -21,7 +21,7 @@
 #include <iostream>
 #include <limits>
 #include <queue>
-
+#include "Log.hpp"
 #include "type_utils.hpp"
 
 #ifndef _WIN32
@@ -121,25 +121,38 @@ class Utils {
         return static_cast<float>(radians * 180.0f / M_PI);
     }
 
-    static void logMessage(const char* message, ...) {
+    static void logMessage(const char* format, ...) {
         va_list args;
-        va_start(args, message);
-
-        vprintf(message, args);
-        printf("\n");
-        fflush (stdout);
-
+        va_start(args, format);
+
+        auto size = _vscprintf(format, args) + 1U;
+        std::unique_ptr<char[]> buf(new char[size]);
+
+#ifndef _MSC_VER
+        vsnprintf(buf.get(), size, format, args);
+#else
+        vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
+#endif
         va_end(args);
+
+        Log::getLog()->logMessage(buf.get());
     }
-    static void logError(const char* message, ...) {
+
+    static void logError(const char* format, ...) {
         va_list args;
-        va_start(args, message);
-
-        vfprintf(stderr, message, args);
-        fprintf(stderr, "\n");
-        fflush (stderr);
-
+        va_start(args, format);
+
+        auto size = _vscprintf(format, args) + 1U;
+        std::unique_ptr<char[]> buf(new char[size]);
+
+#ifndef _MSC_VER
+        vsnprintf(buf.get(), size, format, args);
+#else
+        vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
+#endif
         va_end(args);
+
+        Log::getLog()->logError(buf.get());
     }
 
     template <typename T>

diff --git a/AirLib/include/controllers/MavLinkDroneController.hpp b/AirLib/include/controllers/MavLinkDroneController.hpp
@@ -48,6 +48,7 @@ class MavLinkDroneController : public DroneControllerBase
 
         // The log viewer can be on a different machine, so you can configure it's ip address and port here.
         int logviewer_ip_port = 14388;
+        int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle.
         std::string logviewer_ip_address = "127.0.0.1";
 
         // The QGroundControl app can be on a different machine, so you can configure it's ip address and port here.

diff --git a/AirLib/include/vehicles/configs/PX4ConfigCreator.hpp b/AirLib/include/vehicles/configs/PX4ConfigCreator.hpp
@@ -58,6 +58,7 @@ class PX4ConfigCreator {
 
             connection_info.logviewer_ip_address = child.getString("LogViewerHostIp", connection_info.logviewer_ip_address);
             connection_info.logviewer_ip_port = child.getInt("LogViewerPort", connection_info.logviewer_ip_port);
+            connection_info.logviewer_ip_sport = child.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport);
 
             connection_info.qgc_ip_address = child.getString("QgcHostIp", connection_info.qgc_ip_address);
             connection_info.qgc_ip_port = child.getInt("QgcPort", connection_info.qgc_ip_port);
@@ -90,6 +91,7 @@ class PX4ConfigCreator {
 
 			changed |= child.setString("LogViewerHostIp", connection_info.logviewer_ip_address);
 			changed |= child.setInt("LogViewerPort", connection_info.logviewer_ip_port);
+            changed |= child.setInt("LogViewerSendPort", connection_info.logviewer_ip_sport);
 
 			changed |= child.setString("QgcHostIp", connection_info.qgc_ip_address);
 			changed |= child.setInt("QgcPort", connection_info.qgc_ip_port);

diff --git a/AirLib/src/controllers/DroneControllerBase.cpp b/AirLib/src/controllers/DroneControllerBase.cpp
@@ -124,8 +124,10 @@ bool DroneControllerBase::moveOnPath(const vector<Vector3r>& path, float velocit
     path3d.push_back(getPosition());
 
     std::ofstream flog;
-    if (log_to_file)
+    if (log_to_file) {
         common_utils::FileSystem::createLogFile("MoveToPosition", flog);
+        flog << "seg_index\toffset\tx\ty\tz\tgoal_dist\tseg_index\toffset\tx\ty\tz\tlookahead\tlookahead_error\tseg_index\toffset\tx\ty\tz";
+    }
 
     Vector3r point;
     float path_length = 0;
@@ -154,6 +156,7 @@ bool DroneControllerBase::moveOnPath(const vector<Vector3r>& path, float velocit
     cur_path_loc.offset = 0;
     cur_path_loc.position = path3d[0];
 
+    float lookahead_error_increasing = 0;
     float lookahead_error = 0;
     Waiter waiter(getCommandPeriod());
 
@@ -223,10 +226,21 @@ bool DroneControllerBase::moveOnPath(const vector<Vector3r>& path, float velocit
                                                             //if adaptive lookahead is enabled the calculate lookahead error (see above fig)
             if (adaptive_lookahead) {
                 const Vector3r& actual_on_goal = goal_normalized * goal_dist;
-                lookahead_error = (actual_vect - actual_on_goal).norm() * adaptive_lookahead;
+                float error = (actual_vect - actual_on_goal).norm() * adaptive_lookahead;
+                if (error > lookahead_error) {
+                    lookahead_error_increasing++;
+                    if (lookahead_error_increasing > 100) {
+                        throw std::runtime_error("lookahead error is continually increasing so we do not have safe control, aborting moveOnPath operation");
+                    }
+                }
+                else { 
+                    lookahead_error_increasing = 0; 
+                }
+                lookahead_error = error;
             }
         }
         else {
+            lookahead_error_increasing = 0;
             goal_dist = 0;
             lookahead_error = 0; //this is not really required because we will exit
         }

diff --git a/AirLib/src/controllers/Log.cpp b/AirLib/src/controllers/Log.cpp
@@ -0,0 +1,15 @@
+// in header only mode, control library is not available
+#ifndef AIRLIB_HEADER_ONLY
+//if using Unreal Build system then include precompiled header file first
+#ifdef AIRLIB_PCH
+#include "AirSim.h"
+#endif
+
+#include "common/common_utils/Log.hpp"
+
+using namespace common_utils;
+
+// default implementation that can be overridden by the user via Log::setLog().
+Log* Log::log_ = new Log();
+
+#endif;