add bmi323 driver

app/CMakeLists.txt

@@ -7,6 +7,8 @@ add_library(${PROJECT} SHARED
     "${CMAKE_CURRENT_SOURCE_DIR}/src/TimerFibre"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/EtherCatFibre"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/IMUFibre"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/BMI088Fibre"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/BMI323Fibre"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/ForceSensorFibre"
 )
 

app/inc/BMI088Fibre.h

@@ -0,0 +1,35 @@
+#include "DataModel.h"
+#include "Fibre.h"
+#include "BMI088.h"
+
+class BMI088Fibre : public Fibre
+{
+public:
+    BMI088Fibre(const char *name,
+             board::spi_identifier spi_name,
+             DataItemId imuGyroX,
+             DataItemId imuGyroY,
+             DataItemId imuGyroZ,
+             DataItemId imuAccelX,
+             DataItemId imuAccelY,
+             DataItemId imuAccelZ,
+             DataItemId imuTemp);
+
+    ~BMI088Fibre() override;
+
+    void Init() override;
+
+    void Run() override;
+
+    void Interrupt();
+
+private:
+    BMI088 boardIMU_;
+    DataItem imuGyroX_;
+    DataItem imuGyroY_;
+    DataItem imuGyroZ_;
+    DataItem imuAccelX_;
+    DataItem imuAccelY_;
+    DataItem imuAccelZ_;
+    DataItem imuTemp_;
+};

app/inc/BMI323Fibre.h

@@ -0,0 +1,35 @@
+#include "DataModel.h"
+#include "Fibre.h"
+#include "BMI323.h"
+
+class BMI323Fibre : public Fibre
+{
+public:
+    BMI323Fibre(const char *name,
+             board::spi_identifier spi_name,
+             DataItemId imuGyroX,
+             DataItemId imuGyroY,
+             DataItemId imuGyroZ,
+             DataItemId imuAccelX,
+             DataItemId imuAccelY,
+             DataItemId imuAccelZ,
+             DataItemId imuTemp);
+
+    ~BMI323Fibre() override;
+
+    void Init() override;
+
+    void Run() override;
+
+    void Interrupt();
+
+private:
+    BMI323 boardIMU_;
+    DataItem imuGyroX_;
+    DataItem imuGyroY_;
+    DataItem imuGyroZ_;
+    DataItem imuAccelX_;
+    DataItem imuAccelY_;
+    DataItem imuAccelZ_;
+    DataItem imuTemp_;
+};

app/src/BMI088Fibre.cc

@@ -0,0 +1,80 @@
+#include "BMI088Fibre.h"
+
+BMI088Fibre::BMI088Fibre(const char *name,
+                   board::spi_identifier spi_name,
+                   DataItemId imuGyroX,
+                   DataItemId imuGyroY,
+                   DataItemId imuGyroZ,
+                   DataItemId imuAccelX,
+                   DataItemId imuAccelY,
+                   DataItemId imuAccelZ,
+                   DataItemId imuTemp)
+    : Fibre(name),
+      boardIMU_{spi_name},
+      imuGyroX_{DataItem(imuGyroX, true)},
+      imuGyroY_{DataItem(imuGyroY, true)},
+      imuGyroZ_{DataItem(imuGyroZ, true)},
+      imuAccelX_{DataItem(imuAccelX, true)},
+      imuAccelY_{DataItem(imuAccelY, true)},
+      imuAccelZ_{DataItem(imuAccelZ, true)},
+      imuTemp_{DataItem(imuTemp, true)}
+{
+
+    FibreManager &thread = FibreManager::getInstance(THREAD_1MS_ID);
+    thread.Add(std::shared_ptr<Fibre>(std::shared_ptr<Fibre>{}, this));
+}
+
+BMI088Fibre::~BMI088Fibre()
+{
+}
+
+void BMI088Fibre::Init()
+{
+    boardIMU_.init();
+}
+
+void BMI088Fibre::Run()
+{
+    if (boardIMU_.isBusy())
+        return;
+
+    if (boardIMU_.getState() == SPI_Slave::INITIALIZING)
+    {
+        boardIMU_.configure();
+    }
+    else if (boardIMU_.getState() == SPI_Slave::ERROR)
+    {
+        // TODO: Reset Board or Turn ON Error LED
+    }
+    else
+    {
+        boardIMU_.request_read();
+    }
+    boardIMU_.setBusy();
+}
+
+void BMI088Fibre::Interrupt()
+{
+    if (boardIMU_.getState() == SPI_Slave::INITIALIZING)
+    {
+        boardIMU_.checkConfiguration();
+    }
+    else if (boardIMU_.getState() == SPI_Slave::INITIALIZED)
+    {
+        boardIMU_.setState(SPI_Slave::READY);
+    }
+    else if (boardIMU_.getState() == SPI_Slave::READY)
+    {
+        boardIMU_.read();
+        int16_t *imuData = boardIMU_.getIMUData();
+
+        imuGyroX_.set(imuData[0]);
+        imuGyroY_.set(imuData[1]);
+        imuGyroZ_.set(imuData[2]);
+        imuAccelX_.set(imuData[3]);
+        imuAccelY_.set(imuData[4]);
+        imuAccelZ_.set(imuData[5]);
+        imuTemp_.set(imuData[6]);
+    }
+    boardIMU_.setAvailable();
+}

app/src/BMI323Fibre.cc

@@ -0,0 +1,80 @@
+#include "BMI323Fibre.h"
+
+BMI323Fibre::BMI323Fibre(const char *name,
+                   board::spi_identifier spi_name,
+                   DataItemId imuGyroX,
+                   DataItemId imuGyroY,
+                   DataItemId imuGyroZ,
+                   DataItemId imuAccelX,
+                   DataItemId imuAccelY,
+                   DataItemId imuAccelZ,
+                   DataItemId imuTemp)
+    : Fibre(name),
+      boardIMU_{spi_name},
+      imuGyroX_{DataItem(imuGyroX, true)},
+      imuGyroY_{DataItem(imuGyroY, true)},
+      imuGyroZ_{DataItem(imuGyroZ, true)},
+      imuAccelX_{DataItem(imuAccelX, true)},
+      imuAccelY_{DataItem(imuAccelY, true)},
+      imuAccelZ_{DataItem(imuAccelZ, true)},
+      imuTemp_{DataItem(imuTemp, true)}
+{
+
+    FibreManager &thread = FibreManager::getInstance(THREAD_1MS_ID);
+    thread.Add(std::shared_ptr<Fibre>(std::shared_ptr<Fibre>{}, this));
+}
+
+BMI323Fibre::~BMI323Fibre()
+{
+}
+
+void BMI323Fibre::Init()
+{
+    boardIMU_.init();
+}
+
+void BMI323Fibre::Run()
+{
+    if (boardIMU_.isBusy())
+        return;
+
+    if (boardIMU_.getState() == SPI_Slave::INITIALIZING)
+    {
+        boardIMU_.configure();
+    }
+    else if (boardIMU_.getState() == SPI_Slave::ERROR)
+    {
+        // TODO: Reset Board or Turn ON Error LED
+    }
+    else
+    {
+        boardIMU_.request_read();
+    }
+    boardIMU_.setBusy();
+}
+
+void BMI323Fibre::Interrupt()
+{
+    if (boardIMU_.getState() == SPI_Slave::INITIALIZING)
+    {
+        boardIMU_.checkConfiguration();
+    }
+    else if (boardIMU_.getState() == SPI_Slave::INITIALIZED)
+    {
+        boardIMU_.setState(SPI_Slave::READY);
+    }
+    else if (boardIMU_.getState() == SPI_Slave::READY)
+    {
+        boardIMU_.read();
+        int16_t *imuData = boardIMU_.getIMUData();
+
+        imuGyroX_.set(imuData[0]);
+        imuGyroY_.set(imuData[1]);
+        imuGyroZ_.set(imuData[2]);
+        imuAccelX_.set(imuData[3]);
+        imuAccelY_.set(imuData[4]);
+        imuAccelZ_.set(imuData[5]);
+        imuTemp_.set(imuData[6]);
+    }
+    boardIMU_.setAvailable();
+}

hal/CMakeLists.txt

@@ -7,6 +7,7 @@ IF (NOT TEST)
 	  ${CMAKE_CURRENT_SOURCE_DIR}/src/ForceSensor.cc
 	  ${CMAKE_CURRENT_SOURCE_DIR}/src/LSM6DSM.cc
 	  ${CMAKE_CURRENT_SOURCE_DIR}/src/BMI088.cc
+	  ${CMAKE_CURRENT_SOURCE_DIR}/src/BMI323.cc
 	  ${CMAKE_CURRENT_SOURCE_DIR}/src/SOES_hooks.cc
 	  ${CMAKE_CURRENT_SOURCE_DIR}/xmc/GPIO_XMC.cc
 	  ${CMAKE_CURRENT_SOURCE_DIR}/xmc/SPI_XMC.cc

hal/inc/BMI323.h

@@ -0,0 +1,41 @@
+#ifndef BMI323_H_
+#define BMI323_H_
+
+#include "SPI_Slave.h"
+
+// Details of register map: 
+// Register map of the IMU
+#define CHIP_ID 0x00
+
+#define ACC_CONF 0x20
+#define GYRO_CONF 0x21
+
+#define ACC_DATA_X 0x03
+
+#define IMU_READ 0x80
+#define IMU_WRITE 0x00
+
+class BMI323 : public SPI_Slave
+{
+public:
+    BMI323(board::spi_identifier spi_name);
+    ~BMI323() = default;
+
+    void configure() override;
+    void request_read() override;
+    void read() override;
+    void checkConfiguration() override;
+
+    int16_t *getIMUData();
+
+private:
+    int16_t *buffer0_;
+    int16_t *buffer1_;
+    int16_t **free_;
+    int16_t **consume_;
+    uint8_t step_ = 0;
+    uint8_t imuConfig_[6] = {};
+    int16_t imuData_[7];
+};
+
+#endif