1.DrawPoint function must be in GameThread.

AirLib/AirLib.vcxproj

@@ -337,7 +337,7 @@
       <Optimization>Disabled</Optimization>
       <PreprocessorDefinitions>_SCL_SECURE_NO_WARNINGS;_CRT_SECURE_NO_WARNINGS;_DEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
       <AdditionalIncludeDirectories>include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include</AdditionalIncludeDirectories>
-      <TreatWarningAsError>true</TreatWarningAsError>
+      <TreatWarningAsError>false</TreatWarningAsError>
       <AdditionalOptions>/w34263 /w34266 %(AdditionalOptions)</AdditionalOptions>
       <DisableSpecificWarnings>4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings)</DisableSpecificWarnings>
       <LanguageStandard>stdcpp20</LanguageStandard>
@@ -424,7 +424,7 @@
       <IntrinsicFunctions>true</IntrinsicFunctions>
       <PreprocessorDefinitions>NDEBUG;_SCL_SECURE_NO_WARNINGS;_CRT_SECURE_NO_WARNINGS;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
       <AdditionalIncludeDirectories>include;deps\eigen3;deps\rpclib\include;$(ProjectDir)..\MavLinkCom\include</AdditionalIncludeDirectories>
-      <TreatWarningAsError>true</TreatWarningAsError>
+      <TreatWarningAsError>false</TreatWarningAsError>
       <AdditionalOptions>/w34263 /w34266 %(AdditionalOptions)</AdditionalOptions>
       <LanguageStandard>stdcpp20</LanguageStandard>
     </ClCompile>

AirLib/include/common/CancelToken.hpp

@@ -8,6 +8,7 @@
 #include <atomic>
 #include "common/Common.hpp"
 #include "common/common_utils/Utils.hpp"
+#include "common/common_utils/AdvanceSleep.h"
 
 namespace msr
 {
@@ -47,7 +48,7 @@ namespace airlib
             if (isCancelled()) {
                 return false;
             }
-
+#if SLEEP_MODE == 0
             TTimePoint start = ClockFactory::get()->nowNanos();
             static constexpr std::chrono::duration<double> MinSleepDuration(0);
 
@@ -56,7 +57,9 @@ namespace airlib
 
                 std::this_thread::sleep_for(MinSleepDuration);
             }
-
+#else
+            advanceSleep(secs * 1.0E3);
+#endif
             return !isCancelled();
         }
 

AirLib/include/common/ClockBase.hpp

@@ -7,7 +7,7 @@
 #include <thread>
 #include <chrono>
 #include "Common.hpp"
-
+#include "common/common_utils/AdvanceSleep.h"
 namespace msr
 {
 namespace airlib
@@ -73,12 +73,15 @@ namespace airlib
         {
             if (dt <= 0)
                 return;
-
+#if SLEEP_MODE == 0
             static constexpr std::chrono::duration<double> MinSleepDuration(0);
             TTimePoint start = nowNanos();
             //spin wait
             while (elapsedSince(start) < dt)
                 std::this_thread::sleep_for(MinSleepDuration);
+#else
+            advanceSleep(dt * 1e3);
+#endif
         }
 
         double getTrueScaleWrtWallClock()

AirLib/include/common/common_utils/AdvanceSleep.h

@@ -0,0 +1,47 @@
+#ifndef SLEEP_MODE
+#define SLEEP_MODE 1
+#endif
+#if SLEEP_MODE != 0
+#ifndef ADVANCE_SLEEP
+#define ADVANCE_SLEEP
+#include <chrono>
+#include <future>
+#include <thread>
+#include <queue>
+double nowMs();
+void advanceSleep(double ms);
+#if SLEEP_MODE == 1
+#ifndef __cpp_lib_atomic_is_always_lock_free
+#define __cpp_lib_atomic_is_always_lock_free 0
+#endif
+#include "atomic_queue/atomic_queue.h"
+namespace advance_sleep
+{
+class Event
+{
+public:
+    double wakeUpTimeMs;
+    std::promise<void> p;
+};
+struct CompareEvent
+{
+    bool operator()(advance_sleep::Event* a, advance_sleep::Event* b)
+    {
+        return a->wakeUpTimeMs > b->wakeUpTimeMs;
+    }
+};
+extern atomic_queue::AtomicQueueB<
+    advance_sleep::Event*,
+    std::allocator<Event*>,
+    (Event*)NULL,
+    false,
+    false,
+    false>
+    eventQueue;
+extern volatile bool busySpinQuit;
+extern std::priority_queue<advance_sleep::Event*, std::vector<advance_sleep::Event*>, advance_sleep::CompareEvent> pq;
+void busySpin();
+}
+#endif
+#endif
+#endif

AirLib/include/common/common_utils/ScheduledExecutor.hpp

@@ -11,7 +11,7 @@
 #include <system_error>
 #include <mutex>
 #include <cstdint>
-
+#include "AdvanceSleep.h"
 namespace common_utils
 {
 
@@ -158,7 +158,7 @@ class ScheduledExecutor
         probbaly does spin loop anyway.
 
         */
-
+#if SLEEP_MODE == 0
         if (delay_nanos >= 5000000LL) { //put thread to sleep
             std::this_thread::sleep_for(std::chrono::duration<double>(delay_nanos / 1.0E9));
         }
@@ -169,6 +169,9 @@ class ScheduledExecutor
                 //std::this_thread::sleep_for(std::chrono::duration<double>(0));
             }
         }
+#else
+        advanceSleep(delay_nanos / 1.0E6);
+#endif
     }
 
     void executorLoop()