Merge pull request #11436 from Scavanger/SITL-Improvements

[SITL] Maintenance and Improvements

Author: sensei-hacker

cmake/main.cmake

@@ -108,10 +108,10 @@ function(setup_firmware_target exe name)
 endfunction()
 
 function(exclude_from_all target)
-    set_property(TARGET ${target} PROPERTY
+    set_target_properties(${target} PROPERTIES
         TARGET_MESSAGES OFF
-        EXCLUDE_FROM_ALL 1
-        EXCLUDE_FROM_DEFAULT_BUILD 1)
+        EXCLUDE_FROM_ALL ON
+        EXCLUDE_FROM_DEFAULT_BUILD ON)
 endfunction()
 
 function(collect_targets)

cmake/sitl.cmake

@@ -19,8 +19,8 @@ main_sources(SITL_SRC
     target/SITL/sim/realFlight.h
     target/SITL/sim/simHelper.c
     target/SITL/sim/simHelper.h
-    target/SITL/sim/simple_soap_client.c
-    target/SITL/sim/simple_soap_client.h
+    target/SITL/sim/soap_client.c
+    target/SITL/sim/soap_client.h
     target/SITL/sim/xplane.c
     target/SITL/sim/xplane.h
 )
@@ -163,8 +163,8 @@ function (target_sitl name)
             WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
             COMMAND ${generator_cmd} clean
             COMMENT "Removing intermediate files for ${name}")
-        set_property(TARGET ${clean_target} PROPERTY
-            EXCLUDE_FROM_ALL 1
-            EXCLUDE_FROM_DEFAULT_BUILD 1)
+        set_target_properties(${clean_target} PROPERTIES
+            EXCLUDE_FROM_ALL ON
+            EXCLUDE_FROM_DEFAULT_BUILD ON)
     endif()
 endfunction()

src/main/config/config_streamer_file.c

@@ -47,7 +47,6 @@ bool configFileSetPath(char* path)
 void config_streamer_impl_unlock(void)
 {
     if (eepromFd != NULL) {
-        fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath);
         return;
     }
 
@@ -103,7 +102,6 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
 
     if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) {
         *((uint32_t*)c->address) = *buffer;
-        fprintf(stderr, "[EEPROM] Program word  %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
     } else {
         fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address);
     }

src/main/scheduler/scheduler.c

@@ -19,6 +19,10 @@
 #include <stdint.h>
 #include <string.h>
 
+#if defined(SITL_BUILD)
+#include <unistd.h>
+#endif
+
 #include "platform.h"
 
 #include "scheduler.h"
@@ -215,11 +219,22 @@ void FAST_CODE NOINLINE scheduler(void)
     uint16_t selectedTaskDynamicPriority = 0;
     bool forcedRealTimeTask = false;
 
+#if defined(SITL_BUILD)
+    // Track the earliest time at which the next task will become due so we can
+    // sleep until then instead of busy-waiting.  Cap at 1 ms so event-driven
+    // tasks (checkFunc) are still polled frequently enough.
+    timeUs_t sitlEarliestNextTaskAt = currentTimeUs + 1000;
+    bool sitlHasCheckFuncTask = false;
+#endif
+
     // Update task dynamic priorities
     uint16_t waitingTasks = 0;
     for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
         // Task has checkFunc - event driven
         if (task->checkFunc) {
+#if defined(SITL_BUILD)
+            sitlHasCheckFuncTask = true;
+#endif
             const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
 
             // Increase priority for event driven tasks
@@ -248,7 +263,19 @@ void FAST_CODE NOINLINE scheduler(void)
                 waitingTasks++;
                 forcedRealTimeTask = true;
             }
+#if defined(SITL_BUILD)
+            const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod;
+            if (taskNextAt < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = taskNextAt;
+            }            
+#endif
         } else {
+#if defined(SITL_BUILD)
+            const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod;
+            if (taskNextAt < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = taskNextAt;
+            }            
+#endif
             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
             // Task age is calculated from last execution
             task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod;
@@ -294,4 +321,27 @@ void FAST_CODE NOINLINE scheduler(void)
         selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
         selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
     }
+
+#if defined(SITL_BUILD)
+    {
+        // Avoid busy-waiting and burning 100% CPU in SITL.  After executing the
+        // current task (or finding nothing to do), sleep until just before the
+        // next task is due.  For event-driven tasks (checkFunc) we limit the
+        // sleep so the check function is still called frequently.
+        if (sitlHasCheckFuncTask) {
+            // Poll event-driven tasks at least every 500 µs
+            const timeUs_t eventCap = micros() + 500;
+            if (eventCap < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = eventCap;
+            }
+        }
+        const timeUs_t nowUs = micros();
+        if (sitlEarliestNextTaskAt > nowUs + 50) {
+            const timeDelta_t sleepUs = (timeDelta_t)(sitlEarliestNextTaskAt - nowUs) - 50;
+            if (sleepUs > 0) {
+                usleep((useconds_t)sleepUs);
+            }
+        }
+    }
+#endif
 }