fix

Author: dnakhooda

Core/Inc/u_can.h

@@ -6,5 +6,6 @@
 #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,38 +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 reads and sends data from the lsm6dsv
- * @return whether there were errors in reading/sending the data
- */
-uint8_t read_imu();
-
 /**
  * @brief initializes Magnetometer for reading
  * @return returns tx status for errors
  */
 uint16_t init_magnetometer();
 
 /**
- * @brief reads and returns the information from magnetometer
- * @return returns any tx errors in reading from the 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
  */
-uint16_t read_magnetometer();
+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

@@ -72,6 +72,7 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
   CATCH_ERROR(init_imu(), U_SUCCESS);
   CATCH_ERROR(init_magnetometer(), U_SUCCESS);
   CATCH_ERROR(init_sht30(), U_SUCCESS);
+  CATCH_ERROR(motion_fx_init(), U_SUCCESS);
   CATCH_ERROR(threads_init(byte_pool), U_SUCCESS);
 
   /* USER CODE END App_ThreadX_MEM_POOL */