diff --git a/include/6axismotion.cpp b/include/6axismotion.cpp new file mode 100644 index 0000000..8b66816 --- /dev/null +++ b/include/6axismotion.cpp @@ -0,0 +1,143 @@ +#include "motionbase.cpp" + +// 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 + +void motionSetup() { + // initialize device + accelgyro.initialize(); + devStatus = accelgyro.dmpInitialize(); + + accelgyro.setXGyroOffset(calibration.G_off[0]); + accelgyro.setYGyroOffset(calibration.G_off[1]); + accelgyro.setZGyroOffset(calibration.G_off[2]); + accelgyro.setXAccelOffset(calibration.A_B[0]); + accelgyro.setYAccelOffset(calibration.A_B[1]); + accelgyro.setZAccelOffset(calibration.A_B[2]); + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP...")); + accelgyro.setDMPEnabled(true); + mpuIntStatus = accelgyro.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = accelgyro.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } +} + +void motionLoop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + mpuIntStatus = accelgyro.getIntStatus(); + + // get current FIFO count + fifoCount = accelgyro.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + accelgyro.resetFIFO(); + Serial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = accelgyro.getFIFOCount(); + + // read a packet from FIFO + accelgyro.getFIFOBytes(fifoBuffer, packetSize); + + // track FIFO count here in case there is > 1 packet available + // (this lets us immediately read more without waiting for an interrupt) + fifoCount -= packetSize; + + accelgyro.dmpGetQuaternion(&rawQuat, fifoBuffer); + q[0] = rawQuat.x; + q[1] = rawQuat.y; + q[2] = rawQuat.z; + q[3] = rawQuat.w; + cq.set(-q[1], q[0], q[2], q[3]); + cq *= rotationQuat; + } +} + +void sendData() { + sendQuat(&cq, PACKET_ROTATION); +} + +void performCalibration() { + digitalWrite(CALIBRATING_LED, LOW); + Serial.println("Gathering raw data for device calibration..."); + int calibrationSamples = 500; + // Reset values + float Gxyz[3]; + int16_t gx; + int16_t gy; + int16_t gz; + int16_t ax; + int16_t ay; + int16_t az; + + // Wait for sensor to calm down before calibration + Serial.println("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + for (int i = 0; i < calibrationSamples; i++) + { + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + Gxyz[0] += float(gx); + Gxyz[1] += float(gy); + Gxyz[2] += float(gz); + delay(10); + } + Gxyz[0] /= calibrationSamples; + Gxyz[1] /= calibrationSamples; + Gxyz[2] /= calibrationSamples; + Serial.printf("Gyro calibration results: %f %f %f\n", Gxyz[0], Gxyz[1], Gxyz[2]); + sendVector(Gxyz, PACKET_GYRO_CALIBRATION_DATA); + + // Blink calibrating led before user should rotate the sensor + Serial.println("Gently rotate the device while it's gathering accelerometer data"); + for (int i = 0; i < 3000 / 310; ++i) + { + digitalWrite(CALIBRATING_LED, LOW); + delay(15); + digitalWrite(CALIBRATING_LED, HIGH); + delay(300); + } + int calibrationData[6]; + for (int i = 0; i < calibrationSamples; i++) + { + digitalWrite(CALIBRATING_LED, LOW); + accelgyro.getAcceleration(&ax, &ay, &az); + calibrationData[0] = ax; + calibrationData[1] = ay; + calibrationData[2] = az; + calibrationData[3] = 0; + calibrationData[4] = 0; + calibrationData[5] = 0; + sendRawCalibrationData(calibrationData, PACKET_RAW_CALIBRATION_DATA); + digitalWrite(CALIBRATING_LED, HIGH); + delay(50); + } + Serial.println("Calibration data gathered and sent"); + digitalWrite(CALIBRATING_LED, HIGH); +} \ No newline at end of file diff --git a/include/9axismotion.cpp b/include/9axismotion.cpp new file mode 100644 index 0000000..835c16a --- /dev/null +++ b/include/9axismotion.cpp @@ -0,0 +1,242 @@ +#include "motionbase.cpp" + +//raw data and scaled as vector +int16_t ax, ay, az; +int16_t gx, gy, gz; +int16_t mx, my, mz; +float Axyz[3]; +float Gxyz[3]; +float Mxyz[3]; +float rawMag[3]; +#define gscale (250. / 32768.0) * (PI / 180.0) //gyro default 250 LSB per d/s -> rad/s + +// NOW USING MAHONY FILTER + +// These are the free parameters in the Mahony filter and fusion scheme, +// Kp for proportional feedback, Ki for integral +// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required. +#define Kp 10.0 +#define Ki 0.0 + +// Loop timing globals +unsigned long now = 0, last = 0; //micros() timers +float deltat = 0; //loop time in seconds + +void get_MPU_scaled(); +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat); + +void motionSetup() { + // initialize device + accelgyro.initialize(); +} + +void motionLoop() { + // Update quaternion + now = micros(); + deltat = (now - last) * 1.0e-6; //seconds since last update + last = now; + get_MPU_scaled(); + MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[1], Mxyz[0], -Mxyz[2], deltat); + cq.set(-q[1], -q[2], -q[0], q[3]); + cq *= rotationQuat; +} + +void sendData() { + sendQuat(&cq, PACKET_ROTATION); + sendVector(rawMag, PACKET_RAW_MAGENTOMETER); + sendVector(Axyz, PACKET_ACCEL); + sendVector(Mxyz, PACKET_MAG); +} + +void get_MPU_scaled() +{ + float temp[3]; + int i; + accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); + + Gxyz[0] = ((float)gx - calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s + Gxyz[1] = ((float)gy - calibration.G_off[1]) * gscale; + Gxyz[2] = ((float)gz - calibration.G_off[2]) * gscale; + + Axyz[0] = (float)ax; + Axyz[1] = (float)ay; + Axyz[2] = (float)az; + //apply offsets (bias) and scale factors from Magneto + if(useFullCalibrationMatrix) { + for (i = 0; i < 3; i++) + temp[i] = (Axyz[i] - calibration.A_B[i]); + Axyz[0] = calibration.A_Ainv[0][0] * temp[0] + calibration.A_Ainv[0][1] * temp[1] + calibration.A_Ainv[0][2] * temp[2]; + Axyz[1] = calibration.A_Ainv[1][0] * temp[0] + calibration.A_Ainv[1][1] * temp[1] + calibration.A_Ainv[1][2] * temp[2]; + Axyz[2] = calibration.A_Ainv[2][0] * temp[0] + calibration.A_Ainv[2][1] * temp[1] + calibration.A_Ainv[2][2] * temp[2]; + } else { + for (i = 0; i < 3; i++) + Axyz[i] = (Axyz[i] - calibration.A_B[i]); + } + vector_normalize(Axyz); + + Mxyz[0] = (float)mx; + Mxyz[1] = (float)my; + Mxyz[2] = (float)mz; + //apply offsets and scale factors from Magneto + if(useFullCalibrationMatrix) { + for (i = 0; i < 3; i++) + temp[i] = (Mxyz[i] - calibration.M_B[i]); + Mxyz[0] = calibration.M_Ainv[0][0] * temp[0] + calibration.M_Ainv[0][1] * temp[1] + calibration.M_Ainv[0][2] * temp[2]; + Mxyz[1] = calibration.M_Ainv[1][0] * temp[0] + calibration.M_Ainv[1][1] * temp[1] + calibration.M_Ainv[1][2] * temp[2]; + Mxyz[2] = calibration.M_Ainv[2][0] * temp[0] + calibration.M_Ainv[2][1] * temp[1] + calibration.M_Ainv[2][2] * temp[2]; + } else { + for (i = 0; i < 3; i++) + Mxyz[i] = (Mxyz[i] - calibration.M_B[i]); + } + rawMag[0] = Mxyz[0]; + rawMag[1] = Mxyz[1]; + rawMag[2] = Mxyz[2]; + vector_normalize(Mxyz); +} + +// Mahony orientation filter, assumed World Frame NWU (xNorth, yWest, zUp) +// Modified from Madgwick version to remove Z component of magnetometer: +// reference vectors are Up (Acc) and West (Acc cross Mag) +// sjr 12/2020 +// input vectors ax, ay, az and mx, my, mz MUST be normalized! +// gx, gy, gz must be in units of radians/second +// +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) +{ + // Vector to hold integral error for Mahony method + static float eInt[3] = {0.0, 0.0, 0.0}; + + // short name local variable for readability + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; + float norm; + float hx, hy, hz; //observed West vector W = AxM + float ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame + float ex, ey, ez; + float pa, pb, pc; + + // Auxiliary variables to avoid repeated arithmetic + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Measured horizon vector = a x m (in body frame) + hx = ay * mz - az * my; + hy = az * mx - ax * mz; + hz = ax * my - ay * mx; + // Normalise horizon vector + norm = sqrt(hx * hx + hy * hy + hz * hz); + if (norm == 0.0f) return; // Handle div by zero + + norm = 1.0f / norm; + hx *= norm; + hy *= norm; + hz *= norm; + + // Estimated direction of Up reference vector + ux = 2.0f * (q2q4 - q1q3); + uy = 2.0f * (q1q2 + q3q4); + uz = q1q1 - q2q2 - q3q3 + q4q4; + + // estimated direction of horizon (West) reference vector + wx = 2.0f * (q2q3 + q1q4); + wy = q1q1 - q2q2 + q3q3 - q4q4; + wz = 2.0f * (q3q4 - q1q2); + + // Error is cross product between estimated direction and measured direction of the reference vectors + ex = (ay * uz - az * uy) + (hy * wz - hz * wy); + ey = (az * ux - ax * uz) + (hz * wx - hx * wz); + ez = (ax * uy - ay * ux) + (hx * wy - hy * wx); + + if (Ki > 0.0f) + { + eInt[0] += ex; // accumulate integral error + eInt[1] += ey; + eInt[2] += ez; + // Apply I feedback + gx += Ki * eInt[0]; + gy += Ki * eInt[1]; + gz += Ki * eInt[2]; + } + + // Apply P feedback + gx = gx + Kp * ex; + gy = gy + Kp * ey; + gz = gz + Kp * ez; + + // Integrate rate of change of quaternion + pa = q2; + pb = q3; + pc = q4; + q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); + q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); + q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); + q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); + + // Normalise quaternion + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); + norm = 1.0f / norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + +void performCalibration() { + digitalWrite(CALIBRATING_LED, LOW); + Serial.println("Gathering raw data for device calibration..."); + int calibrationSamples = 300; + // Reset values + Gxyz[0] = 0; + Gxyz[1] = 0; + Gxyz[2] = 0; + + // Wait for sensor to calm down before calibration + Serial.println("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + for (int i = 0; i < calibrationSamples; i++) + { + accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); + Gxyz[0] += float(gx); + Gxyz[1] += float(gy); + Gxyz[2] += float(gz); + } + Gxyz[0] /= calibrationSamples; + Gxyz[1] /= calibrationSamples; + Gxyz[2] /= calibrationSamples; + Serial.printf("Gyro calibration results: %f %f %f\n", Gxyz[0], Gxyz[1], Gxyz[2]); + sendVector(Gxyz, PACKET_GYRO_CALIBRATION_DATA); + + // Blink calibrating led before user should rotate the sensor + Serial.println("Gently rotate the device while it's gathering accelerometer and magnetometer data"); + for (int i = 0; i < 3000 / 310; ++i) + { + digitalWrite(CALIBRATING_LED, LOW); + delay(15); + digitalWrite(CALIBRATING_LED, HIGH); + delay(300); + } + int calibrationData[6]; + for (int i = 0; i < calibrationSamples; i++) + { + digitalWrite(CALIBRATING_LED, LOW); + accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); + calibrationData[0] = ax; + calibrationData[1] = ay; + calibrationData[2] = az; + calibrationData[3] = mx; + calibrationData[4] = my; + calibrationData[5] = mz; + sendRawCalibrationData(calibrationData, PACKET_RAW_CALIBRATION_DATA); + digitalWrite(CALIBRATING_LED, HIGH); + delay(250); + } + Serial.println("Calibration data gathered and sent"); + digitalWrite(CALIBRATING_LED, HIGH); +} \ No newline at end of file diff --git a/include/MPU9250.cpp b/include/MPU9250.cpp index fbc8332..1abea2f 100644 --- a/include/MPU9250.cpp +++ b/include/MPU9250.cpp @@ -3151,3 +3151,120 @@ uint8_t MPU9250::getDMPConfig2() { void MPU9250::setDMPConfig2(uint8_t config) { I2Cdev::writeByte(devAddr, MPU9250_RA_DMP_CFG_2, config); } + +//*************************************************************************************** +//********************** Calibration Routines ********************** +//*************************************************************************************** +/** + @brief Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU9250::CalibrateGyro(uint8_t Loops ) { + double kP = 0.3; + double kI = 90; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + + PID( 0x43, kP, kI, Loops); +} + +/** + @brief Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU9250::CalibrateAccel(uint8_t Loops ) { + + float kP = 0.3; + float kI = 20; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + PID( 0x3B, kP, kI, Loops); +} + +void MPU9250::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){ + uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13; + + int16_t Data; + float Reading; + int16_t BitZero[3]; + uint8_t shift =(SaveAddress == 0x77)?3:2; + float Error, PTerm, ITerm[3]; + int16_t eSample; + uint32_t eSum ; + Serial.write('>'); + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if(SaveAddress != 0x13){ + BitZero[i] = Data & 1; // Capture Bit Zero to properly handle Accelerometer calibration + ITerm[i] = ((float)Reading) * 8; + } else { + ITerm[i] = Reading * 4; + } + } + for (int L = 0; L < Loops; L++) { + eSample = 0; + for (int c = 0; c < 100; c++) {// 100 PI Calculations + eSum = 0; + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity + Error = -Reading; + eSum += abs(Reading); + PTerm = kP * Error; + ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001 + if(SaveAddress != 0x13){ + Data = round((PTerm + ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((PTerm + ITerm[i] ) / 4); //Compute PID Output + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); + } + if((c == 99) && eSum > 1000){ // Error is still to great to continue + c = 0; + Serial.write('*'); + } + if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++; // Successfully found offsets prepare to advance + if((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop + delay(1); + } + Serial.write('.'); + kP *= .75; + kI *= .75; + for (int i = 0; i < 3; i++){ + if(SaveAddress != 0x13) { + Data = round((ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((ITerm[i]) / 4); + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); + } + } + resetFIFO(); + resetDMP(); +} + +#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt +void MPU9250::PrintActiveOffsets() { + uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU9250_RA_XA_OFFS_H:0x77; + int16_t Data[3]; + //Serial.print(F("Offset Register 0x")); + //Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX); + Serial.print(F("\n// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro\n//OFFSETS ")); + if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data); + else { + I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data); + I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1); + I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2); + } + // A_OFFSET_H_READ_A_OFFS(Data); + printfloatx("", Data[0], 5, 0, ", "); + printfloatx("", Data[1], 5, 0, ", "); + printfloatx("", Data[2], 5, 0, ", "); + I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data); + // XG_OFFSET_H_READ_OFFS_USR(Data); + printfloatx("", Data[0], 5, 0, ", "); + printfloatx("", Data[1], 5, 0, ", "); + printfloatx("", Data[2], 5, 0, "\n"); +} \ No newline at end of file diff --git a/include/MPU9250.h b/include/MPU9250.h index dc5b139..f38fd6f 100644 --- a/include/MPU9250.h +++ b/include/MPU9250.h @@ -783,6 +783,12 @@ class MPU9250 { uint8_t getDMPConfig2(); void setDMPConfig2(uint8_t config); + // Calibration Routines + void CalibrateGyro(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops. + void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops. + void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math + void PrintActiveOffsets(); // See the results of the Calibration + // special methods for MotionApps 2.0 implementation #ifdef MPU9250_INCLUDE_DMP_MOTIONAPPS20 uint8_t *dmpPacketBuffer; diff --git a/include/MPU9250MotionApps.h b/include/MPU9250MotionApps.h new file mode 100644 index 0000000..3cc28f9 --- /dev/null +++ b/include/MPU9250MotionApps.h @@ -0,0 +1,615 @@ +// I2Cdev library collection - MPU9250 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array +// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU9250_6AXIS_MOTIONAPPS20_H_ +#define _MPU9250_6AXIS_MOTIONAPPS20_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU9250_INCLUDE_DMP_MOTIONAPPS20 + +#include "MPU9250.cpp" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU9250_DMP_CODE_SIZE 1929 // dmpMemory[] +#define MPU9250_DMP_CONFIG_SIZE 192 // dmpConfig[] +#define MPU9250_DMP_UPDATES_SIZE 47 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// I Only Changed this by applying all the configuration data and capturing it before startup: +// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it +const unsigned char dmpMemory[MPU9250_DMP_CODE_SIZE] PROGMEM = { + /* bank # 0 */ + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + /* bank # 1 */ + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + /* bank # 2 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + /* bank # 4 */ + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + /* bank # 5 */ + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + /* bank # 6 */ + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + /* bank # 7 */ + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, + +}; + +#ifndef MPU9250_DMP_FIFO_RATE_DIVISOR +#define MPU9250_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// I Simplified this: +uint8_t MPU9250::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU9250...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + setSleepEnabled(false); + + // get MPU hardware revision + setMemoryBank(0x10, true, true); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLN(readMemoryByte()); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // setup weird slave stuff (?) + DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + setSlaveAddress(0, 0x7F); + DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + setI2CMasterModeEnabled(false); + DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + setSlaveAddress(0, 0x68); + DEBUG_PRINTLN(F("Resetting I2C Master control...")); + resetI2CMaster(); + delay(20); + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU9250_CLOCK_PLL_XGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1<= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU9250::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU9250::dmpGetFIFORate(); +// uint8_t MPU9250::dmpGetSampleStepSizeMS(); +// uint8_t MPU9250::dmpGetSampleFrequency(); +// int32_t MPU9250::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU9250::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU9250::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU9250::dmpRunFIFORateProcesses(); + +// uint8_t MPU9250::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU9250::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU9250::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]); + return 0; +} +uint8_t MPU9250::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]; + return 0; +} +uint8_t MPU9250::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]; + return 0; +} +uint8_t MPU9250::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]); + return 0; +} +uint8_t MPU9250::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]); + return 0; +} +uint8_t MPU9250::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU9250::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU9250::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]); + return 0; +} +uint8_t MPU9250::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]; + return 0; +} +uint8_t MPU9250::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]; + return 0; +} +// uint8_t MPU9250::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU9250::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU9250::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU9250::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU9250::dmpGetLinearAccelInWorld(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)); + v -> rotate(q); + return 0; +} +// uint8_t MPU9250::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU9250::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + 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]) / (2 * 16384); + return status; +} + +uint8_t MPU9250::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + 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 MPU9250::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU9250::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU9250::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU9250::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU9250::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU9250::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU9250::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU9250::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU9250::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU9250::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU9250::dmpInitFIFOParam(); +// uint8_t MPU9250::dmpCloseFIFO(); +// uint8_t MPU9250::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU9250::dmpDecodeQuantizedAccel(); +// uint32_t MPU9250::dmpGetGyroSumOfSquare(); +// uint32_t MPU9250::dmpGetAccelSumOfSquare(); +// void MPU9250::dmpOverrideQuaternion(long *q); +uint16_t MPU9250::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU9250_6AXIS_MOTIONAPPS20_H_ */ diff --git a/include/helper_3dmath.h b/include/helper_3dmath.h new file mode 100644 index 0000000..e27362f --- /dev/null +++ b/include/helper_3dmath.h @@ -0,0 +1,218 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper +// 6/5/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2012-06-05 - add 3D math helper file to DMP6 example sketch + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "Arduino.h" + +#ifndef _HELPER_3DMATH_H_ +#define _HELPER_3DMATH_H_ + +class Quaternion { + public: + float w; + float x; + float y; + float z; + + Quaternion() { + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + Quaternion(float nw, float nx, float ny, float nz) { + w = nw; + x = nx; + y = ny; + z = nz; + } + + Quaternion getProduct(Quaternion q) { + // Quaternion multiplication is defined by: + // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) + // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) + // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) + // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 + return Quaternion( + w*q.w - x*q.x - y*q.y - z*q.z, // new w + w*q.x + x*q.w + y*q.z - z*q.y, // new x + w*q.y - x*q.z + y*q.w + z*q.x, // new y + w*q.z + x*q.y - y*q.x + z*q.w); // new z + } + + Quaternion getConjugate() { + return Quaternion(w, -x, -y, -z); + } + + float getMagnitude() { + return sqrt(w*w + x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + w /= m; + x /= m; + y /= m; + z /= m; + } + + Quaternion getNormalized() { + Quaternion r(w, x, y, z); + r.normalize(); + return r; + } +}; + +class VectorInt16 { + public: + int16_t x; + int16_t y; + int16_t z; + + VectorInt16() { + x = 0; + y = 0; + z = 0; + } + + VectorInt16(int16_t nx, int16_t ny, int16_t nz) { + x = nx; + y = ny; + z = nz; + } + + float getMagnitude() { + return sqrt(x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + x /= m; + y /= m; + z /= m; + } + + VectorInt16 getNormalized() { + VectorInt16 r(x, y, z); + r.normalize(); + return r; + } + + void rotate(Quaternion *q) { + // http://www.cprogramming.com/tutorial/3d/quaternions.html + // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm + // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation + // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 + + // P_out = q * P_in * conj(q) + // - P_out is the output vector + // - q is the orientation quaternion + // - P_in is the input vector (a*aReal) + // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + p = q -> getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + p = p.getProduct(q -> getConjugate()); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorInt16 getRotated(Quaternion *q) { + VectorInt16 r(x, y, z); + r.rotate(q); + return r; + } +}; + +class VectorFloat { + public: + float x; + float y; + float z; + + VectorFloat() { + x = 0; + y = 0; + z = 0; + } + + VectorFloat(float nx, float ny, float nz) { + x = nx; + y = ny; + z = nz; + } + + float getMagnitude() { + return sqrt(x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + x /= m; + y /= m; + z /= m; + } + + VectorFloat getNormalized() { + VectorFloat r(x, y, z); + r.normalize(); + return r; + } + + void rotate(Quaternion *q) { + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + p = q -> getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + p = p.getProduct(q -> getConjugate()); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorFloat getRotated(Quaternion *q) { + VectorFloat r(x, y, z); + r.rotate(q); + return r; + } +}; + +#endif /* _HELPER_3DMATH_H_ */ \ No newline at end of file diff --git a/include/motionbase.cpp b/include/motionbase.cpp new file mode 100644 index 0000000..1c05dbf --- /dev/null +++ b/include/motionbase.cpp @@ -0,0 +1,23 @@ +#include "I2Cdev.cpp" +#include "MPU9250MotionApps.h" +#include "defines.h" +#include "quat.cpp" +#include "configuration.cpp" +#include "util.cpp" +#include "udpclient.cpp" + +MPU9250 accelgyro; +DeviceConfig config; +const CalibrationConfig &calibration = config.calibration; + +// Vector to hold quaternion +float q[4] = {1.0, 0.0, 0.0, 0.0}; +Quaternion rawQuat = Quaternion(); +const Quat rotationQuat = Quat(Vector3(0, 0, 1), PI / 2.0); // Adjust rotation to match Android rotations +Quat cq = Quat(); + +void motionSetup(); + +void motionLoop(); + +void sendData(); \ No newline at end of file diff --git a/include/udpclient.cpp b/include/udpclient.cpp index 1126f1c..0485430 100644 --- a/include/udpclient.cpp +++ b/include/udpclient.cpp @@ -1,5 +1,6 @@ #include "udpclient.h" #include "configuration.h" +#include "defines.h" #define TIMEOUT 3000UL @@ -205,6 +206,18 @@ void sendRawCalibrationData(int *const data, int type) } } +void returnLastPacket(int len) { + if (Udp.beginPacket(host, port) > 0) + { + Udp.write(incomingPacket, len); + if (Udp.endPacket() == 0) + { + //Serial.print("Write error: "); + //Serial.println(Udp.getWriteError()); + } + } +} + void setConfigRecievedCallback(configRecievedCallback callback) { fp_configCallback = callback; @@ -224,13 +237,15 @@ void clientUpdate() if (packetSize) { lastPacketMs = millis(); - // receive incoming UDP packets - Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort()); int len = Udp.read(incomingPacket, sizeof(incomingPacket)); - Serial.print("UDP packet contents: "); - for (int i = 0; i < len; ++i) - Serial.print((byte)incomingPacket[i]); - Serial.println(); + // receive incoming UDP packets + if(serialDebug) { + Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort()); + Serial.print("UDP packet contents: "); + for (int i = 0; i < len; ++i) + Serial.print((byte)incomingPacket[i]); + Serial.println(); + } switch (convert_chars(incomingPacket)) { @@ -251,7 +266,9 @@ void clientUpdate() Serial.println("Command packet too short"); break; } - Serial.printf("Recieved command %d\n", incomingPacket[4]); + if(serialDebug) { + Serial.printf("Recieved command %d\n", incomingPacket[4]); + } if (fp_commandCallback) { fp_commandCallback(incomingPacket[4], &incomingPacket[5], len - 6); @@ -268,6 +285,9 @@ void clientUpdate() fp_configCallback(convert_chars(&incomingPacket[4])); } break; + case PACKET_PING_PONG: + returnLastPacket(len); + break; } } if(lastPacketMs + TIMEOUT < millis()) diff --git a/include/udpclient.h b/include/udpclient.h index e1a4e34..dbeaa75 100644 --- a/include/udpclient.h +++ b/include/udpclient.h @@ -15,6 +15,8 @@ #define PACKET_RAW_CALIBRATION_DATA 6 #define PACKET_GYRO_CALIBRATION_DATA 7 #define PACKET_CONFIG 8 +#define PACKET_RAW_MAGENTOMETER 9 +#define PACKET_PING_PONG 10 #define PACKET_RECIEVE_HEARTBEAT 1 #define PACKET_RECIEVE_VIBRATE 2 diff --git a/platformio.ini b/platformio.ini index dfd7c35..fe95174 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,7 +14,7 @@ board = esp12e framework = arduino monitor_speed = 115200 -; If you want to enable OTA Updates, uncommect and set OTA password here and in credentials.h +; If you want to enable OTA Updates, uncomment and set OTA password here and in credentials.h ; You can set upload_port to device's ip after it's set up for the first time ; Use the same password in SlimeVR Server to let it OTA Update the device ;upload_protocol = espota diff --git a/src/defines.h b/src/defines.h index a7ff600..6a764d2 100644 --- a/src/defines.h +++ b/src/defines.h @@ -23,9 +23,12 @@ /////////////////////////////////////////////////////////////////// //Determines how often we sample and send data /////////////////////////////////////////////////////////////////// -#define samplingRateInMillis 250 +#define samplingRateInMillis 10 /////////////////////////////////////////////////////////////////// //Setup for the Magnetometer /////////////////////////////////////////////////////////////////// -#define calibrateMagnetometer false //Setting requires requires you to move device in figure 8 pattern when prompted over serial port. Typically, you do this once, then manually provide the calibration values moving forward. +#define useFullCalibrationMatrix true + +#define sensorIdTime 1000 +#define sensorIdInterval 100 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index e1ca2d5..63c9d97 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,64 +19,13 @@ // This version must be compiled with library routines in subfolder "libs" #include "Wire.h" -// I2Cdev and MPU9250 must be installed as libraries, or else the .cpp/.h files -// for both classes must be in the include path of your project -#include "I2Cdev.cpp" -#include "MPU9250.cpp" -#include "udpclient.cpp" -#include "defines.h" -#include "quat.cpp" -#include "util.cpp" -#include "configuration.cpp" #include "ota.cpp" +#include "6axismotion.cpp" -#define sensorIdTime 1000 -#define sensorIdInterval 100 - -// class default I2C address is 0x68 -// specific I2C addresses may be passed as a parameter here -// AD0 low = 0x68 (default for Sparkfun module) -// AD0 high = 0x69 -// MAHONY FILTER SELECTED BELOW - -MPU9250 accelgyro; -I2Cdev I2C_M; -DeviceConfig config; -const CalibrationConfig &calibration = config.calibration; - -//raw data and scaled as vector -int16_t ax, ay, az; -int16_t gx, gy, gz; -int16_t mx, my, mz; -float Axyz[3]; -float Gxyz[3]; -float Mxyz[3]; bool isCalibrating = false; -#define gscale (250. / 32768.0) * (PI / 180.0) //gyro default 250 LSB per d/s -> rad/s - -// NOW USING MAHONY FILTER - -// These are the free parameters in the Mahony filter and fusion scheme, -// Kp for proportional feedback, Ki for integral -// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required. -#define Kp 8.0 -#define Ki 0.1 - -// Loop timing globals -unsigned long now = 0, last = 0; //micros() timers -float deltat = 0; //loop time in seconds -unsigned long now_ms, last_ms = 0; //millis() timers -unsigned long update_ms = 20; //send updates every "update_ms" milliseconds bool blinking = false; unsigned long blinkStart = 0; - -// Vector to hold quaternion -float q[4] = {1.0, 0.0, 0.0, 0.0}; -const Quat rotationQuat = Quat(Vector3(0, 0, 1), PI / 2.0); // Adjust rotation to match Android rotations -Quat cq = Quat(); - -void get_MPU_scaled(); -void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat); +unsigned long now_ms, last_ms = 0; //millis() timers void setConfig(DeviceConfig newConfig) { @@ -101,7 +50,6 @@ void commandRecieved(int command, void * const commandData, int commandDataLengt } } -void performCalibration(); void processBlinking(); void setup() @@ -120,15 +68,15 @@ void setup() // Load default calibration values and set up callbacks config.calibration = { - {-807.71, 136.80, 290.84}, - {{0.60963, -0.00451, 0.00042}, - {-0.00451, 0.62762, 0.00492}, - {0.00042, 0.00492, 0.60485}}, - {32.06, 34.44, -97.59}, - {{1.68116, 0.08451, -0.01659}, - {0.08451, 1.55939, 0.06883}, - {-0.01659, 0.06883, 1.61732}}, - {421.3, -236.7, 4.8}}; + { 49.22, 1.26, -203.52}, + {{ 1.01176, -0.00048, 0.00230}, + { -0.00048, 1.00366, -0.00535}, + { 0.00230, -0.00535, 0.99003}}, + { 9.85, 47.89, -10.94}, + {{ 1.11351, -0.01290, -0.00077}, + { -0.01290, 1.18048, 0.01839}, + { -0.00077, 0.01839, 1.15212}}, + { 24.59, -4.96, -93.38}}; if (hasConfigStored()) { loadConfig(&config); @@ -136,8 +84,7 @@ void setup() setConfigRecievedCallback(setConfig); setCommandRecievedCallback(commandRecieved); - // initialize device - accelgyro.initialize(); + motionSetup(); // Don't start if not connected to MPU /*while(!accelgyro.testConnection()) { @@ -166,222 +113,30 @@ void loop() performCalibration(); isCalibrating = false; } + motionLoop(); + // Send updates now_ms = millis(); - if (now_ms - last_ms >= update_ms) + if (now_ms - last_ms >= samplingRateInMillis) { - now = micros(); - deltat = (now - last) * 1.0e-6; //seconds since last update - last = now; + last_ms = now_ms; processBlinking(); - get_MPU_scaled(); - MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[1], Mxyz[0], -Mxyz[2], deltat); - last_ms = now_ms; - cq.set(-q[1], -q[2], -q[0], q[3]); - cq *= rotationQuat; - - sendQuat(&cq, PACKET_ROTATION); - //sendQuat(Axyz, PACKET_ACCEL); - //sendQuat(Mxyz, PACKET_MAG); - //sendQuat(Gxyz, PACKET_GYRO); + sendData(); } } } -void get_MPU_scaled() -{ - float temp[3]; - int i; - accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - - Gxyz[0] = ((float)gx - calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s - Gxyz[1] = ((float)gy - calibration.G_off[1]) * gscale; - Gxyz[2] = ((float)gz - calibration.G_off[2]) * gscale; - - Axyz[0] = (float)ax; - Axyz[1] = (float)ay; - Axyz[2] = (float)az; - //apply offsets (bias) and scale factors from Magneto - for (i = 0; i < 3; i++) - temp[i] = (Axyz[i] - calibration.A_B[i]); - Axyz[0] = calibration.A_Ainv[0][0] * temp[0] + calibration.A_Ainv[0][1] * temp[1] + calibration.A_Ainv[0][2] * temp[2]; - Axyz[1] = calibration.A_Ainv[1][0] * temp[0] + calibration.A_Ainv[1][1] * temp[1] + calibration.A_Ainv[1][2] * temp[2]; - Axyz[2] = calibration.A_Ainv[2][0] * temp[0] + calibration.A_Ainv[2][1] * temp[1] + calibration.A_Ainv[2][2] * temp[2]; - vector_normalize(Axyz); - - Mxyz[0] = (float)mx; - Mxyz[1] = (float)my; - Mxyz[2] = (float)mz; - //apply offsets and scale factors from Magneto - for (i = 0; i < 3; i++) - temp[i] = (Mxyz[i] - calibration.M_B[i]); - Mxyz[0] = calibration.M_Ainv[0][0] * temp[0] + calibration.M_Ainv[0][1] * temp[1] + calibration.M_Ainv[0][2] * temp[2]; - Mxyz[1] = calibration.M_Ainv[1][0] * temp[0] + calibration.M_Ainv[1][1] * temp[1] + calibration.M_Ainv[1][2] * temp[2]; - Mxyz[2] = calibration.M_Ainv[2][0] * temp[0] + calibration.M_Ainv[2][1] * temp[1] + calibration.M_Ainv[2][2] * temp[2]; - vector_normalize(Mxyz); -} - -// Mahony scheme uses proportional and integral filtering on -// the error between estimated reference vectors and measured ones. -void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) -{ - // Vector to hold integral error for Mahony method - static float eInt[3] = {0.0, 0.0, 0.0}; - // short name local variable for readability - float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; - float norm; - float hx, hy, bx, bz; - float vx, vy, vz, wx, wy, wz; - float ex, ey, ez; - float pa, pb, pc; - - // Auxiliary variables to avoid repeated arithmetic - float q1q1 = q1 * q1; - float q1q2 = q1 * q2; - float q1q3 = q1 * q3; - float q1q4 = q1 * q4; - float q2q2 = q2 * q2; - float q2q3 = q2 * q3; - float q2q4 = q2 * q4; - float q3q3 = q3 * q3; - float q3q4 = q3 * q4; - float q4q4 = q4 * q4; - /* - // already done in loop() - - // Normalise accelerometer measurement - norm = sqrt(ax * ax + ay * ay + az * az); - if (norm == 0.0f) return; // Handle NaN - norm = 1.0f / norm; // Use reciprocal for division - ax *= norm; - ay *= norm; - az *= norm; - - // Normalise magnetometer measurement - norm = sqrt(mx * mx + my * my + mz * mz); - if (norm == 0.0f) return; // Handle NaN - norm = 1.0f / norm; // Use reciprocal for division - mx *= norm; - my *= norm; - mz *= norm; - */ - // Reference direction of Earth's magnetic field - hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); - hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); - bx = sqrt((hx * hx) + (hy * hy)); - bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); - - // Estimated direction of gravity and magnetic field - vx = 2.0f * (q2q4 - q1q3); - vy = 2.0f * (q1q2 + q3q4); - vz = q1q1 - q2q2 - q3q3 + q4q4; - wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); - wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); - wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); - - // Error is cross product between estimated direction and measured direction of the reference vectors - ex = (ay * vz - az * vy) + (my * wz - mz * wy); - ey = (az * vx - ax * vz) + (mz * wx - mx * wz); - ez = (ax * vy - ay * vx) + (mx * wy - my * wx); - if (Ki > 0.0f) - { - eInt[0] += ex; // accumulate integral error - eInt[1] += ey; - eInt[2] += ez; - // Apply I feedback - gx += Ki * eInt[0]; - gy += Ki * eInt[1]; - gz += Ki * eInt[2]; - } - - // Apply P feedback - gx = gx + Kp * ex; - gy = gy + Kp * ey; - gz = gz + Kp * ez; - - // Integrate rate of change of quaternion - pa = q2; - pb = q3; - pc = q4; - q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); - q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); - q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); - q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); - - // Normalise quaternion - norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); - norm = 1.0f / norm; - q[0] = q1 * norm; - q[1] = q2 * norm; - q[2] = q3 * norm; - q[3] = q4 * norm; -} - -void performCalibration() -{ - digitalWrite(CALIBRATING_LED, LOW); - Serial.println("Gathering raw data for device calibration..."); - int calibrationSamples = 300; - // Reset values - Gxyz[0] = 0; - Gxyz[1] = 0; - Gxyz[2] = 0; - - // Wait for sensor to calm down before calibration - Serial.println("Put down the device and wait for baseline gyro reading calibration"); - delay(2000); - for (int i = 0; i < calibrationSamples; i++) - { - accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - Gxyz[0] += float(gx); - Gxyz[1] += float(gy); - Gxyz[2] += float(gz); - } - Gxyz[0] /= calibrationSamples; - Gxyz[1] /= calibrationSamples; - Gxyz[2] /= calibrationSamples; - Serial.printf("Gyro calibration results: %f %f %f\n", Gxyz[0], Gxyz[1], Gxyz[2]); - sendVector(Gxyz, PACKET_GYRO_CALIBRATION_DATA); - - // Blink calibrating led before user should rotate the sensor - Serial.println("Gently rotate the device while it's gathering accelerometer and magnetometer data"); - for (int i = 0; i < 3000 / 310; ++i) - { - digitalWrite(CALIBRATING_LED, LOW); - delay(15); - digitalWrite(CALIBRATING_LED, HIGH); - delay(300); - } - int calibrationData[6]; - for (int i = 0; i < calibrationSamples; i++) - { - digitalWrite(CALIBRATING_LED, LOW); - accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - calibrationData[0] = ax; - calibrationData[1] = ay; - calibrationData[2] = az; - calibrationData[3] = mx; - calibrationData[4] = my; - calibrationData[5] = mz; - sendRawCalibrationData(calibrationData, PACKET_RAW_CALIBRATION_DATA); - digitalWrite(CALIBRATING_LED, HIGH); - delay(250); - } - Serial.println("Calibration data gathered and sent"); - digitalWrite(CALIBRATING_LED, HIGH); -} - void processBlinking() { if (blinking) { - if (blinkStart + sensorIdTime < now) + if (blinkStart + sensorIdTime < now_ms) { blinking = false; digitalWrite(LOADING_LED, HIGH); } else { - int t = (now - blinkStart) / sensorIdInterval; + int t = (now_ms - blinkStart) / sensorIdInterval; if(t % 2) { digitalWrite(LOADING_LED, LOW); } else {