feat: use cpp version of common drivers

.gitignore

@@ -1,6 +1,7 @@
 build/
+.vscode
 .settings
-.cproject
 .project
 .launch
+.cproject
 .cache

CMakeLists.txt

@@ -12,6 +12,9 @@ set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)
 set(CMAKE_C_EXTENSIONS ON)
 
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS ON)
 
 # Define the build type
 if(NOT CMAKE_BUILD_TYPE)
@@ -29,7 +32,7 @@ project(${CMAKE_PROJECT_NAME})
 message("Build type: " ${CMAKE_BUILD_TYPE})
 
 # Enable CMake support for ASM and C languages
-enable_language(C ASM)
+enable_language(C CXX ASM)
 
 # Create an executable object type
 add_executable(${CMAKE_PROJECT_NAME})
@@ -46,6 +49,7 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
 # Add sources to executable
 target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user sources here
+    ./Core/Src/device-init.cpp
 )
 
 # Add include paths
@@ -56,6 +60,7 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
 # Add project symbols (macros)
 target_compile_definitions(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user defined symbols
+    USE_CDC_DEBUG
 )
 
 # Remove wrong libob.a library dependency when using cpp files

Core/Inc/device-init.h

@@ -0,0 +1,24 @@
+/**
+ * 
+ */
+
+#ifndef DEVICE_INIT_H_
+#define DEVICE_INIT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "hal.h"
+
+/**
+ * @brief device init
+ */
+void device_init(SPI_HandleTypeDef *hspi1, SPI_HandleTypeDef *hspi2, SPI_HandleTypeDef *hspi3, CRC_HandleTypeDef *hcrc);
+void device_disable_flash();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

Core/Src/device-init.cpp

@@ -0,0 +1,188 @@
+#include "device-init.h"
+
+#include "main.h"
+#include "FreeRTOS.h"
+#include "semphr.h"
+
+#include "log.h"
+
+#include "sensor.h"
+#include "spi.h"
+#include "bmi088.h"
+#include "bmp581.h"
+#include "gd5f1gq5xe.h"
+#include "stm32f411xe.h"
+#include "stm32f4xx_hal_crc.h"
+
+#include <array>
+#include <optional>
+
+namespace {
+  inline uint32_t checksum(CRC_HandleTypeDef &hcrc, const uint8_t *data, size_t length) {
+    return HAL_CRC_Calculate(&hcrc, (uint32_t *)data, length);
+  }
+}
+
+enum Sensors { BMP581, BMI088, NUM_SENSORS };
+class TEST_CLASS {
+private:
+  CRC_HandleTypeDef &hcrc;
+
+  // Peripherals
+  Common::SPI accel;
+  Common::SPI gyro;
+  Common::SPI bmp;
+  Common::SPI flash;
+
+  // Sensors and Flash
+  Common::GD5F1GQ5XE flash_dev;
+  Common::BMP581 bmp581_dev;
+  Common::BMI088 bmi088_dev;
+  std::array<Common::Sensor*, NUM_SENSORS> sensors;
+
+  // Runtime State
+  SemaphoreHandle_t packet_mutex = xSemaphoreCreateMutex();
+  Common::Packet packet = Common::Packet();
+  volatile bool save_to_flash = false;
+  lfs_file_t packet_file;
+
+  int32_t fs_size = 0;
+  uint32_t boot_count = 0;
+  uint32_t file_size = 0;
+
+  // Task attributes
+  TaskHandle_t sensor_task_handle = NULL;
+  TaskHandle_t flash_task_handle = NULL;
+  // Tasks
+  static void sensor_task(void *argument) {
+    auto *self = static_cast<TEST_CLASS*>(argument);
+    self->sensor_loop();
+  }
+
+  static void flash_task(void *argument) {
+    auto *self = static_cast<TEST_CLASS*>(argument);
+    self->flash_loop();
+  }
+public:
+  TEST_CLASS(SPI_HandleTypeDef &hspi1, SPI_HandleTypeDef &hspi2, SPI_HandleTypeDef &hspi3, CRC_HandleTypeDef &hcrc)
+  : hcrc(hcrc),
+    accel(&hspi1, IMU2_ACC_CS_GPIO_Port, IMU2_ACC_CS_Pin),
+    gyro(&hspi1, IMU2_GYRO_CS_GPIO_Port, IMU2_GYRO_CS_Pin),
+    bmp(&hspi2,  BAR1_CS_GPIO_Port, BAR1_CS_Pin),
+    flash(&hspi3, FLASH_CS_GPIO_Port, FLASH_CS_Pin),
+
+    flash_dev(flash),
+    bmp581_dev(bmp),
+    bmi088_dev(accel, gyro),
+    sensors{&bmp581_dev, &bmi088_dev}
+  {
+    // Initialize sensors
+    for (auto& sensor : sensors) {
+      bool ready = 0;
+      for (int c = 0; c < 10; ++c) {
+        ready = sensor->init();
+        if (ready) break;
+        Delay(20000);
+      }
+      if (!ready) sensor = nullptr; 
+    }
+
+    // Initialize flash
+    for (int c = 0; c < 3; ++c) {
+      if (flash_dev.init()) {
+        fs_size = flash_dev.mount();
+        if (fs_size >= 0) {
+          save_to_flash = true;
+          boot_count = flash_dev.bootcount(false);
+          file_size = flash_dev.open(&packet_file, "packets");
+        }
+        break;
+      }
+      Delay(5000);
+    }
+    Common::LOG("Flash: %u (size), %u (boot), %u (packet_size)\r\n", fs_size, boot_count, file_size);
+  }
+
+  void sensor_loop() {
+    // Simple counter for task notification
+    uint32_t counter = 0;
+    // Run this task 200 times per second
+    TickType_t last_wake_up = xTaskGetTickCount();
+    int hertz = 200;
+
+    for (;;) {
+      if (xSemaphoreTake(packet_mutex, portMAX_DELAY) == pdTRUE) {
+        // Read from all sensors
+        for (auto *sensor : sensors)
+          if (sensor != nullptr) sensor->read(packet);
+
+        // This is originally for camera, but we will use it for flash for now
+        packet.status = (flash_task_handle != NULL);
+        // TODO: change this to microseconds at a later time
+        packet.time_us = xTaskGetTickCount() * portTICK_PERIOD_MS;
+        packet.checksum = checksum(hcrc, (const uint8_t *) &packet + sizeof(short),
+                                   sizeof(packet) - 6);
+        HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
+        xSemaphoreGive(packet_mutex);
+
+        // Tell flash to save data every other packet
+        if ((++counter) >= 2 && flash_task_handle != nullptr) {
+          counter = 0;
+          if (save_to_flash) xTaskNotifyGive(flash_task_handle);
+        }
+      }
+      // Go to sleep little task...
+      vTaskDelayUntil(&last_wake_up, configTICK_RATE_HZ / hertz);
+    }
+  }
+
+  void flash_loop() {
+    auto copy = Common::Packet();
+    for (;;) {
+      // Wait until notified
+      ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+      // Someone notified us to stop task collection, shut the poor flash down :(
+      if (!save_to_flash) {
+        flash_dev.close(&packet_file);
+        flash_dev.unmount();
+        vTaskDelete(NULL);
+        return;
+      }
+      // Make a local copy, so we don't block
+      if (xSemaphoreTake(packet_mutex, portMAX_DELAY) == pdTRUE) {
+        copy = packet;
+        xSemaphoreGive(packet_mutex);
+      }
+
+      // Save to flash
+      flash_dev.append(&packet_file, (uint8_t*) &copy, sizeof(copy));
+    }
+  }
+
+  void disable_flash() {
+    if (save_to_flash) {
+      save_to_flash = false;
+      vTaskNotifyGiveFromISR(flash_task_handle, NULL);
+    }
+  }
+
+  void start_tasks() {
+    // Idle task has priority of 0
+    xTaskCreate(flash_task, "flash task", 512, this, 1, &flash_task_handle);
+    xTaskCreate(sensor_task, "sensor task", 256, this, 2, &sensor_task_handle);
+  }
+};
+
+// TODO: need to discuss how to better do this
+static TEST_CLASS* device_ptr = nullptr;
+extern "C" {
+  void device_init(SPI_HandleTypeDef *hspi1, SPI_HandleTypeDef *hspi2, SPI_HandleTypeDef *hspi3, CRC_HandleTypeDef *hcrc) {
+    static TEST_CLASS device = TEST_CLASS(*hspi1, *hspi2, *hspi3, *hcrc);
+    device_ptr = &device;
+    device.start_tasks();
+  }
+
+  void device_disable_flash() {
+    if (device_ptr != nullptr) device_ptr->disable_flash();
+  }
+}