imu: add mpu6050 calibration

Signed-off-by: Thomas Chou <[email protected]>
hippo5329 · May 8, 2024 · d3c03d2 · d3c03d2
1 parent 697f8cd
commit d3c03d2
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 0 deletions.
diff --git a/firmware/lib/imu/default_imu.h b/firmware/lib/imu/default_imu.h
@@ -126,6 +126,8 @@ class MPU6050IMU: public IMUInterface
             if(!ret)
                 return false;
 
+            accelgyro_.CalibrateAccel();
+            accelgyro_.CalibrateGyro();
             return true;
         }
 

diff --git a/firmware/lib/imu/imu_interface.h b/firmware/lib/imu/imu_interface.h
@@ -72,9 +72,11 @@ class IMUInterface
         sensor_msgs__msg__Imu getData()
         {
             imu_msg_.angular_velocity = readGyroscope();
+#ifndef USE_MPU6050_IMU // mpu6050 already calibrated in driver
             imu_msg_.angular_velocity.x -= gyro_cal_.x; 
             imu_msg_.angular_velocity.y -= gyro_cal_.y; 
             imu_msg_.angular_velocity.z -= gyro_cal_.z; 
+#endif
 
             if(imu_msg_.angular_velocity.x > -0.01 && imu_msg_.angular_velocity.x < 0.01 )
                 imu_msg_.angular_velocity.x = 0;