diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc index 17a2bf0..ffa4070 100644 --- a/main/ugv_io_mpu.cc +++ b/main/ugv_io_mpu.cc @@ -5,7 +5,7 @@ #include -constexpr float M_PI = atanf(1.f) * 4; +constexpr float M_PI = 3.1415926535897932384626433832795; #include "MPU.hpp" #include "i2c_mutex.h" @@ -22,8 +22,10 @@ static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); static const Vec3f ACCEL_OFFSET = {0., 0., 0.}; static const Mat3f ACCEL_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; -static const Vec3f MAG_OFFSET = {-7.79683, 3.6735, 32.3868}; -static const Mat3f MAG_MAT = {0., -0.0281408, 0., -0.0284409, 0., 0., 0., 0., 0.0261544}; +//static const Vec3f MAG_OFFSET = {-7.79683, 3.6735, 32.3868}; +static const Vec3f MAG_OFFSET = {0., 0., 0.}; +//static const Mat3f MAG_MAT = {0., -0.0281408, 0., -0.0284409, 0., 0., 0., 0., 0.0261544}; +static const Mat3f MAG_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; static const Mat3f GYRO_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; static const Vec3f GYRO_OFFSET = {0., 0., 0.};