From 2998e2d15b6098be4c272b187af9438d95b140cf Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 22 Jul 2014 22:19:16 +0200 Subject: [PATCH] some cleaning of imu --- MadgwickAHRS.c | 228 +++++++++++++------------- MadgwickAHRS.h | 45 ++++-- Makefile | 1 - i2c_helper.c | 55 ------- i2c_helper.h | 8 - imu.c | 117 +++++++------- imu.h | 83 ++++++++++ main.c | 6 +- mpu6050.c | 430 +++++++++++++++++++++++-------------------------- mpu6050.h | 179 +++++--------------- mpu6050_regs.h | 116 +++++++++++++ vector.h | 2 +- 12 files changed, 644 insertions(+), 626 deletions(-) delete mode 100644 i2c_helper.c delete mode 100644 i2c_helper.h create mode 100644 imu.h create mode 100644 mpu6050_regs.h diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c index 240470f..5f03d6f 100644 --- a/MadgwickAHRS.c +++ b/MadgwickAHRS.c @@ -1,6 +1,25 @@ -//===================================================================================================== +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2011-2012, SOH Madgwick + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +//============================================================================ // MadgwickAHRS.c -//===================================================================================================== +//============================================================================ // // Implementation of Madgwick's IMU and AHRS algorithms. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms @@ -10,76 +29,73 @@ // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised // -//===================================================================================================== - -//--------------------------------------------------------------------------------------------------- -// Header files +//============================================================================ #include "MadgwickAHRS.h" #include - -//--------------------------------------------------------------------------------------------------- -// Definitions - //#define sampleFreq 512.0f // sample frequency in Hz //#define sampleFreq 46.0f // sample frequency in Hz #define sampleFreq 85.0f // sample frequency in Hz #define betaDef 0.1f // 2 * proportional gain -//--------------------------------------------------------------------------------------------------- -// Variable definitions - volatile float beta = betaDef; // 2 * proportional gain (Kp) volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame -//--------------------------------------------------------------------------------------------------- -// Function declarations - -float invSqrt(float x); - -//==================================================================================================== -// Functions - -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update +static float invSqrt(float x) +{ + return 1.0f / sqrtf(x); +} -void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { +/* AHRS algorithm update */ +void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) +{ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz; + float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3; + float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + float mx, my, mz, ax, ay, az; + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in + * magnetometer normalisation) */ + if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) { + MadgwickAHRSupdateIMU(imu, quat); return; } - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic + /* Rate of change of quaternion from gyroscope */ + qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); + qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); + qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); + qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN + * in accelerometer normalisation) */ + if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { + + /* Normalise accelerometer measurement */ + recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + + imu->az * imu->az); + ax = imu->ax * recipNorm; + ay = imu->ay * recipNorm; + az = imu->az * recipNorm; + + /* Normalise magnetometer measurement */ + recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my + + imu->mz * imu->mz); + mx = imu->mx * recipNorm; + my = imu->my * recipNorm; + mz = imu->mz * recipNorm; + + /* Auxiliary variables to avoid repeated arithmetic */ _2q0mx = 2.0f * q0 * mx; _2q0my = 2.0f * q0 * my; _2q0mz = 2.0f * q0 * mz; @@ -101,7 +117,7 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float q2q3 = q2 * q3; q3q3 = q3 * q3; - // Reference direction of Earth's magnetic field + /* Reference direction of Earth's magnetic field */ hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; _2bx = sqrt(hx * hx + hy * hy); @@ -109,64 +125,70 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; - // Gradient decent algorithm corrective step + /* Gradient decent algorithm corrective step */ s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + + /* normalize step magnitude */ + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; - // Apply feedback step + /* Apply feedback step */ qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; } - // Integrate rate of change of quaternion to yield quaternion + /* Integrate rate of change of quaternion to yield quaternion */ q0 += qDot1 * (1.0f / sampleFreq); q1 += qDot2 * (1.0f / sampleFreq); q2 += qDot3 * (1.0f / sampleFreq); q3 += qDot4 * (1.0f / sampleFreq); - // Normalise quaternion + /* Normalise quaternion */ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; + quat->q0 = q0 * recipNorm; + quat->q1 = q1 * recipNorm; + quat->q2 = q2 * recipNorm; + quat->q3 = q3 * recipNorm; } -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { +/* IMU algorithm update (does not take magneto in account) */ +void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat) +{ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + float ax, ay, az; + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + /* Rate of change of quaternion from gyroscope */ + qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); + qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); + qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); + qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */ + if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; + /* Normalise accelerometer measurement */ + recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az); + ax = imu->ax * recipNorm; + ay = imu->ay * recipNorm; + az = imu->az * recipNorm; - // Auxiliary variables to avoid repeated arithmetic + /* Auxiliary variables to avoid repeated arithmetic */ _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; @@ -181,65 +203,37 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo q2q2 = q2 * q2; q3q3 = q3 * q3; - - // Gradient decent algorithm corrective step + /* Gradient decent algorithm corrective step */ s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - - + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); /* normalise step magnitude */ s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; - - // Apply feedback step + /* Apply feedback step */ qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; } - // Integrate rate of change of quaternion to yield quaternion + /* Integrate rate of change of quaternion to yield quaternion */ q0 += qDot1 * (1.0f / sampleFreq); q1 += qDot2 * (1.0f / sampleFreq); q2 += qDot3 * (1.0f / sampleFreq); q3 += qDot4 * (1.0f / sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2); - + /* Normalise quaternion */ + recipNorm = invSqrt(q0 * q0 + q1 * q1 + + q2 * q2 + q3 * q3); + quat->q0 = q0 * recipNorm; + quat->q1 = q1 * recipNorm; + quat->q2 = q2 * recipNorm; + quat->q3 = q3 * recipNorm; } - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -/* -float invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} -*/ -float invSqrt(float x) { - return 1.0f / sqrtf(x); -} -//==================================================================================================== -// END OF CODE -//==================================================================================================== diff --git a/MadgwickAHRS.h b/MadgwickAHRS.h index 18e19fa..018f34a 100644 --- a/MadgwickAHRS.h +++ b/MadgwickAHRS.h @@ -1,6 +1,25 @@ -//===================================================================================================== +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2011, SOH Madgwick + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +//============================================================================= // MadgwickAHRS.h -//===================================================================================================== +//============================================================================= // // Implementation of Madgwick's IMU and AHRS algorithms. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms @@ -9,25 +28,19 @@ // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // -//===================================================================================================== +//============================================================================= + #ifndef MadgwickAHRS_h #define MadgwickAHRS_h -//---------------------------------------------------------------------------------------------------- -// Variable declaration +#include "imu.h" -extern volatile float beta; // algorithm gain -extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame +extern volatile float beta; // algorithm gain -//--------------------------------------------------------------------------------------------------- -// Function declarations +/* update quaternion structure using the new IMU infos */ +void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat); -void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); -void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); -void Mad_f32_init(void); -void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az); +/* update quaternion structure using the new IMU infos, without using magneto */ +void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat); #endif -//===================================================================================================== -// End of file -//===================================================================================================== diff --git a/Makefile b/Makefile index 91a2763..4ab55ad 100644 --- a/Makefile +++ b/Makefile @@ -12,7 +12,6 @@ SRC += i2cm_sw.c SRC += imu.c SRC += mpu6050.c SRC += MadgwickAHRS.c -SRC += i2c_helper.c SRC += byteordering.c SRC += fat.c SRC += sd_main.c diff --git a/i2c_helper.c b/i2c_helper.c deleted file mode 100644 index 255aaec..0000000 --- a/i2c_helper.c +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include - -#include - -#include -#include - -#include "i2cm_sw.h" - -uint8_t read_reg(uint8_t address_dev, uint8_t address_reg, uint8_t * value) -{ - uint8_t err = 0; - err = i2cm_send(address_dev, &address_reg, 1); - if (err) { - printf("read reg: i2c error send\r\n"); - return err; - } - err = i2cm_recv(address_dev, 1); - if (err) { - printf("read reg: i2c error recv\r\n"); - return err; - } - err = i2cm_get_recv_buffer(value, 1); - if (err != 1) { - printf("read reg: i2c error get recv\r\n"); - return 0xff; - } - return 0; - -} - - -uint8_t read_reg_len(uint8_t address_dev, uint8_t address_reg, - uint8_t *values, uint8_t len) -{ - uint8_t err = 0; - err = i2cm_send(address_dev, &address_reg, 1); - if (err) { - printf("read reg len: i2c error send\r\n"); - return err; - } - err = i2cm_recv(address_dev, len); - if (err) { - printf("read reg len: i2c error recv\r\n"); - return err; - } - err = i2cm_get_recv_buffer(values, len); - if (err != len) { - printf("read reg len: i2c error get recv\r\n"); - return 0xFF; - } - return 0; - -} diff --git a/i2c_helper.h b/i2c_helper.h deleted file mode 100644 index 57f16df..0000000 --- a/i2c_helper.h +++ /dev/null @@ -1,8 +0,0 @@ - -#ifndef _I2C_HELPER_H_ -#define _I2C_HELPER_H_ - -uint8_t read_reg(uint8_t address_dev, uint8_t address_reg, uint8_t * value); -uint8_t read_reg_len(uint8_t address_dev, uint8_t address_reg, uint8_t * value, uint8_t len); - -#endif // _I2C_HELPER_H_ diff --git a/imu.c b/imu.c index 0833a81..0c41556 100644 --- a/imu.c +++ b/imu.c @@ -8,7 +8,6 @@ #include -#include "i2c_helper.h" #include "mpu6050.h" #include "vector.h" #include "matrix.h" @@ -17,40 +16,42 @@ #include "imu.h" #include "main.h" -static struct callout imu_timer; +/* structure storing the latest imu infos returned by the sensor */ +static struct imu_info g_imu; + +/* structure storing the latest quaternion */ +static struct quaternion g_quat; -double roll; -double pitch; -double yaw; +/* structure storing the latest euler position */ +static struct euler g_euler; + +/* periodical timer structure */ +static struct callout imu_timer; -void quaternion2euler(void) +static void quaternion2euler(const struct quaternion *quat, struct euler *euler) { - roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3); - pitch = -asinf(2.0f * (q1 * q3 - q0 * q2)); - yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3); + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; + + euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3), + q0*q0 - q1*q1 - q2*q2 + q3*q3); + euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2)); + euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), + q0*q0 + q1*q1 - q2*q2 - q3*q3); } +/* timer callback that polls IMU and does the calculation */ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) { - int16_t mpu6050_axes[10]; - (void)arg; - mpu6050_read_all_axes(mpu6050_axes); - - MadgwickAHRSupdate(mpu6050_gx, - mpu6050_gy, - mpu6050_gz, - mpu6050_ax, - mpu6050_ay, - mpu6050_az, - mpu6050_mx, - mpu6050_my, - mpu6050_mz - ); - - quaternion2euler(); + mpu6050_read_all_axes(&g_imu); + MadgwickAHRSupdate(&g_imu, &g_quat); + quaternion2euler(&g_quat, &g_euler); + /* reschedule event */ callout_schedule(cm, tim, 2); } @@ -64,20 +65,21 @@ void imu_init(void) void imu_get_info(struct imu_info *imu) { - imu->gx = mpu6050_gx; - imu->gy = mpu6050_gy; - imu->gz = mpu6050_gz; - imu->ax = mpu6050_ax; - imu->ay = mpu6050_ay; - imu->az = mpu6050_az; - imu->mx = mpu6050_mx; - imu->my = mpu6050_my; - imu->mz = mpu6050_mz; - imu->temp = 0; + memcpy(imu, &g_imu, sizeof(*imu)); } +void imu_get_pos_quat(struct quaternion *quat) +{ + memcpy(quat, &g_quat, sizeof(*quat)); +} -int imu_loop(void) +void imu_get_pos_euler(struct euler *euler) +{ + memcpy(euler, &g_euler, sizeof(*euler)); +} + + +int imu_log(void) { char buf[128]; int16_t len; @@ -85,29 +87,26 @@ int imu_loop(void) uint8_t flags; struct imu_info imu; - while (1) { - - IRQ_LOCK(flags); - ms = global_ms; - imu_get_info(&imu); - IRQ_UNLOCK(flags); - - if (sd_log_enabled()) { - len = snprintf(buf, sizeof(buf), - "%"PRIu32"\t" - "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t" - "accel %+3.3f\t%+3.3f\t%+3.3f\t\t" - "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n", - ms, - imu.gx, imu.gy, imu.gz, - imu.ax, imu.ay, imu.az, - imu.mx, imu.my, imu.mz); - if (sd_log_write(buf, len) != len) { - printf_P(PSTR("error writing to file\n")); - return -1; - } - } - + if (sd_log_enabled() == 0) + return 0; + + IRQ_LOCK(flags); + ms = global_ms; + imu_get_info(&imu); + IRQ_UNLOCK(flags); + + len = snprintf(buf, sizeof(buf), + "%"PRIu32"\t" + "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t" + "accel %+3.3f\t%+3.3f\t%+3.3f\t\t" + "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n", + ms, + imu.gx, imu.gy, imu.gz, + imu.ax, imu.ay, imu.az, + imu.mx, imu.my, imu.mz); + if (sd_log_write(buf, len) != len) { + printf_P(PSTR("error writing to file\n")); + return -1; } return 0; diff --git a/imu.h b/imu.h new file mode 100644 index 0000000..06c03a1 --- /dev/null +++ b/imu.h @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef IMU_H_ +#define IMU_H_ + +struct imu_info { + /* gyro */ + double gx; + double gy; + double gz; + + /* accelero */ + double ax; + double ay; + double az; + + /* magneto */ + double mx; + double my; + double mz; + + /* temperature */ + double temp; +}; + +struct quaternion { + double q0; + double q1; + double q2; + double q3; +}; + +struct euler { + double roll; + double pitch; + double yaw; +}; + +/* initialize the IMU */ +void imu_init(void); + +/* if sd log file is opened, log the status of IMU on the sdcard */ +int imu_log(void); + +/* return a filled imu_info structure corresponding to the latest axes read in + * the timer callback. Does not lock irq, so it's up to the user to do that. */ +void imu_get_info(struct imu_info *imu); + +/* return the latest position in a quaternion struct read in the timer + * callback. Does not lock irq, so it's up to the user to do that. */ +void imu_get_pos_quat(struct quaternion *pos); + +/* return the latest position in an euler struct read in the timer + * callback. Does not lock irq, so it's up to the user to do that. */ +void imu_get_pos_euler(struct euler *pos); + +#endif diff --git a/main.c b/main.c index a1d6af1..8f0f7ed 100644 --- a/main.c +++ b/main.c @@ -167,18 +167,18 @@ int main(void) /* communication with mpu6050 */ i2cm_init(); - /* i2c hw with mainboard */ + /* i2c hw to communicate with mainboard */ i2c_init(I2C_MODE_SLAVE, I2C_IMUBOARD_ADDR); i2c_protocol_init(); i2c_register_recv_event(i2c_recvevent); i2c_register_send_event(i2c_sendevent); - sei(); - eeprom_load_config(); sd_log_open(); + sei(); + printf_P(PSTR("\r\n")); rdline_newline(&imuboard.rdl, imuboard.prompt); diff --git a/mpu6050.c b/mpu6050.c index 4dda84b..2966ee4 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -1,19 +1,41 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + #include #include -#include - #include -#include - -#include -#include - -#include "i2c_helper.h" +#include "i2cm_sw.h" #include "mpu6050.h" +#include "mpu6050_regs.h" -#define ToRad(x) ((x)*0.01745329252) // *pi/180 +#define ToRad(x) ((x) * 0.01745329252) // *pi/180 // fs_sel = 1 // 500°/s @@ -31,95 +53,96 @@ #define accel_y_resolution 2048.0 #define accel_z_resolution 2048.0 - - - -#define MPU6050_OFF_MSB 1 -#define MPU6050_OFF_LSB 0 - - #define MPU6050_ADDRESS 0x69 #define MPU6050_MAGNETO_ADDRESS 0x0D #define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff)) -double mpu6050_gx; -double mpu6050_gy; -double mpu6050_gz; +int16_t drift_g[3] = {0, 0, 0}; -double mpu6050_ax; -double mpu6050_ay; -double mpu6050_az; +/* read "len" bytes of mpu6050 registers starting at specified address */ +static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr, + uint8_t *values, uint8_t len) +{ + uint8_t err = 0; -double mpu6050_mx; -double mpu6050_my; -double mpu6050_mz; + err = i2cm_send(i2c_addr, ®_addr, 1); + if (err) { + printf("read reg len: i2c error send\r\n"); + return err; + } -double mpu6050_temp; + err = i2cm_recv(i2c_addr, len); + if (err) { + printf("read reg len: i2c error recv\r\n"); + return err; + } -int16_t drift_g[3] = {0, 0, 0}; + err = i2cm_get_recv_buffer(values, len); + if (err != len) { + printf("read reg len: i2c error get recv\r\n"); + return 0xFF; + } + return 0; +} -uint8_t mpu6050_read_gyro_raw(int16_t *values) +/* read one byte of mpu6050 register at specified address */ +static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr, + uint8_t *value) { - uint8_t err; - uint8_t i; - err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3); - for (i=0;i<3; i++) { - values[i] = SWAP_16(values[i]); - } - - return err; + return read_reg_len(i2c_addr, reg_addr, value, 1);; } -uint8_t mpu6050_read_all_axes(int16_t *values) +/* fill the axes[3] pointer with the 3 axes of gyro (16bits) */ +uint8_t mpu6050_read_gyro_raw(int16_t *axes) { uint8_t err; uint8_t i; - err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10); - for (i=0;i<7; i++) { - values[i] = SWAP_16(values[i]); + err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H, + (uint8_t *)axes, sizeof(int16_t) * 3); + for (i = 0; i < 3; i++) { + axes[i] = SWAP_16(axes[i]); } - mpu6050_ax = 9.81 * (double)values[0] / accel_x_resolution ; - mpu6050_ay = 9.81 * (double)values[1] / accel_y_resolution ; - mpu6050_az = 9.81 * (double)values[2] / accel_z_resolution ; - - mpu6050_temp = (double)values[3]/340. + 36.5; - - mpu6050_gx = ToRad((double)(values[4] - drift_g[0]) / gyro_x_resolution ); - mpu6050_gy = ToRad((double)(values[5] - drift_g[1]) / gyro_y_resolution ); - mpu6050_gz = ToRad((double)(values[6] - drift_g[2]) / gyro_z_resolution ); - - mpu6050_mx = (double) values[7] * 0.3; - mpu6050_my = (double) values[8] * 0.3; - mpu6050_mz = (double) values[9] * 0.3; - - return err; } +/* fill the imu structure with axes comming from mpu6050 */ +uint8_t mpu6050_read_all_axes(struct imu_info *imu) +{ + int16_t axes[10]; + uint8_t err; + uint8_t i; + err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, + (uint8_t *)axes, sizeof(axes)); + for (i = 0; i < 7; i++) { + axes[i] = SWAP_16(axes[i]); + } + imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ; + imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ; + imu->az = 9.81 * (double)axes[2] / accel_z_resolution ; -uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) -{ - uint8_t err; - uint8_t buffer[2]; + imu->temp = (double)axes[3]/340. + 36.5; - buffer[0] = reg; - buffer[1] = val; + imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution); + imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution); + imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution); - err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC); - if (err) - printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); + imu->mx = (double) axes[7] * 0.3; + imu->my = (double) axes[8] * 0.3; + imu->mz = (double) axes[9] * 0.3; return err; } -uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) + +/* write a 8bits value at the specified register of the mpu6050 */ +static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) { uint8_t err; uint8_t buffer[2]; @@ -128,9 +151,9 @@ uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) buffer[0] = reg; buffer[1] = val; - err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC); + err = i2cm_send(address, (unsigned char*)buffer, 2); if (err) - printf("send_check_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); + printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err); err = read_reg(address, reg, &check); if (err) @@ -144,8 +167,8 @@ uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val) return err; } - -void mpu6050_compute_drift(void) +/* XXX add comment */ +static void mpu6050_compute_drift(void) { uint16_t i; int32_t s_gx, s_gy, s_gz; @@ -178,8 +201,7 @@ void mpu6050_compute_drift(void) drift_g[2]); } - -uint8_t setup_mpu9150_magneto(void) +static uint8_t setup_mpu9150_magneto(void) { //uint8_t i, err; @@ -188,49 +210,48 @@ uint8_t setup_mpu9150_magneto(void) /* XXX doesn't work, no answer from magneto */ /* for (i = 0; i < 127; i++) { */ /* printf("i=%d\r\n", i); */ - /* err = send_check_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */ + /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */ /* if (err == 0) */ /* printf("COIN\r\n"); */ /* } */ - send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode - send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest - send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode + send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode + send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest + send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0 - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C - //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts - //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1 - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80); - - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode - send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4 + send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0 + send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C + //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts + send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts + //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable + send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable + send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C + send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts + send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1 + send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register + send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate + send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80); + + send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay + send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register + send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting + send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register + send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode + send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4 return 0; } -uint8_t mpu6050_init(void) +int8_t mpu6050_init(void) { uint8_t err; uint8_t id = 0; - //uint8_t config = 0; - wait_ms(1000); + wait_ms(1000); /* XXX needed ? */ - while(1) { + while (1) { /* XXX timeout */ err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id); if (err) continue; @@ -238,141 +259,90 @@ uint8_t mpu6050_init(void) break; } - /* use one of the gyro for clock ref: if we don't do that, some i2c commands fail ?? */ - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); - - //Sets sample rate to 1000/1+1 = 500Hz - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); - //Disable FSync, 48Hz DLPF - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03); - //Disable gyro self tests, scale of 500 degrees/s - //send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); - //Disable gyro self tests, scale of 2000 degrees/s - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000); - - //Disable accel self tests, scale of +-16g, no DHPF - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000); - - //Freefall threshold of <|0mg| - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); - //Freefall duration limit of 0 - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); - //Motion threshold of >0mg - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); - //Motion duration of >0s - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); - //Zero motion threshold - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); - //Zero motion duration threshold - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); - //Disable sensor output to FIFO buffer - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); - - //AUX I2C setup - //Sets AUX I2C to single master control, plus other config - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00); - //Setup AUX I2C slaves - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00); - - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00); - - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00); - - - //MPU6050_RA_I2C_MST_STATUS //Read-only - //Setup INT pin and AUX I2C pass through - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); - //Enable data ready interrupt - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00); - - //MPU6050_RA_DMP_INT_STATUS //Read-only - //MPU6050_RA_INT_STATUS 3A //Read-only - //MPU6050_RA_ACCEL_XOUT_H //Read-only - //MPU6050_RA_ACCEL_XOUT_L //Read-only - //MPU6050_RA_ACCEL_YOUT_H //Read-only - //MPU6050_RA_ACCEL_YOUT_L //Read-only - //MPU6050_RA_ACCEL_ZOUT_H //Read-only - //MPU6050_RA_ACCEL_ZOUT_L //Read-only - //MPU6050_RA_TEMP_OUT_H //Read-only - //MPU6050_RA_TEMP_OUT_L //Read-only - //MPU6050_RA_GYRO_XOUT_H //Read-only - //MPU6050_RA_GYRO_XOUT_L //Read-only - //MPU6050_RA_GYRO_YOUT_H //Read-only - //MPU6050_RA_GYRO_YOUT_L //Read-only - //MPU6050_RA_GYRO_ZOUT_H //Read-only - //MPU6050_RA_GYRO_ZOUT_L //Read-only - //MPU6050_RA_EXT_SENS_DATA_00 //Read-only - //MPU6050_RA_EXT_SENS_DATA_01 //Read-only - //MPU6050_RA_EXT_SENS_DATA_02 //Read-only - //MPU6050_RA_EXT_SENS_DATA_03 //Read-only - //MPU6050_RA_EXT_SENS_DATA_04 //Read-only - //MPU6050_RA_EXT_SENS_DATA_05 //Read-only - //MPU6050_RA_EXT_SENS_DATA_06 //Read-only - //MPU6050_RA_EXT_SENS_DATA_07 //Read-only - //MPU6050_RA_EXT_SENS_DATA_08 //Read-only - //MPU6050_RA_EXT_SENS_DATA_09 //Read-only - //MPU6050_RA_EXT_SENS_DATA_10 //Read-only - //MPU6050_RA_EXT_SENS_DATA_11 //Read-only - //MPU6050_RA_EXT_SENS_DATA_12 //Read-only - //MPU6050_RA_EXT_SENS_DATA_13 //Read-only - //MPU6050_RA_EXT_SENS_DATA_14 //Read-only - //MPU6050_RA_EXT_SENS_DATA_15 //Read-only - //MPU6050_RA_EXT_SENS_DATA_16 //Read-only - //MPU6050_RA_EXT_SENS_DATA_17 //Read-only - //MPU6050_RA_EXT_SENS_DATA_18 //Read-only - //MPU6050_RA_EXT_SENS_DATA_19 //Read-only - //MPU6050_RA_EXT_SENS_DATA_20 //Read-only - //MPU6050_RA_EXT_SENS_DATA_21 //Read-only - //MPU6050_RA_EXT_SENS_DATA_22 //Read-only - //MPU6050_RA_EXT_SENS_DATA_23 //Read-only - //MPU6050_RA_MOT_DETECT_STATUS //Read-only - - //Slave out, dont care - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00); - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00); - - //More slave config - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); - //Reset sensor signal paths - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00); - //Motion detection control - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00); - //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00); - //Sets clock source to gyro reference w/ PLL - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); - //Controls frequency of wakeups in accel low power mode plus the sensor standby modes - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); - //MPU6050_RA_BANK_SEL //Not in datasheet - //MPU6050_RA_MEM_START_ADDR //Not in datasheet - //MPU6050_RA_MEM_R_W //Not in datasheet - //MPU6050_RA_DMP_CFG_1 //Not in datasheet - //MPU6050_RA_DMP_CFG_2 //Not in datasheet - //MPU6050_RA_FIFO_COUNTH //Read-only - //MPU6050_RA_FIFO_COUNTL //Read-only - //Data transfer to and from the FIFO buffer - send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); - //MPU6050_RA_WHO_AM_I //Read-only, I2C address + /* XX use one of the gyro for clock ref: if we don't do that, some i2c + * commands fail... why ?? */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); + + /* Sets sample rate to 1000/1+1 = 500Hz */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); + /* Disable FSync, 48Hz DLPF */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03); + /* Disable gyro self tests, scale of 500 degrees/s */ + /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */ + /* Disable gyro self tests, scale of 2000 degrees/s */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000); + + /* Disable accel self tests, scale of +-16g, no DHPF */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000); + + /* Freefall threshold of <|0mg| */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00); + /* Freefall duration limit of 0 */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00); + /* Motion threshold of >0mg */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00); + /* Motion duration of >0s */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); + /* Zero motion threshold */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); + /* Zero motion duration threshold */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); + /* Disable sensor output to FIFO buffer */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); + + /* AUX I2C setup */ + /* Sets AUX I2C to single master control, plus other config */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00); + /* Setup AUX I2C slaves */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00); + + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00); + + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00); + + /* Setup INT pin and AUX I2C pass through */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); + /* Enable data ready interrupt */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00); + + /* Slave out, dont care */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00); + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00); + + /* More slave config */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00); + /* Reset sensor signal paths */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00); + /* Motion detection control */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00); + /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00); + /* Sets clock source to gyro reference w/ PLL */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010); + /* Controls frequency of wakeups in accel low power mode plus the sensor + * standby modes */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); + /* Data transfer to and from the FIFO buffer */ + send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00); setup_mpu9150_magneto(); - printf("MPU6050 Setup Complete\r\n"); mpu6050_compute_drift(); printf("MPU6050 drift computed\r\n"); diff --git a/mpu6050.h b/mpu6050.h index 327cd4a..3fabfa9 100644 --- a/mpu6050.h +++ b/mpu6050.h @@ -1,136 +1,43 @@ - -#ifndef _MPU6050_H_ -#define _MPU6050_H_ - - -#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN -#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN -#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN -#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS -#define MPU6050_RA_XA_OFFS_L_TC 0x07 -#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS -#define MPU6050_RA_YA_OFFS_L_TC 0x09 -#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS -#define MPU6050_RA_ZA_OFFS_L_TC 0x0B -#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR -#define MPU6050_RA_XG_OFFS_USRL 0x14 -#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR -#define MPU6050_RA_YG_OFFS_USRL 0x16 -#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR -#define MPU6050_RA_ZG_OFFS_USRL 0x18 -#define MPU6050_RA_SMPLRT_DIV 0x19 -#define MPU6050_RA_CONFIG 0x1A -#define MPU6050_RA_GYRO_CONFIG 0x1B -#define MPU6050_RA_ACCEL_CONFIG 0x1C -#define MPU6050_RA_FF_THR 0x1D -#define MPU6050_RA_FF_DUR 0x1E -#define MPU6050_RA_MOT_THR 0x1F -#define MPU6050_RA_MOT_DUR 0x20 -#define MPU6050_RA_ZRMOT_THR 0x21 -#define MPU6050_RA_ZRMOT_DUR 0x22 -#define MPU6050_RA_FIFO_EN 0x23 -#define MPU6050_RA_I2C_MST_CTRL 0x24 -#define MPU6050_RA_I2C_SLV0_ADDR 0x25 -#define MPU6050_RA_I2C_SLV0_REG 0x26 -#define MPU6050_RA_I2C_SLV0_CTRL 0x27 -#define MPU6050_RA_I2C_SLV1_ADDR 0x28 -#define MPU6050_RA_I2C_SLV1_REG 0x29 -#define MPU6050_RA_I2C_SLV1_CTRL 0x2A -#define MPU6050_RA_I2C_SLV2_ADDR 0x2B -#define MPU6050_RA_I2C_SLV2_REG 0x2C -#define MPU6050_RA_I2C_SLV2_CTRL 0x2D -#define MPU6050_RA_I2C_SLV3_ADDR 0x2E -#define MPU6050_RA_I2C_SLV3_REG 0x2F -#define MPU6050_RA_I2C_SLV3_CTRL 0x30 -#define MPU6050_RA_I2C_SLV4_ADDR 0x31 -#define MPU6050_RA_I2C_SLV4_REG 0x32 -#define MPU6050_RA_I2C_SLV4_DO 0x33 -#define MPU6050_RA_I2C_SLV4_CTRL 0x34 -#define MPU6050_RA_I2C_SLV4_DI 0x35 -#define MPU6050_RA_I2C_MST_STATUS 0x36 -#define MPU6050_RA_INT_PIN_CFG 0x37 -#define MPU6050_RA_INT_ENABLE 0x38 -#define MPU6050_RA_DMP_INT_STATUS 0x39 -#define MPU6050_RA_INT_STATUS 0x3A -#define MPU6050_RA_ACCEL_XOUT_H 0x3B -#define MPU6050_RA_ACCEL_XOUT_L 0x3C -#define MPU6050_RA_ACCEL_YOUT_H 0x3D -#define MPU6050_RA_ACCEL_YOUT_L 0x3E -#define MPU6050_RA_ACCEL_ZOUT_H 0x3F -#define MPU6050_RA_ACCEL_ZOUT_L 0x40 -#define MPU6050_RA_TEMP_OUT_H 0x41 -#define MPU6050_RA_TEMP_OUT_L 0x42 -#define MPU6050_RA_GYRO_XOUT_H 0x43 -#define MPU6050_RA_GYRO_XOUT_L 0x44 -#define MPU6050_RA_GYRO_YOUT_H 0x45 -#define MPU6050_RA_GYRO_YOUT_L 0x46 -#define MPU6050_RA_GYRO_ZOUT_H 0x47 -#define MPU6050_RA_GYRO_ZOUT_L 0x48 -#define MPU6050_RA_EXT_SENS_DATA_00 0x49 -#define MPU6050_RA_EXT_SENS_DATA_01 0x4A -#define MPU6050_RA_EXT_SENS_DATA_02 0x4B -#define MPU6050_RA_EXT_SENS_DATA_03 0x4C -#define MPU6050_RA_EXT_SENS_DATA_04 0x4D -#define MPU6050_RA_EXT_SENS_DATA_05 0x4E -#define MPU6050_RA_EXT_SENS_DATA_06 0x4F -#define MPU6050_RA_EXT_SENS_DATA_07 0x50 -#define MPU6050_RA_EXT_SENS_DATA_08 0x51 -#define MPU6050_RA_EXT_SENS_DATA_09 0x52 -#define MPU6050_RA_EXT_SENS_DATA_10 0x53 -#define MPU6050_RA_EXT_SENS_DATA_11 0x54 -#define MPU6050_RA_EXT_SENS_DATA_12 0x55 -#define MPU6050_RA_EXT_SENS_DATA_13 0x56 -#define MPU6050_RA_EXT_SENS_DATA_14 0x57 -#define MPU6050_RA_EXT_SENS_DATA_15 0x58 -#define MPU6050_RA_EXT_SENS_DATA_16 0x59 -#define MPU6050_RA_EXT_SENS_DATA_17 0x5A -#define MPU6050_RA_EXT_SENS_DATA_18 0x5B -#define MPU6050_RA_EXT_SENS_DATA_19 0x5C -#define MPU6050_RA_EXT_SENS_DATA_20 0x5D -#define MPU6050_RA_EXT_SENS_DATA_21 0x5E -#define MPU6050_RA_EXT_SENS_DATA_22 0x5F -#define MPU6050_RA_EXT_SENS_DATA_23 0x60 -#define MPU6050_RA_MOT_DETECT_STATUS 0x61 -#define MPU6050_RA_I2C_SLV0_DO 0x63 -#define MPU6050_RA_I2C_SLV1_DO 0x64 -#define MPU6050_RA_I2C_SLV2_DO 0x65 -#define MPU6050_RA_I2C_SLV3_DO 0x66 -#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 -#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 -#define MPU6050_RA_MOT_DETECT_CTRL 0x69 -#define MPU6050_RA_USER_CTRL 0x6A -#define MPU6050_RA_PWR_MGMT_1 0x6B -#define MPU6050_RA_PWR_MGMT_2 0x6C -#define MPU6050_RA_BANK_SEL 0x6D -#define MPU6050_RA_MEM_START_ADDR 0x6E -#define MPU6050_RA_MEM_R_W 0x6F -#define MPU6050_RA_DMP_CFG_1 0x70 -#define MPU6050_RA_DMP_CFG_2 0x71 -#define MPU6050_RA_FIFO_COUNTH 0x72 -#define MPU6050_RA_FIFO_COUNTL 0x73 -#define MPU6050_RA_FIFO_R_W 0x74 -#define MPU6050_RA_WHO_AM_I 0x75 - - -uint8_t mpu6050_init(void); -uint8_t mpu6050_read_all_axes(int16_t *values); - -extern double mpu6050_gx; -extern double mpu6050_gy; -extern double mpu6050_gz; - -extern double mpu6050_ax; -extern double mpu6050_ay; -extern double mpu6050_az; - - -extern double mpu6050_mx; -extern double mpu6050_my; -extern double mpu6050_mz; - -extern double mpu6050_temp; - -#endif // _MPU6050_H_ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of California, Berkeley nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MPU6050_H_ +#define MPU6050_H_ + +#include "imu.h" + +/* initialize the sensor, and calibrate it */ +int8_t mpu6050_init(void); + +/* fill the axes[3] pointer with the 3 axes of gyro (16bits) */ +uint8_t mpu6050_read_gyro_raw(int16_t *values); + +/* fill the imu structure with axes comming from mpu6050 */ +uint8_t mpu6050_read_all_axes(struct imu_info *imu); + +#endif /* MPU6050_H_ */ diff --git a/mpu6050_regs.h b/mpu6050_regs.h new file mode 100644 index 0000000..83b7e3a --- /dev/null +++ b/mpu6050_regs.h @@ -0,0 +1,116 @@ + +#ifndef _MPU6050_REGS_H_ +#define _MPU6050_REGS_H_ + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#endif // _MPU6050_REGS_H_ diff --git a/vector.h b/vector.h index 588def3..af7ec9b 100644 --- a/vector.h +++ b/vector.h @@ -4,6 +4,6 @@ float Vector_Dot_Product(float vector1[3],float vector2[3]); void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]); void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2); -void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]);; +void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]); #endif// _VECTOR_H_ -- 2.20.1