ROS related data extraction

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);
+    }
+}

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

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];

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);

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);