feat(flex-stacker): Bring up the internal/external status led bars and add gcode to control them.

stm32-modules/flex-stacker/firmware/CMakeLists.txt

@@ -21,7 +21,9 @@ set(${TARGET_MODULE_NAME}_FW_LINTABLE_SRCS
     ${SYSTEM_DIR}/freertos_idle_timer_task.cpp
     ${SYSTEM_DIR}/freertos_system_task.cpp
     ${SYSTEM_DIR}/system_policy.cpp
+    ${SYSTEM_DIR}/i2c_comms.cpp
     ${UI_DIR}/freertos_ui_task.cpp
+    ${UI_DIR}/ui_policy.cpp
     ${MOTOR_CONTROL_DIR}/freertos_motor_task.cpp
     ${MOTOR_CONTROL_DIR}/freertos_motor_driver_task.cpp
     ${MOTOR_CONTROL_DIR}/motor_driver_policy.cpp
@@ -36,6 +38,7 @@ set(${TARGET_MODULE_NAME}_FW_NONLINTABLE_SRCS
     ${SYSTEM_DIR}/hal_tick.c
     ${SYSTEM_DIR}/system_serial_number.c
     ${SYSTEM_DIR}/system_hardware.c
+    ${SYSTEM_DIR}/i2c_hardware.c
     ${UI_DIR}/ui_hardware.c
     ${COMMS_DIR}/usbd_conf.c
     ${COMMS_DIR}/usbd_desc.c

stm32-modules/flex-stacker/firmware/main.cpp

@@ -1,6 +1,8 @@
 #include "FreeRTOS.h"
 #include "firmware/firmware_tasks.hpp"
 #include "firmware/freertos_tasks.hpp"
+#include "firmware/i2c_comms.hpp"
+#include "firmware/i2c_hardware.h"
 #include "firmware/motor_hardware.h"
 #include "firmware/system_stm32g4xx.h"
 #include "flex-stacker/messages.hpp"
@@ -15,6 +17,8 @@
 #pragma GCC diagnostic pop
 
 using EntryPoint = std::function<void(tasks::FirmwareTasks::QueueAggregator *)>;
+using EntryPointUI = std::function<void(tasks::FirmwareTasks::QueueAggregator *,
+                                        i2c::hardware::I2C *)>;
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto motor_driver_task_entry = EntryPoint(motor_driver_task::run);
@@ -23,7 +27,7 @@ static auto motor_driver_task_entry = EntryPoint(motor_driver_task::run);
 static auto motor_task_entry = EntryPoint(motor_control_task::run);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
-static auto ui_task_entry = EntryPoint(ui_control_task::run);
+static auto ui_task_entry = EntryPointUI(ui_control_task::run);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto host_comms_entry = EntryPoint(host_comms_control_task::run);
@@ -51,7 +55,7 @@ static auto host_comms_task =
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto ui_task =
-    ot_utils::freertos_task::FreeRTOSTask<tasks::UI_STACK_SIZE, EntryPoint>(
+    ot_utils::freertos_task::FreeRTOSTask<tasks::UI_STACK_SIZE, EntryPointUI>(
         ui_task_entry);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
@@ -71,18 +75,24 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
     }
 }
 
+static auto i2c2_comms = i2c::hardware::I2C();
+static auto i2c3_comms = i2c::hardware::I2C();
+static auto i2c_handles = I2CHandlerStruct{};
+
 auto main() -> int {
     HardwareInit();
 
-    system_task.start(tasks::SYSTEM_TASK_PRIORITY, "System", &aggregator);
+    i2c_hardware_init(&i2c_handles);
+
+    i2c2_comms.set_handle(i2c_handles.i2c2, I2C_BUS_2);
+    i2c3_comms.set_handle(i2c_handles.i2c3, I2C_BUS_3);
 
+    system_task.start(tasks::SYSTEM_TASK_PRIORITY, "System", &aggregator);
     driver_task.start(tasks::MOTOR_DRIVER_TASK_PRIORITY, "Motor Driver",
                       &aggregator);
     motor_task.start(tasks::MOTOR_TASK_PRIORITY, "Motor", &aggregator);
-
     host_comms_task.start(tasks::COMMS_TASK_PRIORITY, "Comms", &aggregator);
-
-    ui_task.start(tasks::UI_TASK_PRIORITY, "UI", &aggregator);
+    ui_task.start(tasks::UI_TASK_PRIORITY, "UI", &aggregator, &i2c2_comms);
 
     vTaskStartScheduler();
     return 0;

stm32-modules/flex-stacker/firmware/system/i2c_comms.cpp

@@ -0,0 +1,27 @@
+#include "firmware/i2c_comms.hpp"
+
+#include <stdint.h>
+
+#include "firmware/i2c_hardware.h"
+#include "systemwide.h"
+
+using namespace i2c::hardware;
+
+auto I2C::set_handle(HAL_I2C_HANDLE i2c_handle, I2C_BUS i2c_bus) -> void {
+    this->bus = i2c_bus;
+    i2c_register_handle(i2c_handle, i2c_bus);
+}
+
+auto I2C::i2c_write(uint16_t dev_addr, uint16_t reg, uint8_t* data,
+                    uint16_t size) -> RxTxReturn {
+    MessageT resp{0};
+    auto ret = hal_i2c_write(bus, dev_addr, reg, data, size);
+    return RxTxReturn(ret, resp);
+}
+
+auto I2C::i2c_read(uint16_t dev_addr, uint16_t reg, uint16_t size)
+    -> RxTxReturn {
+    MessageT resp{0};
+    auto ret = hal_i2c_read(bus, dev_addr, reg, resp.data(), size);
+    return RxTxReturn(ret, resp);
+}

stm32-modules/flex-stacker/firmware/system/i2c_hardware.c

@@ -0,0 +1,275 @@
+#include <stdint.h>
+
+#include "stm32g4xx_hal.h"
+#include "stm32g4xx_hal_conf.h"
+#include "stm32g4xx_hal_gpio.h"
+#include "stm32g4xx_hal_def.h"
+#include "stm32g4xx_it.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "firmware/i2c_hardware.h"
+
+#define MAX_TIMEOUT (pdMS_TO_TICKS(100))
+#define MAX_RETRIES (3)
+#define MAX_I2C_HANDLES (3)
+#define NO_HANDLE_ERROR (255)
+#define REGISTER_ADDR_LEN (1)  // 1 byte
+
+static I2C_HandleTypeDef hi2c2;
+static I2C_HandleTypeDef hi2c3;
+
+typedef struct {
+    I2C_BUS bus;
+    I2C_HandleTypeDef *i2c_handle;
+    TaskHandle_t task_to_notify;
+    bool should_retry;
+} NotificationHandle_t;
+
+static NotificationHandle_t _notification_handles[MAX_I2C_HANDLES];
+
+static bool _initialized = false;
+
+HAL_I2C_HANDLE MX_I2C2_Init()
+{
+    hi2c2.State = HAL_I2C_STATE_RESET;
+    hi2c2.Instance = I2C2;
+    hi2c2.Init.Timing = 0x10C0ECFF;
+    hi2c2.Init.OwnAddress1 = 0;
+    hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c2.Init.OwnAddress2 = 0;
+    hi2c2.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+    hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c2) != HAL_OK)
+    {
+        Error_Handler();
+    }
+    /** Configure Analogue filter
+    */
+    if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
+    {
+        Error_Handler();
+    }
+    /** Configure Digital filter
+    */
+    if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK)
+    {
+        Error_Handler();
+    }
+
+    /** I2C Fast mode Plus enable */
+    __HAL_SYSCFG_FASTMODEPLUS_ENABLE(I2C_FASTMODEPLUS_I2C2);
+
+    return &hi2c2;
+}
+
+HAL_I2C_HANDLE MX_I2C3_Init() {
+    hi2c3.Instance = I2C3;
+    hi2c3.Init.Timing = 0x10C0ECFF;
+    hi2c3.Init.OwnAddress1 = 0;
+    hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c3.Init.OwnAddress2 = 0;
+    hi2c3.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+    hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c3) != HAL_OK) {
+        Error_Handler();
+    }
+    /** Configure Analogue filter
+     */
+    if (HAL_I2CEx_ConfigAnalogFilter(&hi2c3, I2C_ANALOGFILTER_ENABLE) !=
+        HAL_OK) {
+        Error_Handler();
+    }
+    /** Configure Digital filter
+     */
+    if (HAL_I2CEx_ConfigDigitalFilter(&hi2c3, 0) != HAL_OK) {
+        Error_Handler();
+    }
+
+    /** I2C Fast mode Plus enable */
+    __HAL_SYSCFG_FASTMODEPLUS_ENABLE(I2C_FASTMODEPLUS_I2C3);
+
+    return &hi2c3;
+}
+
+void i2c_hardware_init(I2CHandlerStruct* i2c_handles) {
+    HAL_I2C_HANDLE i2c2 = MX_I2C2_Init();
+    HAL_I2C_HANDLE i2c3 = MX_I2C3_Init();
+    i2c_handles->i2c2 = i2c2;
+    i2c_handles->i2c3 = i2c3;
+}
+
+/**
+ * @brief Get the notification handle based on the I2C Bus.
+ * Returns NULL if the handle is not found.
+ */
+static NotificationHandle_t* lookup_handle(I2C_BUS bus) {
+    for(size_t i = 0; i < MAX_I2C_HANDLES; ++i) {
+        I2C_BUS notif_bus = _notification_handles[i].bus;
+        if(notif_bus != NO_BUS && notif_bus == bus) {
+            return &_notification_handles[i];
+        }
+    }
+    return NULL;
+}
+
+/**
+ * @brief Get the notification handle based on the HAL I2C struct.
+ * Returns NULL if the handle is not found.
+ */
+static NotificationHandle_t* get_handle(I2C_HandleTypeDef *i2c_handle) {
+    for(size_t i = 0; i < MAX_I2C_HANDLES; ++i) {
+        if(_notification_handles[i].i2c_handle == i2c_handle) {
+            return &_notification_handles[i];
+        }
+    }
+    return NULL;
+}
+
+static void initialize_notification_handles() {
+    if(!_initialized) {
+        for(size_t i = 0; i < MAX_I2C_HANDLES; ++i) {
+            _notification_handles[i].i2c_handle = NULL;
+            _notification_handles[i].task_to_notify = NULL;
+            _notification_handles[i].should_retry = false;
+            _notification_handles[i].bus = NO_BUS;
+        }
+        _initialized = true;
+    }
+}
+
+/**
+ * @brief Common handler for all I2C callbacks.
+ */
+static void handle_i2c_callback(I2C_HandleTypeDef *i2c_handle, bool error) {
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    NotificationHandle_t *instance = get_handle(i2c_handle);
+    if(instance == NULL) return;
+    if(instance->task_to_notify == NULL) return;
+
+    vTaskNotifyGiveFromISR(instance->task_to_notify, 
+                            &xHigherPriorityTaskWoken);
+    instance->task_to_notify = NULL;
+    instance->should_retry = error;
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+}
+
+bool i2c_register_handle(HAL_I2C_HANDLE handle, I2C_BUS bus) {
+    initialize_notification_handles();
+
+    I2C_HandleTypeDef* i2c_handle = (I2C_HandleTypeDef*)handle;
+    NotificationHandle_t *notif_handle = lookup_handle(bus);
+    if(notif_handle != NULL) return true;  // Already Registered
+
+    // Now find an empty slot
+    notif_handle = get_handle(NULL);
+    if(notif_handle == NULL) return false; // No empty slots
+
+    notif_handle->i2c_handle = i2c_handle;
+    notif_handle->bus = bus;
+    return true;
+}
+
+/**
+ * Wrapper around HAL_I2C_Mem_Write
+ */
+uint8_t hal_i2c_write(I2C_BUS bus, uint16_t DevAddress, uint8_t reg, uint8_t *data, uint16_t size) {
+    uint32_t notification_val = 0;
+    NotificationHandle_t *notification_handle = lookup_handle(bus);
+    I2C_HandleTypeDef* i2c_handle = (I2C_HandleTypeDef*)notification_handle->i2c_handle;
+
+    // Bus was not registered
+    if(notification_handle == NULL) return false;
+
+    uint8_t retries = 0;
+    HAL_StatusTypeDef tx_result = HAL_OK;
+    do {
+        // Setup task handle and send message
+        notification_handle->task_to_notify = xTaskGetCurrentTaskHandle();
+        tx_result = HAL_I2C_Mem_Write_IT(i2c_handle,
+                            DevAddress, reg, REGISTER_ADDR_LEN, data, size);
+
+        // Wait for callback and check result
+        notification_val = ulTaskNotifyTake(pdTRUE, MAX_TIMEOUT);
+        if (tx_result == HAL_OK) break;
+        if (notification_handle->should_retry) tx_result = HAL_BUSY;
+        if (notification_val != 1) tx_result = HAL_TIMEOUT;
+
+        retries += 1;
+    } while(retries < MAX_RETRIES);
+    return tx_result;
+}
+
+/**
+ * Wrapper around HAL_I2C_Mem_Read
+ */
+uint8_t hal_i2c_read(I2C_BUS bus, uint16_t DevAddress, uint16_t reg, uint8_t *data, uint16_t size) {
+    uint32_t notification_val = 0;
+    NotificationHandle_t *notification_handle = lookup_handle(bus);
+    I2C_HandleTypeDef* i2c_handle = (I2C_HandleTypeDef*)notification_handle->i2c_handle;
+
+    // Bus was not registered
+    if(notification_handle == NULL) return NO_HANDLE_ERROR;
+
+    uint8_t retries = 0;
+    HAL_StatusTypeDef rx_result = HAL_OK;
+    do {
+        // Setup task handle and send message
+        notification_handle->task_to_notify = xTaskGetCurrentTaskHandle();
+        rx_result = HAL_I2C_Mem_Read_IT(i2c_handle,
+                            DevAddress, reg, REGISTER_ADDR_LEN, data, size);
+
+        // Wait for callback and check result.
+        notification_val = ulTaskNotifyTake(pdTRUE, MAX_TIMEOUT);
+        if (rx_result == HAL_OK) break;
+        if (notification_handle->should_retry) rx_result = HAL_BUSY;
+        if (notification_val != 1) rx_result = HAL_TIMEOUT;
+
+        retries += 1;
+    } while (retries < MAX_RETRIES);
+    return rx_result;
+}
+
+void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, true);
+}
+
+void I2C2_EV_IRQHandler(void)
+{
+    HAL_I2C_EV_IRQHandler(&hi2c2);
+}
+
+void I2C2_ER_IRQHandler(void)
+{
+    HAL_I2C_ER_IRQHandler(&hi2c2);
+}
+
+void I2C3_EV_IRQHandler(void)
+{
+    HAL_I2C_EV_IRQHandler(&hi2c3);
+}
+
+void I2C3_ER_IRQHandler(void)
+{
+    HAL_I2C_ER_IRQHandler(&hi2c3);
+}