Use MadgwickAHRS (#13)

* AHRS Implementation * Fix updateIMU bug --------- Co-authored-by: Zhiquan Yeo <[email protected]>
wpilibsuite · Aug 16, 2023 · f3be79e · f3be79e
1 parent 97c0573
commit f3be79e
Show file tree

Hide file tree

Showing 4 changed files with 270 additions and 67 deletions.
diff --git a/include/imu.h b/include/imu.h
@@ -16,16 +16,24 @@ bool imuIsEnabled();
 void imuInit(uint8_t addr, TwoWire *theWire);
 void imuCalibrate(unsigned long calibrationTime);
 
-bool imuPeriodic();
+void imuPeriodic();
+bool imuDataReady();
 
-float gyroGetAngleX();
-float gyroGetRateX();
+float imuGetAccelX();
+float imuGetAccelY();
+float imuGetAccelZ();
 
-float gyroGetAngleY();
-float gyroGetRateY();
+float imuGetGyroRateX();
+float imuGetGyroRateY();
+float imuGetGyroRateZ();
 
-float gyroGetAngleZ();
-float gyroGetRateZ();
+float imuGetRoll();
+float imuGetPitch();
+float imuGetYaw();
+
+void imuResetRoll();
+void imuResetPitch();
+void imuResetYaw();
 
 void gyroReset();
 

diff --git a/platformio.ini b/platformio.ini
@@ -12,4 +12,5 @@ lib_deps =
     Wire
     adafruit/Adafruit LSM6DS@^4.7.0
     adafruit/Adafruit BusIO@^1.14.1
-    adafruit/Adafruit Unified Sensor@^1.1.9
+    adafruit/Adafruit Unified Sensor@^1.1.9
+    arduino-libraries/Madgwick@^1.2.0