REVONANO - Prevent modules to start before init: move most code to pios_initcall.h. System and system-Like modules

flight/modules/OPLink/oplinkmod.c

@@ -66,6 +66,7 @@
 static xTaskHandle systemTaskHandle;
 static bool stackOverflow;
 static bool mallocFailed;
+volatile int initTaskDone = 0;
 
 // Private functions
 static void systemTask(void *parameters);
@@ -113,6 +114,9 @@ static void systemTask(__attribute__((unused)) void *parameters)
     uint16_t prev_rx_count = 0;
     bool first_time = true;
 
+    while (!initTaskDone) {
+        vTaskDelay(10);
+    }
     /* create all modules thread */
     MODULE_TASKCREATE_ALL;
 

flight/modules/System/systemmod.c

@@ -102,6 +102,8 @@ static bool mallocFailed;
 static HwSettingsData bootHwSettings;
 static FrameType_t bootFrameType;
 
+volatile int initTaskDone = 0;
+
 // Private functions
 static void objectUpdatedCb(UAVObjEvent *ev);
 static void checkSettingsUpdatedCb(UAVObjEvent *ev);
@@ -171,8 +173,6 @@ int32_t SystemModInitialize(void)
     return 0;
 }
 
-extern volatile int initTaskDone;
-
 MODULE_INITCALL(SystemModInitialize, 0);
 /**
  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS

flight/modules/gpsp/gpsdsysmod.c

@@ -60,6 +60,7 @@ extern uint32_t pios_com_main_id;
 // Private variables
 static xTaskHandle systemTaskHandle;
 static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
+volatile int initTaskDone = 0;
 
 
 static bool mallocFailed;
@@ -106,6 +107,9 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
 #ifdef PIOS_INCLUDE_WDG
     PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
 #endif
+    while (!initTaskDone) {
+        vTaskDelay(10);
+    }
     /* create all modules thread */
     MODULE_TASKCREATE_ALL;
 

flight/pios/inc/pios_initcall.h

@@ -50,7 +50,7 @@ typedef struct {
 
 /* Init module section */
 extern initmodule_t __module_initcall_start[], __module_initcall_end[];
-
+extern volatile int initTaskDone;
 #ifdef USE_SIM_POSIX
 
 extern void InitModules();
@@ -69,9 +69,10 @@ extern void StartModules();
         /* Initialize modules */ \
         InitModules(); \
         /* Initialize the system thread */ \
-        SystemModInitialize(); }
+        SystemModInitialize(); \
+        initTaskDone = 1; }
 
-#else
+#else // ifdef USE_SIM_POSIX
 
 /* initcalls are now grouped by functionality into separate
  * subsections. Ordering inside the subsections is determined
@@ -96,6 +97,7 @@ extern void StartModules();
           if (fn->fn_minit) { \
               (fn->fn_minit)(); } \
       } \
+      initTaskDone = 1; \
     }
 
 #define MODULE_TASKCREATE_ALL \

flight/targets/boards/revonano/firmware/revolution.c

@@ -115,8 +115,6 @@ int main()
     return 0;
 }
 
-
-volatile int initTaskDone = 0;
 /**
  * Initialisation task.
  *
@@ -130,8 +128,6 @@ void initTask(__attribute__((unused)) void *parameters)
     /* Initialize modules */
     MODULE_INITIALISE_ALL;
 
-    initTaskDone = 1;
-
     /* terminate this task */
     vTaskDelete(NULL);
 }