From f5a1235038bb38d0af83601ec77c1bebc7f90739 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Mon, 23 Sep 2019 01:42:22 -0400
Subject: [PATCH] Construct the object tree outside of the comms task

---
 Firmware/Board/v3/Src/freertos.c         |  2 +
 Firmware/MotorControl/main.cpp           | 51 +++++++++++++-----------
 Firmware/communication/communication.cpp | 12 +++---
 Firmware/communication/communication.h   |  1 +
 4 files changed, 37 insertions(+), 29 deletions(-)

diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c
index ac6c6de9..73a56574 100644
--- a/Firmware/Board/v3/Src/freertos.c
+++ b/Firmware/Board/v3/Src/freertos.c
@@ -61,6 +61,7 @@
 extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
 int odrive_main(void);
 int load_configuration(void);
+int construct_objects(void);
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -191,6 +192,7 @@ void MX_FREERTOS_Init(void) {
 
   // Load persistent configuration (or defaults)
   load_configuration();
+  construct_objects();
   /* USER CODE END RTOS_SEMAPHORES */
 
   /* USER CODE BEGIN RTOS_TIMERS */
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index d5acf252..9faee474 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -96,29 +96,8 @@ void enter_dfu_mode() {
     }
 }
 
-extern "C" {
-int odrive_main(void);
-void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
-    for (;;); // TODO: safe action
-}
-void vApplicationIdleHook(void) {
-    if (system_stats_.fully_booted) {
-        system_stats_.uptime = xTaskGetTickCount();
-        system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
-        system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
-        system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
-        system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
-        system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
-        system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
-        system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
-        system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
-    }
-}
-}
-
-int odrive_main(void) {
-
-#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+extern "C" int construct_objects(){
+    #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
     if (board_config.enable_i2c_instead_of_can) {
         // Set up the direction GPIO as input
         GPIO_InitTypeDef GPIO_InitStruct;
@@ -172,7 +151,31 @@ int odrive_main(void) {
         axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
                 *encoder, *sensorless_estimator, *controller, *motor, *trap);
     }
-    
+    initTree();
+    return 0;
+}
+
+extern "C" {
+int odrive_main(void);
+void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
+    for (;;); // TODO: safe action
+}
+void vApplicationIdleHook(void) {
+    if (system_stats_.fully_booted) {
+        system_stats_.uptime = xTaskGetTickCount();
+        system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
+        system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
+        system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
+        system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
+        system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
+        system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
+        system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
+        system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
+    }
+}
+}
+
+int odrive_main(void) {
     // Start ADC for temperature measurements and user measurements
     start_general_purpose_adc();
 
diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp
index 79982e64..b22fae34 100644
--- a/Firmware/communication/communication.cpp
+++ b/Firmware/communication/communication.cpp
@@ -186,16 +186,18 @@ using tree_type = decltype(make_obj_tree());
 uint8_t tree_buffer[sizeof(tree_type)];
 
 
-// Thread to handle deffered processing of USB interrupt, and
-// read commands out of the UART DMA circular buffer
-void communication_task(void * ctx) {
-    (void) ctx; // unused parameter
-
+void initTree(){
     // TODO: this is supposed to use the move constructor, but currently
     // the compiler uses the copy-constructor instead. Thus the make_obj_tree
     // ends up with a stupid stack size of around 8000 bytes. Fix this.
     auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree());
     fibre_publish(*tree_ptr);
+}
+
+// Thread to handle deffered processing of USB interrupt, and
+// read commands out of the UART DMA circular buffer
+void communication_task(void * ctx) {
+    (void) ctx; // unused parameter
 
     // Allow main init to continue
     endpoint_list_valid = true;
diff --git a/Firmware/communication/communication.h b/Firmware/communication/communication.h
index 8b38e6b4..b4bab74b 100644
--- a/Firmware/communication/communication.h
+++ b/Firmware/communication/communication.h
@@ -21,6 +21,7 @@ extern const uint8_t hw_version_minor;
 extern const uint8_t hw_version_variant;
 
 void init_communication(void);
+void initTree();
 void communication_task(void * ctx);
 
 #ifdef __cplusplus