start system daemon after constructors are run

Author: liam-teale

src/system/startup.cpp

@@ -22,7 +22,6 @@
 #include <cstdlib>
 #include <sys/unistd.h>
 
-
 extern "C" {
 // Initialization routines provided elsewhere
 void rtos_initialize();
@@ -50,6 +49,8 @@ static void pros_init() {
     vfs_initialize();
 }
 
+// forward-declare system daemon initialization function
+void initialize_system_daemon();
 // forward-declare main function
 int main();
 
@@ -73,6 +74,9 @@ void _start() {
     // call global constructors
     __libc_init_array();
 
+    // initialize the system daemon
+    initialize_system_daemon();
+
     // start main task
     // these pragmas are needed to silence the same warning on clang and gcc
     // normally you aren't supposed to reference the main function

src/system/system_daemon.cpp

@@ -11,24 +11,36 @@ namespace zest {
 
 // This task runs for the lifetime of the program.
 // It periodically calls vexTasksRun, which copies shared memory to/from vexOS.
-static pros::Task system_daemon([]() {
-    while (true) {
-        // lock all smart port mutexes
-        Brain::smart_port_mutex_lock_all();
-        // flush serial output
-        ser_output_flush();
-        // suspend all tasks
-        rtos_suspend_all();
-        // copy shared memory
-        vexTasksRun();
-        // resume all tasks
-        rtos_resume_all();
-        // unlock all smart port mutexes
-        zest::Brain::smart_port_mutex_unlock_all();
+// it's wrapped in an optional, because we only want to start it after all constructors are run.
+// This means it needs to be initialized by the _start function in startup.cpp
+static std::optional<pros::Task> system_daemon;
 
-        // delay to save resources
-        pros::delay(2);
+/**
+ * @brief Initialize the system daemon task
+ *
+ */
+void initialize_system_daemon() {
+    if (!system_daemon) {
+        system_daemon = pros::Task::create([]() {
+            while (true) {
+                // lock all smart port mutexes
+                Brain::smart_port_mutex_lock_all();
+                // flush serial output
+                ser_output_flush();
+                // suspend all tasks
+                rtos_suspend_all();
+                // copy shared memory
+                vexTasksRun();
+                // resume all tasks
+                rtos_resume_all();
+                // unlock all smart port mutexes
+                zest::Brain::smart_port_mutex_unlock_all();
+
+                // delay to save resources
+                pros::delay(2);
+            }
+        }, TASK_PRIORITY_MAX);
     }
-}, TASK_PRIORITY_MAX);
+}
 
 } // namespace zest