Big Kahuna - IREC 2025 - Dawsyn's Final Commit (#16)HEADmain

# Dawsyn's Final Commit

This one is a little emotional as this is my final commit in this repository and as a member of Rocketry at Virginia Tech. This merges the changes seen in the branch known as 'big_kahuna' into main. This is the version of the ADS software as seen on [Roadkill](https://drive.google.com/file/d/120BvI-0ntliHo6i9UxcCn2pXAl-JsdP_/view?usp=drive_link) in the 2025 IREC competition. There are bound to be bugs, but I have found it useful to have the final competition version to be the one present on main at the end of every academic year. Hopefully this is useful to the next lead.

## Primary Changes
+ NEW I2C drivers to support sensors present on new ADS custom PCB
+ NEW logging library found in separate repository and pulled in as submodule ([pico-logger](https://github.com/rocketryvt/pico-logger))
     + No longer dependent on different flash chip from one used for code storage! Compile executable as RP2040 'copy-to-ram' type to increase flash read/write speeds!
+ NEW fixed-point libraries to allow for increased performance and sensor sampling speeds on RP2040 that lacks FPU
+ FreeRTOS Simultaneous Multi-processing (SMP) architecture for task handling and easier introduction / testing of new features
+ Serial monitor / command system with task performance monitoring commands
+ WORKING Kalman filter that takes altitude from barometer as measurement and z-axis acceleration from IMU as control to generate state vector containing filtered altitude and vertical velocity
+ NEW CFD equations from the Ben-ogrithm (to replace the Chen-ogrithm) that includes:
    + Apogee prediction model that takes current drag force, altitude, and vertical velocity
    + Current Drag Force equation based on current deployment and vertical velocity to use for Apogee Prediction model
    + Desired Drag force equation based on current altitude and vertical velocity to generate what drag force is needed to reach 10K ft
    + Deployment percentage equation based on current velocity and desired drag force to map to flap deployment percentage

1 files changed, 177 insertions, 0 deletions

diff --git a/include/iim42653.hpp b/include/iim42653.hpp
new file mode 100644
index 0000000..ac64edc
--- /dev/null
+++ b/include/iim42653.hpp
@@ -0,0 +1,177 @@
+#pragma once
+
+#include "serial.hpp"
+#include <stdint.h>
+
+#include <hardware/i2c.h>
+#include <hardware/clocks.h>
+
+#if (USE_FREERTOS == 1)
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "portmacro.h"
+#include "projdefs.h"
+#include "serial.hpp"
+#include "task.h"
+#include "semphr.h"
+#endif
+
+#define IIM42653_I2C_ADDR 0x68
+#define IIM42653_CLOCK_SOURCE_SYSTEM 0x6    //Use system clock as source
+#define IIM42653_CLOCK_DIVISOR 3125         //125MHz --> 40kHz
+	
+// DEVICE_CONFIG [State Configuration, SPI mode and Reset]
+#define R_IIM42653_DEVICE_CONFIG 0x11
+typedef union {
+    struct {
+        bool SOFT_RESET_CONFIG :1;  //Enable/disable soft reset
+        uint8_t RESERVED :3;        //Reserved
+        bool SPI_MODE :1;           //SPI Mode (internal), 0/3 or 1/2
+        uint8_t RESERVED_ :3;       //Reserved
+    } fields;
+    uint8_t data;
+} IIM42653_DEVICE_CONFIG;
+#define b_IIM42653_DEVICE_CONFIG_ENABLE_SOFT_RESET  0b1
+#define b_IIM42653_DEVICE_CONFIG_DISABLE_SOFT_RESET 0b0
+
+// DRIVE_CONFIG [Configuration, SPI/I2C Speed]
+#define R_IIM42653_DRIVE_CONFIG 0x13
+typedef union {
+    struct {
+        uint8_t SPI_SLEW_RATE :3;   //SPI slew rate on Pin 14
+        uint8_t I2C_SLEW_RATE :3;   //I2C slew rate on Pin 14
+        uint8_t RESERVED :2;        //Reserved
+    } fields;
+    uint8_t data;
+} IIM42653_DRIVE_CONFIG;
+#define B_IIM42653_DRIVE_CONFIG_SLEW_20_TO_60_nS 0x00
+#define B_IIM42653_DRIVE_CONFIG_SLEW_2_TO_6_nS   0x20
+
+// TEMP_DATA1_UI - TEMP_DATA0_UI [Output, Temperature Sensor]
+#define R_IIM42653_TEMP_DATA1_UI 0x1D
+#define R_IIM42653_TEMP_DATA0_UI 0x1E
+
+// ACCEL_DATA_X1_UI - GYRO_DATA_Z0_UI [Output, 3-Axis Gyro and Accel Data]
+#define R_IIM42653_ACCEL_DATA_X1_UI          0x1F
+#define R_IIM42653_ACCEL_DATA_X0_UI          0x20
+#define R_IIM42653_ACCEL_DATA_Y1_UI          0x21
+#define R_IIM42653_ACCEL_DATA_Y0_UI          0x22
+#define R_IIM42653_ACCEL_DATA_Z1_UI          0x23
+#define R_IIM42653_ACCEL_DATA_Z0_UI          0x24
+#define R_IIM42653_GYRO_DATA_X1_UI           0x25
+#define R_IIM42653_GYRO_DATA_X0_UI           0x26
+#define R_IIM42653_GYRO_DATA_Y1_UI           0x27
+#define R_IIM42653_GYRO_DATA_Y0_UI           0x28
+#define R_IIM42653_GYRO_DATA_Z1_UI           0x29
+#define R_IIM42653_GYRO_DATA_Z0_UI           0x2A
+#define S_IIM42653_ACCEL_SENSITIVITY_FACTOR  1024.0f
+#define S_IIM42653_GYRO_SENSITIVITY_FACTOR   8.2f
+
+// PWR_MGMT0 [State Configuration, Sensor enabling]
+#define R_IIM42653_PWR_MGMT0 0x4E
+typedef union {
+	struct {
+        uint8_t ACCEL_MODE :2;  //Accelerometer power mode
+        uint8_t GYRO_MODE :2;   //Gyroscope power mode
+        bool IDLE :1;           //Run RC oscillator even if Accel/Gyro are off
+        bool TEMP_DIS :1;       //Disable the temperature sensor
+        bool S4S_ENABLE :1;     //Enable synchronous timing control (???) with custom command
+        bool RESERVED :1;       //Reserved
+    } fields;
+    uint8_t data;
+} IIM42653_PWR_MGMT0;
+#define B_IIM42653_PWR_MGMT0_ACCEL_MODE_OFF       0x00
+#define B_IIM42653_PWR_MGMT0_ACCEL_MODE_LOW_NOISE 0x03
+#define B_IIM42653_PWR_MGMT0_GYRO_MODE_OFF        0x00
+#define B_IIM42653_PWR_MGMT0_GYRO_MODE_LOW_NOISE  0x03
+#define B_IIM42653_PWR_MGMT0_TEMP_DISABLE         0b1
+#define B_IIM42653_PWR_MGMT0_TEMP_ENABLE          0b0
+
+// GYRO_CONFIG0 [Configuration, Sensor FSR/ODR]
+#define R_IIM42653_GYRO_CONFIG0 0x4F
+typedef union {
+    struct {
+        uint8_t GYRO_ODR :4;        //ODR, see datasheet for range
+        bool RESERVED :1;           //Reserved
+        uint8_t GYRO_UI_FS_SEL :3;  //FSR, see datasheet for range
+    } fields;
+    uint8_t data;
+} IIM42653_GYRO_CONFIG0;
+#define B_IIM42653_GYRO_CONFIG0_ODR_100HZ   0x08
+#define B_IIM42653_GYRO_CONFIG0_ODR_200HZ   0x07
+#define B_IIM42653_GYRO_CONFIG0_ODR_500HZ   0x0F
+#define B_IIM42653_GYRO_CONFIG0_FSR_4000DPS 0x00
+#define B_IIM42653_GYRO_CONFIG0_FSR_250DPS  0x04
+
+// ACCEL_CONFIG0 [Configuration, Sensor FSR/ODR]
+#define R_IIM42653_ACCEL_CONFIG0 0x50
+typedef union {
+    struct {
+        uint8_t ACCEL_ODR :4;		//ODR, see datasheet for range
+        bool RESERVED :1;			//Reserved
+        uint8_t ACCEL_UI_FS_SEL :3;	//FSR, see datasheet for range
+    } fields;
+    uint8_t data;
+} IIM42653_ACCEL_CONFIG0;
+#define B_IIM42653_ACCEL_CONFIG0_ODR_100HZ 0x08
+#define B_IIM42653_ACCEL_CONFIG0_ODR_200HZ 0x07
+#define B_IIM42653_ACCEL_CONFIG0_ODR_500HZ 0x0F
+#define B_IIM42653_ACCEL_CONFIG0_FSR_32G   0x00
+#define B_IIM42653_ACCEL_CONFIG_FSR_4G     0x03
+
+// WHO_AM_I [Validation] 
+#define R_IIM42653_WHO_AM_I 0x75
+#define B_IIM42653_WHO_AM_I_VALUE 0x56
+
+#define IIM42653_SAMPLE_RATE_HZ 500
+
+class IIM42653 {
+	public: 
+        IIM42653(i2c_inst_t* i2c) : i2c {i2c} {};
+
+        void initialize();
+        
+        void calibrate_gyro();
+
+        void sample();
+        
+        void apply_gyro_offset();
+
+        int16_t get_ax() { return ax; }
+        int16_t get_ay() { return ay; }
+        int16_t get_az() { return az; }
+        int16_t get_gx() { return gx; }
+        int16_t get_gy() { return gy; }
+        int16_t get_gz() { return gz; }
+
+        static float scale_accel(int16_t unscaled) { return ((float) unscaled) / S_IIM42653_ACCEL_SENSITIVITY_FACTOR; }
+        static float scale_gyro(int16_t unscaled) { return ((float) unscaled) / S_IIM42653_GYRO_SENSITIVITY_FACTOR; }
+
+#if ( USE_FREERTOS == 1 )
+        static void update_iim42653_task(void* pvParameters);
+
+        TaskHandle_t update_task_handle = NULL;
+#endif
+
+	private:
+        static int16_t sat_sub(int16_t a, int16_t b);
+        
+        const uint8_t addr = IIM42653_I2C_ADDR;
+
+        i2c_inst_t* i2c;
+
+        uint8_t buffer[16];
+
+        IIM42653_DEVICE_CONFIG device_config;
+        IIM42653_DRIVE_CONFIG drive_config;
+        IIM42653_PWR_MGMT0 pwr_mgmt0;
+        IIM42653_GYRO_CONFIG0 gyro_config0;
+        IIM42653_ACCEL_CONFIG0 accel_config0;
+
+        //Internal data fields
+        int16_t bias_gx = 0, bias_gy = 0, bias_gz = 0;
+        int16_t raw_gx = 0, raw_gy = 0, raw_gz = 0;
+        int16_t ax, ay, az, gx, gy, gz;
+
+        const int64_t n_gyro_bias_readings = 50;
+};