Added bias adjustments for gyroscrope

Propulsive-Landing · Dec 4, 2024 · d9b4e5f · d9b4e5f
1 parent c692591
commit d9b4e5f
Show file tree

Hide file tree

Showing 7 changed files with 95 additions and 5 deletions.
diff --git a/hardware/IMU.cpp b/hardware/IMU.cpp
@@ -26,6 +26,19 @@ IMU::IMU()
     ifstream.close();
 }
 
+void IMU::SetGyroBiasX(double x)
+{
+    gyroBiasX = x;
+}
+void IMU::SetGyroBiasY(double y)
+{
+    gyroBiasY = y;
+}
+void IMU::SetGyroBiasZ(double z)
+{
+    gyroBiasZ = z;
+}
+
 std::tuple<double, double, double> IMU::GetBodyAcceleration()
 {
     std::fstream ifstream(accelPath + "/in_accel_x_raw");
@@ -76,6 +89,6 @@ std::tuple<double, double, double> IMU::GetBodyAngularRate()
     ifstream >> nAnglVelZ;
     ifstream.close();
 
-    return std::make_tuple(nAnglVelX * 0.000266, -nAnglVelY * 0.000266, -nAnglVelZ * 0.000266);
+    return std::make_tuple(nAnglVelX * 0.000266+ gyroBiasX, -nAnglVelY * 0.000266 + gyroBiasY, -nAnglVelZ * 0.000266 + gyroBiasZ);
 
 }
diff --git a/hardware_simulation/IMU.cpp b/hardware_simulation/IMU.cpp
@@ -4,10 +4,24 @@
 
 IMU::IMU() {}
 
+
+void IMU::SetGyroBiasX(double x)
+{
+    gyroBiasX = x;
+}
+void IMU::SetGyroBiasY(double y)
+{
+    gyroBiasY = y;
+}
+void IMU::SetGyroBiasZ(double z)
+{
+    gyroBiasZ = z;
+}
+
 std::tuple<double, double, double> IMU::GetBodyAngularRate() {
     return UDPClient::GetInstance().GetBodyAngularRate();
 }
 
 std::tuple<double, double, double> IMU::GetBodyAcceleration() {
     return UDPClient::GetInstance().GetBodyAcceleration();
-}
+}
diff --git a/hardware_test/IMU.cpp b/hardware_test/IMU.cpp
@@ -15,3 +15,16 @@ std::tuple<double, double, double> IMU::GetBodyAcceleration()
 {
     return std::make_tuple(0,0,10.0);
 }
+
+void IMU::SetGyroBiasX(double x)
+{
+    gyroBiasX = x;
+}
+void IMU::SetGyroBiasY(double y)
+{
+    gyroBiasY = y;
+}
+void IMU::SetGyroBiasZ(double z)
+{
+    gyroBiasZ = z;
+}
diff --git a/include/IMU.hpp b/include/IMU.hpp
@@ -9,4 +9,16 @@ class IMU
         IMU();
         std::tuple<double, double, double> GetBodyAngularRate(); // Returns angular rate, p, q, and r in order
         std::tuple<double, double, double> GetBodyAcceleration(); // Returns linear acceleration, x, y, z
+        void SetGyroBiasX(double x);
+        void SetGyroBiasY(double y);
+        void SetGyroBiasZ(double z);
+        double gyroBiasX;
+        double gyroBiasY;
+        double gyroBiasZ;
+
+    // private:
+    //     double gyroBiasX;
+    //     double gyroBiasY;
+    //     double gyroBiasZ;
+
 };
diff --git a/include/Mode.hpp b/include/Mode.hpp
@@ -13,6 +13,7 @@ class Mode
         enum Phase
             {
                 Calibration,
+                GyroBiasOffset,
                 TestTVC,
                 Idle,
                 Launch,
@@ -23,11 +24,12 @@ class Mode
             };
 
         Mode(Mode::Phase eInitialMode);
-        bool Update(Navigation& navigation, Controller& controller, Igniter& igniter);
+        bool Update(Navigation& navigation, Controller& controller, Igniter& igniter, IMU& imu);
     private:
         Mode::Phase eCurrentMode;
 
         Mode::Phase UpdateCalibration(Navigation& navigation, Controller& controller, double currentTime);
+        Mode::Phase GetGyroBiasOffset(Navigation& navigation, Controller& controller, IMU& imu, double currentTime);
         Mode::Phase UpdateTestTVC(Navigation& navigation, Controller& controller, double currentTime);
         Mode::Phase UpdateIdle(Navigation& navigation, Controller& controller, double currentTime);
         Mode::Phase UpdateLaunch(Navigation& navigation, Controller& controller, Igniter& igniter, double current_time);

diff --git a/src/Main.cpp b/src/Main.cpp
@@ -52,7 +52,7 @@ int main()
 
 
 
-    while(mode.Update(navigation, controller, igniter)) {}
+    while(mode.Update(navigation, controller, igniter, imu)) {}
 
     //#ifdef NDEBUG
     //    gpioTerminate();

diff --git a/src/Mode.cpp b/src/Mode.cpp
@@ -89,6 +89,38 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll
     return Mode::Calibration;
 }
 
+Mode::Phase Mode::GetGyroBiasOffset(Navigation& navigation, Controller& controller, IMU& imu, double currentTime)
+{
+    static int loops = 1;
+    std::tuple<double,double,double> gyro = imu.GetBodyAngularRate();
+    static double gyro_x = 0;
+    static double gyro_y = 0;
+    static double gyro_z = 0;
+
+    gyro_x +=  std::get<0>(gyro);
+    gyro_y += std::get<1>(gyro);
+    gyro_z += std::get<2>(gyro);
+
+
+    if(loops == 100)
+    {
+        gyro_x /= loops;
+        gyro_y /= loops;
+        gyro_z /= loops;
+
+        imu.SetGyroBiasX(-gyro_x);
+        imu.SetGyroBiasY(-gyro_y);
+        imu.SetGyroBiasZ(-gyro_z);
+        return Mode::Idle;
+    }
+
+    ++loops;
+    return Mode::GyroBiasOffset;
+
+
+
+}
+
 Mode::Phase Mode::UpdateTestTVC(Navigation& navigation, Controller& controller, double currentTime) {
 
 
@@ -233,7 +265,7 @@ Mode::Phase Mode::UpdateSafeMode(Navigation& navigation, Controller& controller,
 }
 
 
-bool Mode::Update(Navigation& navigation, Controller& controller, Igniter& igniter) { 
+bool Mode::Update(Navigation& navigation, Controller& controller, Igniter& igniter, IMU& imu) { 
 
     // Track total elapsed time and delta time
     static double currentTime = 0;    
@@ -258,6 +290,10 @@ bool Mode::Update(Navigation& navigation, Controller& controller, Igniter& ignit
             // Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.05, 0.08);
             this->eCurrentMode = UpdateCalibration(navigation, controller, currentTime);
             break;
+        case GyroBiasOffset:
+            // Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.05, 0.08);
+            this->eCurrentMode = GetGyroBiasOffset(navigation, controller, imu, currentTime);
+            break;
         case TestTVC:
             Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.05, 0.08);
             this->eCurrentMode = UpdateTestTVC(navigation, controller, currentTime);