-//=====================================================================================================\r
+/*\r
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>\r
+ * Copyright (c) 2011-2012, SOH Madgwick\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ *\r
+ */\r
+\r
+//============================================================================\r
// MadgwickAHRS.c\r
-//=====================================================================================================\r
+//============================================================================\r
//\r
// Implementation of Madgwick's IMU and AHRS algorithms.\r
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load\r
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised\r
//\r
-//=====================================================================================================\r
-\r
-//---------------------------------------------------------------------------------------------------\r
-// Header files\r
+//============================================================================\r
\r
#include "MadgwickAHRS.h"\r
#include <math.h>\r
\r
-\r
-//---------------------------------------------------------------------------------------------------\r
-// Definitions\r
-\r
//#define sampleFreq 512.0f // sample frequency in Hz\r
//#define sampleFreq 46.0f // sample frequency in Hz\r
#define sampleFreq 85.0f // sample frequency in Hz\r
#define betaDef 0.1f // 2 * proportional gain\r
\r
-//---------------------------------------------------------------------------------------------------\r
-// Variable definitions\r
-\r
volatile float beta = betaDef; // 2 * proportional gain (Kp)\r
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame\r
\r
\r
-//---------------------------------------------------------------------------------------------------\r
-// Function declarations\r
-\r
-float invSqrt(float x);\r
-\r
-//====================================================================================================\r
-// Functions\r
-\r
-//---------------------------------------------------------------------------------------------------\r
-// AHRS algorithm update\r
+static float invSqrt(float x)\r
+{\r
+ return 1.0f / sqrtf(x);\r
+}\r
\r
-void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {\r
+/* AHRS algorithm update */\r
+void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat)\r
+{\r
float recipNorm;\r
float s0, s1, s2, s3;\r
float qDot1, qDot2, qDot3, qDot4;\r
float hx, hy;\r
- 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;\r
-\r
- // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)\r
- if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {\r
- MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);\r
+ float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz;\r
+ float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3;\r
+ float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;\r
+ float mx, my, mz, ax, ay, az;\r
+ float q0 = quat->q0;\r
+ float q1 = quat->q1;\r
+ float q2 = quat->q2;\r
+ float q3 = quat->q3;\r
+\r
+ /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in\r
+ * magnetometer normalisation) */\r
+ if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) {\r
+ MadgwickAHRSupdateIMU(imu, quat);\r
return;\r
}\r
\r
- // Rate of change of quaternion from gyroscope\r
- qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
- qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
- qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
- qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
-\r
- // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
- if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
-\r
- // Normalise accelerometer measurement\r
- recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
- ax *= recipNorm;\r
- ay *= recipNorm;\r
- az *= recipNorm;\r
-\r
- // Normalise magnetometer measurement\r
- recipNorm = invSqrt(mx * mx + my * my + mz * mz);\r
- mx *= recipNorm;\r
- my *= recipNorm;\r
- mz *= recipNorm;\r
-\r
- // Auxiliary variables to avoid repeated arithmetic\r
+ /* Rate of change of quaternion from gyroscope */\r
+ qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
+ qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
+ qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
+ qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
+\r
+ /* Compute feedback only if accelerometer measurement valid (avoids NaN\r
+ * in accelerometer normalisation) */\r
+ if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
+\r
+ /* Normalise accelerometer measurement */\r
+ recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay +\r
+ imu->az * imu->az);\r
+ ax = imu->ax * recipNorm;\r
+ ay = imu->ay * recipNorm;\r
+ az = imu->az * recipNorm;\r
+\r
+ /* Normalise magnetometer measurement */\r
+ recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my +\r
+ imu->mz * imu->mz);\r
+ mx = imu->mx * recipNorm;\r
+ my = imu->my * recipNorm;\r
+ mz = imu->mz * recipNorm;\r
+\r
+ /* Auxiliary variables to avoid repeated arithmetic */\r
_2q0mx = 2.0f * q0 * mx;\r
_2q0my = 2.0f * q0 * my;\r
_2q0mz = 2.0f * q0 * mz;\r
q2q3 = q2 * q3;\r
q3q3 = q3 * q3;\r
\r
- // Reference direction of Earth's magnetic field\r
+ /* Reference direction of Earth's magnetic field */\r
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;\r
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;\r
_2bx = sqrt(hx * hx + hy * hy);\r
_4bx = 2.0f * _2bx;\r
_4bz = 2.0f * _2bz;\r
\r
- // Gradient decent algorithm corrective step\r
+ /* Gradient decent algorithm corrective step */\r
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);\r
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);\r
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);\r
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);\r
- recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
+\r
+ /* normalize step magnitude */\r
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);\r
s0 *= recipNorm;\r
s1 *= recipNorm;\r
s2 *= recipNorm;\r
s3 *= recipNorm;\r
\r
- // Apply feedback step\r
+ /* Apply feedback step */\r
qDot1 -= beta * s0;\r
qDot2 -= beta * s1;\r
qDot3 -= beta * s2;\r
qDot4 -= beta * s3;\r
}\r
\r
- // Integrate rate of change of quaternion to yield quaternion\r
+ /* Integrate rate of change of quaternion to yield quaternion */\r
q0 += qDot1 * (1.0f / sampleFreq);\r
q1 += qDot2 * (1.0f / sampleFreq);\r
q2 += qDot3 * (1.0f / sampleFreq);\r
q3 += qDot4 * (1.0f / sampleFreq);\r
\r
- // Normalise quaternion\r
+ /* Normalise quaternion */\r
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
- q0 *= recipNorm;\r
- q1 *= recipNorm;\r
- q2 *= recipNorm;\r
- q3 *= recipNorm;\r
+ quat->q0 = q0 * recipNorm;\r
+ quat->q1 = q1 * recipNorm;\r
+ quat->q2 = q2 * recipNorm;\r
+ quat->q3 = q3 * recipNorm;\r
}\r
\r
-//---------------------------------------------------------------------------------------------------\r
-// IMU algorithm update\r
-\r
-void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {\r
+/* IMU algorithm update (does not take magneto in account) */\r
+void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat)\r
+{\r
float recipNorm;\r
float s0, s1, s2, s3;\r
float qDot1, qDot2, qDot3, qDot4;\r
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;\r
+ float ax, ay, az;\r
+ float q0 = quat->q0;\r
+ float q1 = quat->q1;\r
+ float q2 = quat->q2;\r
+ float q3 = quat->q3;\r
\r
- // Rate of change of quaternion from gyroscope\r
- qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
- qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
- qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
- qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
+ /* Rate of change of quaternion from gyroscope */\r
+ qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
+ qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
+ qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
+ qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
\r
\r
- // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
- if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
+ /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */\r
+ if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
\r
- // Normalise accelerometer measurement\r
- recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
- ax *= recipNorm;\r
- ay *= recipNorm;\r
- az *= recipNorm;\r
+ /* Normalise accelerometer measurement */\r
+ recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az);\r
+ ax = imu->ax * recipNorm;\r
+ ay = imu->ay * recipNorm;\r
+ az = imu->az * recipNorm;\r
\r
- // Auxiliary variables to avoid repeated arithmetic\r
+ /* Auxiliary variables to avoid repeated arithmetic */\r
_2q0 = 2.0f * q0;\r
_2q1 = 2.0f * q1;\r
_2q2 = 2.0f * q2;\r
q2q2 = q2 * q2;\r
q3q3 = q3 * q3;\r
\r
-\r
- // Gradient decent algorithm corrective step\r
+ /* Gradient decent algorithm corrective step */\r
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
- recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
-\r
-\r
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); /* normalise step magnitude */\r
\r
s0 *= recipNorm;\r
s1 *= recipNorm;\r
s2 *= recipNorm;\r
s3 *= recipNorm;\r
\r
-\r
- // Apply feedback step\r
+ /* Apply feedback step */\r
qDot1 -= beta * s0;\r
qDot2 -= beta * s1;\r
qDot3 -= beta * s2;\r
qDot4 -= beta * s3;\r
}\r
\r
- // Integrate rate of change of quaternion to yield quaternion\r
+ /* Integrate rate of change of quaternion to yield quaternion */\r
q0 += qDot1 * (1.0f / sampleFreq);\r
q1 += qDot2 * (1.0f / sampleFreq);\r
q2 += qDot3 * (1.0f / sampleFreq);\r
q3 += qDot4 * (1.0f / sampleFreq);\r
\r
-\r
- // Normalise quaternion\r
- recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
- q0 *= recipNorm;\r
- q1 *= recipNorm;\r
- q2 *= recipNorm;\r
- q3 *= recipNorm;\r
-\r
- //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2);\r
-\r
+ /* Normalise quaternion */\r
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 +\r
+ q2 * q2 + q3 * q3);\r
+ quat->q0 = q0 * recipNorm;\r
+ quat->q1 = q1 * recipNorm;\r
+ quat->q2 = q2 * recipNorm;\r
+ quat->q3 = q3 * recipNorm;\r
}\r
\r
-\r
-//---------------------------------------------------------------------------------------------------\r
-// Fast inverse square-root\r
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root\r
-/*\r
-float invSqrt(float x) {\r
- float halfx = 0.5f * x;\r
- float y = x;\r
- long i = *(long*)&y;\r
- i = 0x5f3759df - (i>>1);\r
- y = *(float*)&i;\r
- y = y * (1.5f - (halfx * y * y));\r
- return y;\r
-}\r
-*/\r
-float invSqrt(float x) {\r
- return 1.0f / sqrtf(x);\r
-}\r
-//====================================================================================================\r
-// END OF CODE\r
-//====================================================================================================\r
-//=====================================================================================================\r
+/*\r
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>\r
+ * Copyright (c) 2011, SOH Madgwick\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ *\r
+ */\r
+\r
+//=============================================================================\r
// MadgwickAHRS.h\r
-//=====================================================================================================\r
+//=============================================================================\r
//\r
// Implementation of Madgwick's IMU and AHRS algorithms.\r
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
// 29/09/2011 SOH Madgwick Initial release\r
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load\r
//\r
-//=====================================================================================================\r
+//=============================================================================\r
+\r
#ifndef MadgwickAHRS_h\r
#define MadgwickAHRS_h\r
\r
-//----------------------------------------------------------------------------------------------------\r
-// Variable declaration\r
+#include "imu.h"\r
\r
-extern volatile float beta; // algorithm gain\r
-extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame\r
+extern volatile float beta; // algorithm gain\r
\r
-//---------------------------------------------------------------------------------------------------\r
-// Function declarations\r
+/* update quaternion structure using the new IMU infos */\r
+void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat);\r
\r
-void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);\r
-void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);\r
-void Mad_f32_init(void);\r
-void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az);\r
+/* update quaternion structure using the new IMU infos, without using magneto */\r
+void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat);\r
\r
#endif\r
-//=====================================================================================================\r
-// End of file\r
-//=====================================================================================================\r
SRC += imu.c
SRC += mpu6050.c
SRC += MadgwickAHRS.c
-SRC += i2c_helper.c
SRC += byteordering.c
SRC += fat.c
SRC += sd_main.c
+++ /dev/null
-#include <stdio.h>
-#include <string.h>
-
-#include <timer.h>
-
-#include <aversive/wait.h>
-#include <uart.h>
-
-#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;
-
-}
+++ /dev/null
-
-#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_
#include <uart.h>
-#include "i2c_helper.h"
#include "mpu6050.h"
#include "vector.h"
#include "matrix.h"
#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);
}
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;
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;
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * 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
/* 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);
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * 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 <stdio.h>
#include <string.h>
-#include <timer.h>
-
#include <aversive/wait.h>
-#include <aversive/pgmspace.h>
-
-#include <uart.h>
-#include <i2c.h>
-
-#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
#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];
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)
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;
drift_g[2]);
}
-
-uint8_t setup_mpu9150_magneto(void)
+static uint8_t setup_mpu9150_magneto(void)
{
//uint8_t i, err;
/* 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;
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");
-
-#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 <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * 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_ */
--- /dev/null
+
+#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_
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_
git.droids-corp.org / protos / imu.git / commitdiff