From d07517b2e9c57bbd6de663f6fbae5e7ad65512c9 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Wed, 8 May 2019 21:57:44 -0700 Subject: [PATCH] fix mpu math stuff --- main/ugv_io_mpu.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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.};