LIS2MDL & LSM6DSV & MotionFX PR

CMakeLists.txt

@@ -62,15 +62,17 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
 
     # Misc Embedded-Base Stuff (drivers, middleware, utils, etc.)
     "../Drivers/Embedded-Base/general/src/sht30.c"
-    "../Drivers/Embedded-Base/general/src/lsm6dso_reg.c"
-    "../Drivers/Embedded-Base/general/src/lsm6dso.c"
+    "../Drivers/Embedded-Base/general/include/lsm6dsv_reg.h"
+    "../Drivers/Embedded-Base/general/src/lsm6dsv_reg.c"
+    "../Drivers/Embedded-Base/general/src/lis2mdl_reg.c"
     "../Drivers/Embedded-Base/platforms/stm32h563/src/fdcan.c"
     "./Drivers/Embedded-Base/middleware/src/bitstream.c"
     "./Drivers/Embedded-Base/middleware/src/debounce.c"
     "./Drivers/Embedded-Base/middleware/src/timer.c"
     "./Drivers/Embedded-Base/middleware/src/c_utils.c"
 
     # Repo Source Files
+    "./Core/Src/u_utils.c"
     "./Core/Src/u_can.c"
     "./Core/Src/u_mutexes.c"
     "./Core/Src/u_queues.c"

Core/Inc/main.h

@@ -65,10 +65,10 @@ void Error_Handler(void);
 #define TOF_RST_GPIO_Port GPIOE
 #define TOF_LPN_Pin GPIO_PIN_5
 #define TOF_LPN_GPIO_Port GPIOE
-#define DEBUG_LED2_Pin GPIO_PIN_2
-#define DEBUG_LED2_GPIO_Port GPIOG
 #define DEBUG_LED1_Pin GPIO_PIN_3
 #define DEBUG_LED1_GPIO_Port GPIOG
+#define DEBUG_LED2_Pin GPIO_PIN_4
+#define DEBUG_LED2_GPIO_Port GPIOG
 
 /* USER CODE BEGIN Private defines */
 

Core/Inc/u_can.h

@@ -2,6 +2,10 @@
 
 #include "u_tx_can.h"
 
-#define SHT30_CAN_ID 0xFA
+#define SHT30_CAN_ID        0xFA
+#define IMU_ACCEL_CAN_ID    0xFC
+#define IMU_GYRO_CAN_ID     0xFB
+#define MAGNOMETER_CAN_ID   0XFD
+#define ORIENTATION_CAN_ID  0XFD
 
 // u_TODO - finish this file

Core/Inc/u_sensors.h

@@ -2,14 +2,70 @@
 
 #include <stdint.h>
 
+#define MOTION_FX_STATE_SIZE (size_t)(2432)
+
+/* 
+ * Bias correction thresholds from MotionFX example implementations.
+ * Lower GBIAS_* = more correction, higher = more stability.  
+ * Adjust if drift or instability occurs.
+ */
+#define GBIAS_ACC_TH_SC	 (2.0f * 0.000765f)
+#define GBIAS_GYRO_TH_SC (2.0f * 0.002f)
+#define GBIAS_MAG_TH_SC	 0.0f
+
+/*
+ * DECIMATION controls how often sensor data is processed.  
+ * 1U = use every sample, higher = skip samples  
+ * Increase to ignore more samples if CPU usage is too high.
+ */
+#define DECIMATION 1U
+
+/**
+ * @brief initializes the lsm6dsv imu
+ * @return whether there were errors initializing the lsm6dsv imu
+ */
+uint16_t init_imu();
+
+/**
+ * @brief initializes Magnetometer for reading
+ * @return returns tx status for errors
+ */
+uint16_t init_magnetometer();
+
+/**
+ * @brief reads data from the lsm6dsv and lis2mdl, and performs motionfx rotation calculations
+ * @return whether there were errors in reading the data
+ */
+uint16_t read_imu_and_magnometer();
+
+/**
+ * @brief sends data from the lsm6dsv, lis2mdl, and motionfx rotation data over CAN
+ */
+void send_imu_and_magnometer_data();
+
+/**
+ * @brief initializes the motion fx library
+ */
+void motion_fx_init();
+
+/**
+ * @brief zeros the motion fx rotation data
+ */
+void calibrate();
+
 /**
  * @brief initializes the sht30 for reading temp and humidity
  * @return whether there were errors initializing the sht30
  */
-uint8_t init_sht30();
+uint16_t init_sht30();
+
+/**
+ * @brief reads temp and humidity data from the sht30
+ * @return whether there were errors in reading the data
+ */
+uint16_t read_sht30();
 
 /**
- * @brief reads and sends temp and humidity data from the sht30
- * @return whether there were errors in reading/sending the data
+ * @brief sends temp and humidity data from the sht30 over CAN
  */
-uint8_t read_sht30();
+void send_sht30_data();

Core/Inc/u_utils.h

@@ -0,0 +1,10 @@
+#pragma once
+
+/**
+ * Converts a float to a 16 bit integer.
+ * 
+ * @param value The float value to be converted
+ * 
+ * @return The converted 16 bit integer
+ */
+int16_t float_to_int16(float value);

Core/Src/app_threadx.c

@@ -68,9 +68,12 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
 
   /* Init user-written code that uses ThreadX stuff here. */
   CATCH_ERROR(queues_init(byte_pool), U_SUCCESS);
-  CATCH_ERROR(threads_init(byte_pool), U_SUCCESS);
   CATCH_ERROR(mutexes_init(), U_SUCCESS);
+  CATCH_ERROR(init_imu(), U_SUCCESS);
+  CATCH_ERROR(init_magnetometer(), U_SUCCESS);
   CATCH_ERROR(init_sht30(), U_SUCCESS);
+  motion_fx_init();
+  CATCH_ERROR(threads_init(byte_pool), U_SUCCESS);
 
   /* USER CODE END App_ThreadX_MEM_POOL */
   /* USER CODE BEGIN App_ThreadX_Init */