diff --git a/examples/MPU6050_DMP6_ImuData_for_ROS/MPU6050_DMP6_ImuData_for_ROS.ino b/examples/MPU6050_DMP6_ImuData_for_ROS/MPU6050_DMP6_ImuData_for_ROS.ino new file mode 100644 index 0000000..3c47e20 --- /dev/null +++ b/examples/MPU6050_DMP6_ImuData_for_ROS/MPU6050_DMP6_ImuData_for_ROS.ino @@ -0,0 +1,122 @@ +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +//#include "MPU6050.h" // not necessary if using MotionApps include file + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include "Wire.h" +#endif + +MPU6050 mpu; + +#define EARTH_GRAVITY_MS2 9.80665 // m/s2 +#define DEG_TO_RAD 0.017453292519943295769236907684886 +#define RAD_TO_DEG 57.295779513082320876798154814105 + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 gg; // [x, y, z] gyro sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorInt16 ggWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +void setup() { + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + Serial.begin(115200); + + mpu.initialize(); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(-156); + mpu.setYGyroOffset(-11); + mpu.setZGyroOffset(-14); + mpu.setXAccelOffset(-3699); + mpu.setYAccelOffset(-2519); + mpu.setZAccelOffset(1391); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + mpu.PrintActiveOffsets(); + mpu.setDMPEnabled(true); + + mpuIntStatus = mpu.getIntStatus(); + + dmpReady = true; + packetSize = mpu.dmpGetFIFOPacketSize(); + } +} + +void loop() { + if (!dmpReady) return; + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet + + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); + + mpu.dmpGetGravity(&gravity, &q); + + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpConvertToWorldFrame(&aaWorld, &aa, &q); + Serial.print("aworld\t"); + Serial.print(aaWorld.x * mpu.get_acce_resolution() * EARTH_GRAVITY_MS2); + Serial.print("\t"); + Serial.print(aaWorld.y * mpu.get_acce_resolution() * EARTH_GRAVITY_MS2); + Serial.print("\t"); + Serial.println(aaWorld.z * mpu.get_acce_resolution() * EARTH_GRAVITY_MS2); + + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetGyro(&gg, fifoBuffer); + mpu.dmpConvertToWorldFrame(&ggWorld, &gg, &q); + Serial.print("ggWorld\t"); + Serial.print(ggWorld.x * mpu.get_gyro_resolution() * DEG_TO_RAD); + Serial.print("\t"); + Serial.print(ggWorld.y * mpu.get_gyro_resolution() * DEG_TO_RAD); + Serial.print("\t"); + Serial.println(ggWorld.z * mpu.get_gyro_resolution() * DEG_TO_RAD); + + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + Serial.print("ypr\t"); + Serial.print(ypr[0] * RAD_TO_DEG); + Serial.print("\t"); + Serial.print(ypr[1] * RAD_TO_DEG); + Serial.print("\t"); + Serial.println(ypr[2] * RAD_TO_DEG); + + Serial.println(); + + delay(100); + } +} \ No newline at end of file diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp index 8b817c7..957ec2c 100644 --- a/src/MPU6050.cpp +++ b/src/MPU6050.cpp @@ -36,6 +36,7 @@ THE SOFTWARE. =============================================== */ +#include "Arduino.h" #include "MPU6050.h" #if defined(ARDUINO_ARCH_MBED) #include "api/deprecated-avr-comp/avr/dtostrf.c.impl" @@ -63,11 +64,83 @@ MPU6050_Base::MPU6050_Base(uint8_t address, void *wireObj):devAddr(address), wir */ void MPU6050_Base::initialize() { setClockSource(MPU6050_CLOCK_PLL_XGYRO); + setFullScaleGyroRange(MPU6050_GYRO_FS_250); + gyroscopeResolution = 250.0 / 16384.0; + setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + accelerationResolution = 2.0 / 16384.0; + + setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! +} + +/** Power on and prepare for general usage. + * This will activate the device and take it out of sleep mode (which must be done + * after start-up). This function also sets both the accelerometer and the gyroscope + * to selected settings based on user perference. selection can be one of , + * + * Accelerometer: ACCEL_FS::A2G, ACCEL_FS::A4G, ACCEL_FS::A8G, ACCEL_FS::A16G, + * Gyroscope: GYRO_FS::G250DPS, GYRO_FS::G500DPS, GYRO_FS::G1000DPS, GYRO_FS::G2000DPS, + * + * the clock source to use the X Gyro for reference, which is slightly better than + * the default internal clock source. + */ +void MPU6050_Base::initialize(ACCEL_FS accelRange, GYRO_FS gyroRange) { + setClockSource(MPU6050_CLOCK_PLL_XGYRO); + + switch (accelRange) + { + case ACCEL_FS::A2G: + setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + accelerationResolution = 2.0 / 16384.0; + + case ACCEL_FS::A4G: + setFullScaleAccelRange(MPU6050_ACCEL_FS_4); + accelerationResolution = 4.0 / 16384.0; + + case ACCEL_FS::A8G: + setFullScaleAccelRange(MPU6050_ACCEL_FS_8); + accelerationResolution = 8.0 / 16384.0; + + case ACCEL_FS::A16G: + setFullScaleAccelRange(MPU6050_ACCEL_FS_16); + accelerationResolution = 16.0 / 16384.0; + } + + switch (gyroRange) + { + case GYRO_FS::G250DPS: + setFullScaleGyroRange(MPU6050_GYRO_FS_250); + gyroscopeResolution = 250.0 / 16384.0; + + case GYRO_FS::G500DPS: + setFullScaleGyroRange(MPU6050_GYRO_FS_500); + gyroscopeResolution = 500.0 / 16384.0; + + case GYRO_FS::G1000DPS: + setFullScaleGyroRange(MPU6050_GYRO_FS_1000); + gyroscopeResolution = 1000.0 / 16384.0; + + case GYRO_FS::G2000DPS: + setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + gyroscopeResolution = 2000.0 / 16384.0; + } + setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! } +/** Get the accelration resolution. + */ +float MPU6050_Base::get_acce_resolution() { + return accelerationResolution; +} + +/** Get the gyroscope resolution. + */ +float MPU6050_Base::get_gyro_resolution() { + return gyroscopeResolution; +} + /** Verify the I2C connection. * Make sure the device is connected and responds as expected. * @return True if connection is valid, false otherwise diff --git a/src/MPU6050.h b/src/MPU6050.h index 041963e..b8e6a3e 100644 --- a/src/MPU6050.h +++ b/src/MPU6050.h @@ -437,11 +437,30 @@ THE SOFTWARE. #define MPU6050_FIFO_DEFAULT_TIMEOUT 11000 +enum class ACCEL_FS { + A2G, + A4G, + A8G, + A16G +}; + +enum class GYRO_FS { + G250DPS, + G500DPS, + G1000DPS, + G2000DPS +}; + class MPU6050_Base { public: MPU6050_Base(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0); void initialize(); + void initialize(ACCEL_FS accelRange, GYRO_FS gyroRange); + + float get_acce_resolution(); + float get_gyro_resolution(); + bool testConnection(); // AUX_VDDIO register @@ -839,6 +858,9 @@ class MPU6050_Base { void *wireObj; uint8_t buffer[14]; uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT; + + float accelerationResolution; + float gyroscopeResolution; private: int16_t offsets[6]; diff --git a/src/MPU6050_6Axis_MotionApps20.cpp b/src/MPU6050_6Axis_MotionApps20.cpp index 913650f..b8decc0 100644 --- a/src/MPU6050_6Axis_MotionApps20.cpp +++ b/src/MPU6050_6Axis_MotionApps20.cpp @@ -408,43 +408,44 @@ bool MPU6050_6Axis_MotionApps20::dmpPacketAvailable() { uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); - data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); - data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + data[0] = (((int32_t)packet[28] << 24) | ((int32_t)packet[29] << 16) | ((int32_t)packet[30] << 8) | (int32_t)packet[31]); + data[1] = (((int32_t)packet[32] << 24) | ((int32_t)packet[33] << 16) | ((int32_t)packet[34] << 8) | (int32_t)packet[35]); + data[2] = (((int32_t)packet[36] << 24) | ((int32_t)packet[37] << 16) | ((int32_t)packet[38] << 8) | (int32_t)packet[39]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[32] << 8) | packet[33]; - data[2] = (packet[36] << 8) | packet[37]; + data[0] = ((int16_t)packet[28] << 8) | (int16_t)packet[29]; + data[1] = ((int16_t)packet[32] << 8) | (int16_t)packet[33]; + data[2] = ((int16_t)packet[36] << 8) | (int16_t)packet[37]; return 0; } + uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[28] << 8) | packet[29]; - v -> y = (packet[32] << 8) | packet[33]; - v -> z = (packet[36] << 8) | packet[37]; + v -> x = ((int16_t)packet[28] << 8) | (int16_t)packet[29]; + v -> y = ((int16_t)packet[32] << 8) | (int16_t)packet[33]; + v -> z = ((int16_t)packet[36] << 8) | (int16_t)packet[37]; return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + data[0] = (((int32_t)packet[0] << 24) | ((int32_t)packet[1] << 16) | ((int32_t)packet[2] << 8) | (int32_t)packet[3]); + data[1] = (((int32_t)packet[4] << 24) | ((int32_t)packet[5] << 16) | ((int32_t)packet[6] << 8) | (int32_t)packet[7]); + data[2] = (((int32_t)packet[8] << 24) | ((int32_t)packet[9] << 16) | ((int32_t)packet[10] << 8) | (int32_t)packet[11]); + data[3] = (((int32_t)packet[12] << 24) | ((int32_t)packet[13] << 16) | ((int32_t)packet[14] << 8) | (int32_t)packet[15]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); + data[0] = (((int16_t)packet[0] << 8) | (int16_t)packet[1]); + data[1] = (((int16_t)packet[4] << 8) | (int16_t)packet[5]); + data[2] = (((int16_t)packet[8] << 8) | (int16_t)packet[9]); + data[3] = (((int16_t)packet[12] << 8) | (int16_t)packet[13]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { @@ -465,25 +466,25 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_ uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + data[0] = (((int32_t)packet[16] << 24) | ((int32_t)packet[17] << 16) | ((int32_t)packet[18] << 8) | (int32_t)packet[19]); + data[1] = (((int32_t)packet[20] << 24) | ((int32_t)packet[21] << 16) | ((int32_t)packet[22] << 8) | (int32_t)packet[23]); + data[2] = (((int32_t)packet[24] << 24) | ((int32_t)packet[25] << 16) | ((int32_t)packet[26] << 8) | (int32_t)packet[27]); return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; + data[0] = ((int16_t)packet[16] << 8) | (int16_t)packet[17]; + data[1] = ((int16_t)packet[20] << 8) | (int16_t)packet[21]; + data[2] = ((int16_t)packet[24] << 8) | (int16_t)packet[25]; return 0; } uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[16] << 8) | packet[17]; - v -> y = (packet[20] << 8) | packet[21]; - v -> z = (packet[24] << 8) | packet[25]; + v -> x = ((int16_t)packet[16] << 8) | (int16_t)packet[17]; + v -> y = ((int16_t)packet[20] << 8) | (int16_t)packet[21]; + v -> z = ((int16_t)packet[24] << 8) | (int16_t)packet[25]; return 0; } // uint8_t MPU6050_6Axis_MotionApps20::dmpSetLinearAccelFilterCoefficient(float coef); @@ -495,8 +496,8 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(VectorInt16 *v, VectorInt1 v -> z = vRaw -> z - gravity -> z*8192; return 0; } -// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { +// uint8_t MPU6050_6Axis_MotionApps20::dmpConvertToWorldFrame(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpConvertToWorldFrame(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { // rotate measured 3D acceleration vector into original state // frame of reference based on orientation quaternion memcpy(v, vReal, sizeof(VectorInt16)); @@ -514,8 +515,7 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(int16_t *data, const uint8_t* uint8_t status = dmpGetQuaternion(qI, packet); data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; - data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); return status; } @@ -524,6 +524,7 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(VectorFloat *v, Quaternion *q) v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; return 0; + } // uint8_t MPU6050_6Axis_MotionApps20::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); // uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuantizedAccel(long *data, const uint8_t* packet); diff --git a/src/MPU6050_6Axis_MotionApps20.h b/src/MPU6050_6Axis_MotionApps20.h index 7729795..98e7c34 100644 --- a/src/MPU6050_6Axis_MotionApps20.h +++ b/src/MPU6050_6Axis_MotionApps20.h @@ -96,10 +96,10 @@ class MPU6050_6Axis_MotionApps20 : public MPU6050_Base { uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpConvertToWorldFrame(int32_t *data, const uint8_t* packet=0); + uint8_t dmpConvertToWorldFrame(int16_t *data, const uint8_t* packet=0); + uint8_t dmpConvertToWorldFrame(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpConvertToWorldFrame(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);