Test/state machine

Avionics/Firmware/include/IMU_Control.h

@@ -1,28 +1,58 @@
-// Firmware/include/IMU_Control.h
 #ifndef IMU_Control
 #define IMU_Control
 
-#include <Adafruit_ICM20948.h>
+#include <Wire.h>
+#include <ICM20948_WE.h>
+#define ICM20948_ADDR 0x68
 
-#define ICM_ADDR 0x68
-#define GYRO_CONFIG_1 0x01
-#define ACCEL_CONFIG 0x14
+// Initialize IMU (ICM20948)
+extern ICM20948_WE imu;
 
-//Initialize IMU (ICM20948)
-extern Adafruit_ICM20948 imu;
+// Kalman filter variables for Roll and Pitch
+extern float KalmanAngleRoll, KalmanUncertaintyAngleRoll;
+extern float KalmanAnglePitch, KalmanUncertaintyAnglePitch;
+extern float Kalman1DOutput[2]; // [estimated angle, uncertainty]
 
+// Time tracking for angle calculation from gyro data
+extern unsigned long previousTime;
+extern float dt;
+
+// Gyroscope calibration variables
+extern float RateCalibrationRoll;
+extern float RateCalibrationPitch;
+extern float RateCalibrationYaw;
+extern int calibrationSamples;
+
+// Offset variables for sensor calibration
 extern float gyro_x_offset;
 extern float gyro_y_offset;
 extern float gyro_z_offset;
 extern float accel_x_offset;
 extern float accel_y_offset;
 extern float accel_z_offset;
 
+// Function declarations
+bool initIMU(int maxRetries = 3);
 void configIMU();
+void calibrateGyroscope();
 void calibrateGyroAccel();
-bool initIMU(int maxRetries = 3);
-
-// Set low noise modes for both gyroscope and accelerometer
+void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement);
+void updateIMUWithKalman();
 void setLowNoiseMode();
 
+// Getter functions for filtered angles
+float getFilteredRoll();
+float getFilteredPitch();
+float getFilteredYaw(); // Raw yaw from gyro integration
+
+// Getter functions for raw sensor data
+void getRawAccelAngles(float &roll, float &pitch);
+void getRawGyroRates(float &x, float &y, float &z);
+
+// New getter functions for sensor data in formats compatible with existing code
+void getCalibratedAcceleration(float &x, float &y, float &z);
+void getCalibratedGyroscope(float &x, float &y, float &z);
+void getMagnetometer(float &x, float &y, float &z);
+float getTemperature();
+
 #endif //IMU_Control

Avionics/Firmware/include/flightdata.h

@@ -1,12 +1,19 @@
-// Firmware/include/flightdata.h
 #ifndef FLIGHTDATA_H
 #define FLIGHTDATA_H
 
-#include <Adafruit_ICM20X.h>
-#include <Adafruit_ICM20948.h>
-#include <Adafruit_Sensor.h>
+// Remove old Adafruit includes and replace with compatibility types
+// #include <Adafruit_ICM20X.h>
+// #include <Adafruit_ICM20948.h>
+// #include <Adafruit_Sensor.h>
 #include <ESPAsyncWebServer.h>
 
+// Define compatibility structure for sensors_vec_t (to maintain existing interface)
+typedef struct {
+    float x;
+    float y;
+    float z;
+} sensors_vec_t;
+
 /*
  * Data storage class:
  * - Holds most recent data from the IMU.
@@ -15,31 +22,26 @@
  */
 class FlightData
 {
-    private:
-        sensors_vec_t acceleration, gyroscope, magnetic;
-        float temperature;
-        unsigned long time;
-
-    public:
-        FlightData();
-
-        sensors_vec_t getAccel() const;
-        sensors_vec_t getGyro() const;
-        sensors_vec_t getMag() const;
-        float getTemp() const;
-
-        void update_values();
-        void print_values_arduino();
-        void save_values();
-        void serve_csv(WiFiClient& client);
-        int flightPhase;
+private:
+    sensors_vec_t acceleration, gyroscope, magnetic;
+    float temperature;
+    unsigned long time;
+public:
+    FlightData();
+    sensors_vec_t getAccel() const;
+    sensors_vec_t getGyro() const;
+    sensors_vec_t getMag() const;
+    float getTemp() const;
+    void update_values();
+    void print_values_arduino();
+    void save_values();
+    void serve_csv(WiFiClient& client);
+    int flightPhase;
 };
 
 // FlightData object used to store current data and access it
 extern FlightData currentData;
-
 extern unsigned long startTime;
-
 bool initialize_csv();
 
-#endif // FLIGHTDATA_H
+#endif // FLIGHTDATA_H