mirror of
https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git
synced 2026-04-06 02:01:57 +02:00
Added support for accel+gyro mode with internal DMP for MPU 9250 or 6050
Added support for network ping, added support for sending raw mag data Updated Mahony fusion Cleanup debug Added support for new accel+gyro calibration (not used yet)
This commit is contained in:
Eiren Rain
143
include/6axismotion.cpp
Normal file
143
include/6axismotion.cpp
Normal file
@@ -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);
|
||||
}
|
||||
242
include/9axismotion.cpp
Normal file
242
include/9axismotion.cpp
Normal file
@@ -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);
|
||||
}
|
||||
@@ -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");
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
615
include/MPU9250MotionApps.h
Normal file
615
include/MPU9250MotionApps.h
Normal file
@@ -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 <jeff@rowberg.net>
|
||||
// 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 <avr/pgmspace.h>
|
||||
#else
|
||||
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
|
||||
#ifndef __PGMSPACE_H_
|
||||
#define __PGMSPACE_H_ 1
|
||||
#include <inttypes.h>
|
||||
|
||||
#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<<MPU9250_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU9250_INTERRUPT_DMP_INT_BIT);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
|
||||
setRate(4); // 1khz / (1 + 4) = 200 Hz
|
||||
|
||||
DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
|
||||
setExternalFrameSync(MPU9250_EXT_SYNC_TEMP_OUT_L);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
|
||||
setDLPFMode(MPU9250_DLPF_BW_42);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
|
||||
setFullScaleGyroRange(MPU9250_GYRO_FS_2000);
|
||||
|
||||
// load DMP code into memory banks
|
||||
DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
|
||||
DEBUG_PRINT(MPU9250_DMP_CODE_SIZE);
|
||||
DEBUG_PRINTLN(F(" bytes)"));
|
||||
if (!writeProgMemoryBlock(dmpMemory, MPU9250_DMP_CODE_SIZE)) return 1; // Failed
|
||||
DEBUG_PRINTLN(F("Success! DMP code written and verified."));
|
||||
|
||||
// Set the FIFO Rate Divisor int the DMP Firmware Memory
|
||||
unsigned char dmpUpdate[] = {0x00, MPU9250_DMP_FIFO_RATE_DIVISOR};
|
||||
writeMemoryBlock(dmpUpdate, 0x02, 0x02, 0x16); // Lets write the dmpUpdate data to the Firmware image, we have 2 bytes to write in bank 0x02 with the Offset 0x16
|
||||
|
||||
//write start address MSB into register
|
||||
setDMPConfig1(0x03);
|
||||
//write start address LSB into register
|
||||
setDMPConfig2(0x00);
|
||||
|
||||
DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
|
||||
setOTPBankValid(false);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
|
||||
setMotionDetectionThreshold(2);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
|
||||
setZeroMotionDetectionThreshold(156);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
|
||||
setMotionDetectionDuration(80);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
|
||||
setZeroMotionDetectionDuration(0);
|
||||
DEBUG_PRINTLN(F("Enabling FIFO..."));
|
||||
setFIFOEnabled(true);
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting DMP..."));
|
||||
resetDMP();
|
||||
|
||||
DEBUG_PRINTLN(F("DMP is good to go! Finally."));
|
||||
|
||||
DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
|
||||
setDMPEnabled(false);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
|
||||
dmpPacketSize = 42;
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
|
||||
resetFIFO();
|
||||
getIntStatus();
|
||||
|
||||
return 0; // success
|
||||
}
|
||||
// Nothing else changed
|
||||
|
||||
bool MPU9250::dmpPacketAvailable() {
|
||||
return getFIFOCount() >= 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_ */
|
||||
218
include/helper_3dmath.h
Normal file
218
include/helper_3dmath.h
Normal file
@@ -0,0 +1,218 @@
|
||||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
|
||||
// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// 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_ */
|
||||
23
include/motionbase.cpp
Normal file
23
include/motionbase.cpp
Normal file
@@ -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();
|
||||
@@ -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<int>(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<DeviceConfig>(&incomingPacket[4]));
|
||||
}
|
||||
break;
|
||||
case PACKET_PING_PONG:
|
||||
returnLastPacket(len);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(lastPacketMs + TIMEOUT < millis())
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
283
src/main.cpp
283
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 {
|
||||
|
||||
Reference in New Issue
Block a user