From b50d69a2ae2c4e6a5e7bb235fc1e8ba2f39e1f14 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Mon, 11 Nov 2024 13:33:22 -0500
Subject: [PATCH] complimentary filter

---
 lib/LSM6/src/LSM6.cpp | 10 ++++++++--
 src/robot.cpp         | 22 ++++++++++++++++++++--
 2 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/lib/LSM6/src/LSM6.cpp b/lib/LSM6/src/LSM6.cpp
index 346f77d..9a6c124 100644
--- a/lib/LSM6/src/LSM6.cpp
+++ b/lib/LSM6/src/LSM6.cpp
@@ -31,7 +31,13 @@ bool LSM6::checkForNewData(void)
   bool retVal = false;
   if(getStatus() & 0x02)
   {
-    read();
+    readGyro();
+
+    retVal = true;
+  }
+  if(getStatus() & 0x01)
+  {
+    readAcc();
 
     retVal = true;
   }
@@ -232,7 +238,7 @@ void LSM6::enableDefault(void)
 
     // Set the accelerometer full scale and data rate
     setFullScaleAcc(ACC_FS2);
-    setAccDataOutputRate(ODR13);
+    setAccDataOutputRate(ODR104);
 
     // Auto-increment
     writeReg(CTRL3_C, 0x04);
diff --git a/src/robot.cpp b/src/robot.cpp
index 3c644d0..5b1682b 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -81,11 +81,29 @@ void Robot::HandleOrientationUpdate(void)
     else // update orientation
     {
         float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
-        z_rot /= 104;
-        z_rot *= 245;
+        z_rot /= 104.0;
+        z_rot *= 245.0;
         eulerAngles.z += z_rot;
+
     }
 
+    float g_lsb = 0.061 / 1000.;
+    float acc_x = asin(imu.a.x * g_lsb) * 180 / PI;
+    if (isnan(acc_x)) {
+            acc_x = 90; // pointing straight down will never occur
+    }
+    float gyrodelt_x = (float)imu.g.x / (float)INT_MAX;
+    gyrodelt_x *= 245.0 / 104.0;
+    float kappa = 0.1;
+    eulerAngles.x = (1-kappa) * (eulerAngles.x + gyrodelt_x) + kappa * acc_x;
+
+    float acc_y = asin(imu.a.y * g_lsb) * 180 / PI;
+    if (isnan(acc_y)) {
+            acc_y = 90; // pointing straight down will never occur
+    }
+    float gyrodelt_y = (float)imu.g.y / (float)INT_MAX;
+    gyrodelt_y *= 245.0 / 104.0;
+    eulerAngles.y = (1-kappa) * (eulerAngles.y + gyrodelt_y) + kappa * acc_y;
 #ifdef __IMU_DEBUG__
     Serial.print(imu.gyroBias.z);
     Serial.print('\t');