Use MadgwickAHRS (#13)

* AHRS Implementation

* Fix updateIMU bug

---------

Co-authored-by: Zhiquan Yeo <[email protected]>

Author: zhiquanyeo

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();
 

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