kriswiner
commented
7 years ago First of all you are using Sparkfun's code which is cribbed from me, and I don;t know what they did to it.
Secondly, try something like the attached instead. You'll have to make some changes in the wire calls and maybe some other things. It uses an interrupt shceme but this is easily changed to a pollling scheme for data ready.
What you want is to calculate yaw, pitch, and roll from the quaternions (done in the code) and this is your input to unity.
On Mon, Aug 28, 2017 at 9:24 AM, FreakyMex notifications@github.com wrote:
Hello,
I am struggling with using the MPU9250. My goal is rotation tracking for a unity-based app. So far I was successful in communicating between the arduino micro and unity, and with using an MPU6050 with this code (using the MPU's DMP and quaternion output) :
`/ ============================================ Uduino IMU Project. Original MP-6050 library by Jeff Rowberg =============================================== /
include "Uduino.h"
Uduino uduino("IMU");
include "I2Cdev.h"
include "MPU6050_6Axis_MotionApps20.h"
if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
include "Wire.h"
endif
define INTERRUPT_PIN 2
MPU6050 mpu;
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 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
Quaternion q; // [w, x, y, z] quaternion container float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
define OUTPUT_READABLE_QUATERNION
//#define OUTPUT_READABLE_EULER //#define OUTPUT_READABLE_YAWPITCHROLL
void setup() {
if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
endif
Serial.begin(38400); while (!Serial);
mpu.initialize(); pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize(); if (devStatus == 0) { mpu.setDMPEnabled(true); attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); dmpReady = true; packetSize = mpu.dmpGetFIFOPacketSize(); Serial.print("DMP initialzed"); } }
void loop() { int sensorReading = analogRead(A0); uduino.readSerial();
if (uduino.isInit() && dmpReady) {
while (!mpuInterrupt && fifoCount < packetSize) { // If you have other stuff to do in loop() to do, do it here }
mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus();
// get current FIFO count fifoCount = mpu.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 mpu.resetFIFO(); // 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 = mpu.getFIFOCount(); mpu.getFIFOBytes(fifoBuffer, packetSize); fifoCount -= packetSize;
ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print(q.w); Serial.print("/"); Serial.print(q.x); Serial.print("/"); Serial.print(q.y); Serial.print("/"); Serial.print(q.z); Serial.print("/"); Serial.println(sensorReading);
endif
}
} }
void dmpDataReady() { mpuInterrupt = true; }`
The result was really smooth and nice, however rotation along the Z axis was too imprecise. After researching on the subject, it looks like the sensor can't do it appropriately without a magnetometer, which is why I bought a generic MPU9250.
However, I had a lot of trouble making it work.
First I tried this :
`/* MPU9250 Basic Example Code by: Kris Winer date: April 1, 2014 license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. Modified by Brent Wilkins July 19, 2016
Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
SDA and SCL should have external pull-up resistors (to 3.3V). 10k resistors are on the EMSENSR-9250 breakout board.
Hardware setup: MPU9250 Breakout --------- Arduino VDD ---------------------- 3.3V VDDI --------------------- 3.3V SDA ----------------------- A4 SCL ----------------------- A5 GND ---------------------- GND */
include "quaternionFilters.h"
include "MPU9250.h"
include "Uduino.h" // Include Uduino library at the top of the sketch
Uduino uduino("IMU");
/*
ifdef LCD
include
include
// Using NOKIA 5110 monochrome 84 x 48 pixel display // pin 9 - Serial clock out (SCLK) // pin 8 - Serial data out (DIN) // pin 7 - Data/Command select (D/C) // pin 5 - LCD chip select (CS) // pin 6 - LCD reset (RST) Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
endif // LCD
*/
define AHRS true // Set to false for basic data read
define SerialDebug true // Set to true to get Serial output for debugging
// Pin definitions int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins int myLed = 13; // Set up pin 13 led for toggling
MPU9250 myIMU;
void setup() { Wire.begin(); // TWBR = 12; // 400 kbit/sec I2C speed Serial.begin(38400);
// Set up the interrupt pin, its set as active high, push-pull pinMode(intPin, INPUT); digitalWrite(intPin, LOW); pinMode(myLed, OUTPUT); digitalWrite(myLed, HIGH); /*
ifdef LCD
display.begin(); // Ini8ialize the display display.setContrast(58); // Set the contrast
// Start device display with ID of sensor display.clearDisplay(); display.setTextSize(2); display.setCursor(0,0); display.print("MPU9250"); display.setTextSize(1); display.setCursor(0, 20); display.print("9-DOF 16-bit"); display.setCursor(0, 30); display.print("motion sensor"); display.setCursor(20,40); display.print("60 ug LSB"); display.display(); delay(1000);
// Set up for data display display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen display.clearDisplay(); // clears the screen and buffer
endif // LCD
*/
// Read the WHO_AM_I register, this is a good test of communication byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); // Serial.print(" I should be "); Serial.println(0x71, HEX); /*
ifdef LCD
display.setCursor(20,0); display.print("MPU9250"); display.setCursor(0,10); display.print("I AM"); display.setCursor(0,20); display.print(c, HEX); display.setCursor(0,30); display.print("I Should Be"); display.setCursor(0,40); display.print(0x71, HEX); display.display(); delay(1000);
endif // LCD
*/ if (c == 0x71) // WHO_AM_I should always be 0x68 { // Serial.println("MPU9250 is online...");
// Start by performing self test and reporting values myIMU.MPU9250SelfTest(myIMU.SelfTest); / Serial.print("x-axis self test: acceleration trim within : "); Serial.print(myIMU.SelfTest[0],1); Serial.println("% of factory value"); Serial.print("y-axis self test: acceleration trim within : "); Serial.print(myIMU.SelfTest[1],1); Serial.println("% of factory value"); Serial.print("z-axis self test: acceleration trim within : "); Serial.print(myIMU.SelfTest[2],1); Serial.println("% of factory value"); Serial.print("x-axis self test: gyration trim within : "); Serial.print(myIMU.SelfTest[3],1); Serial.println("% of factory value"); Serial.print("y-axis self test: gyration trim within : "); Serial.print(myIMU.SelfTest[4],1); Serial.println("% of factory value"); Serial.print("z-axis self test: gyration trim within : "); Serial.print(myIMU.SelfTest[5],1); Serial.println("% of factory value"); /
// Calibrate gyro and accelerometers, load biases in bias registers myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
/*
ifdef LCD
display.clearDisplay();
display.setCursor(0, 0); display.print("MPU9250 bias"); display.setCursor(0, 8); display.print(" x y z ");
display.setCursor(0, 16); display.print((int)(1000accelBias[0])); display.setCursor(24, 16); display.print((int)(1000accelBias[1])); display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); display.setCursor(72, 16); display.print("mg");
display.setCursor(0, 24); display.print(myIMU.gyroBias[0], 1); display.setCursor(24, 24); display.print(myIMU.gyroBias[1], 1); display.setCursor(48, 24); display.print(myIMU.gyroBias[2], 1); display.setCursor(66, 24); display.print("o/s");
display.display(); delay(1000);
endif // LCD
*/
myIMU.initMPU9250(); // Initialize device for active mode read of acclerometer, gyroscope, and // temperature
// Serial.println("MPU9250 initialized for active data mode....");
// Read the WHO_AM_I register of the magnetometer, this is a good test of // communication byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
// Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); // Serial.print(" I should be "); Serial.println(0x48, HEX); /*
ifdef LCD
display.clearDisplay(); display.setCursor(20,0); display.print("AK8963"); display.setCursor(0,10); display.print("I AM"); display.setCursor(0,20); display.print(d, HEX); display.setCursor(0,30); display.print("I Should Be"); display.setCursor(0,40); display.print(0x48, HEX); display.display(); delay(1000);
endif // LCD
*/
// Get magnetometer calibration from AK8963 ROM myIMU.initAK8963(myIMU.magCalibration); // Initialize device for active mode read of magnetometer
// Serial.println("AK8963 initialized for active data mode...."); if (SerialDebug) { // Serial.println("Calibration values: "); /* Serial.print("X-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[0], 2); Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[1], 2); Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[2], 2);
/ } /
ifdef LCD
display.clearDisplay(); display.setCursor(20,0); display.print("AK8963"); display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(myIMU.magCalibration[0], 2); display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(myIMU.magCalibration[1], 2); display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(myIMU.magCalibration[2], 2); display.display(); delay(1000);
endif // LCD
*/ } // if (c == 0x71) else { // Serial.print("Could not connect to MPU9250: 0x"); // Serial.println(c, HEX); while(1) ; // Loop forever if communication doesn't happen } }
void loop() { int sensorReading = analogRead(A0); uduino.readSerial(); // This part is needed to be detected by Unity. if (uduino.isInit()) {
// If intPin goes high, all data registers have new data // On interrupt, check if data ready interrupt if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values myIMU.getAres();
// Now we'll calculate the accleration value into actual g's // This depends on scale being set myIMU.ax = (float)myIMU.accelCount[0]myIMU.aRes; // - accelBias[0]; myIMU.ay = (float)myIMU.accelCount[1]myIMU.aRes; // - accelBias[1]; myIMU.az = (float)myIMU.accelCount[2]*myIMU.aRes; // - accelBias[2];
myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values myIMU.getGres();
// Calculate the gyro value into actual degrees per second // This depends on scale being set myIMU.gx = (float)myIMU.gyroCount[0]myIMU.gRes; myIMU.gy = (float)myIMU.gyroCount[1]myIMU.gRes; myIMU.gz = (float)myIMU.gyroCount[2]*myIMU.gRes;
myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values myIMU.getMres(); // User environmental x-axis correction in milliGauss, should be // automatically calculated myIMU.magbias[0] = +1.20; // User environmental x-axis correction in milliGauss TODO axis?? myIMU.magbias[1] = +1.21; // User environmental x-axis correction in milliGauss myIMU.magbias[2] = +1.16;
// Calculate the magnetometer values in milliGauss // Include factory calibration per data sheet and user environmental // corrections // Get actual magnetometer value, this depends on scale being set myIMU.mx = (float)myIMU.magCount[0]myIMU.mResmyIMU.magCalibration[0] - myIMU.magbias[0]; myIMU.my = (float)myIMU.magCount[1]myIMU.mResmyIMU.magCalibration[1] - myIMU.magbias[1]; myIMU.mz = (float)myIMU.magCount[2]myIMU.mResmyIMU.magCalibration[2] - myIMU.magbias[2];
} // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
// Must be called before updating quaternions! myIMU.updateTime();
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis // (+ up) of accelerometer and gyro! We have to make some allowance for this // orientationmismatch in feeding the output to the quaternion filter. For the // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward // along the x-axis just like in the LSM9DS0 sensor. This rotation can be // modified to allow any convenient orientation convention. This is ok by // aircraft orientation standards! Pass gyro rate as rad/s // MadgwickQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gz PI/180.0f, my, mx, mz); MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gxDEG_TO_RAD, myIMU.gyDEG_TO_RAD, myIMU.gzDEG_TO_RAD, myIMU.my, myIMU.mx, myIMU.mz, myIMU.deltat);
if (!AHRS) { myIMU.delt_t = millis() - myIMU.count; if (myIMU.delt_t > 500) { if(SerialDebug) { /* // Print acceleration values in milligs! Serial.print("X-acceleration: "); Serial.print(1000
myIMU.ax); Serial.print(" mg "); Serial.print("Y-acceleration: "); Serial.print(1000myIMU.ay); Serial.print(" mg "); Serial.print("Z-acceleration: "); Serial.print(1000*myIMU.az); Serial.println(" mg ");
// Print gyro values in degree/sec Serial.print("X-gyro rate: "); Serial.print(myIMU.gx, 3); Serial.print(" degrees/sec "); Serial.print("Y-gyro rate: "); Serial.print(myIMU.gy, 3); Serial.print(" degrees/sec "); Serial.print("Z-gyro rate: "); Serial.print(myIMU.gz, 3); Serial.println(" degrees/sec"); // Print mag values in degree/sec Serial.print("X-mag field: "); Serial.print(myIMU.mx); Serial.print(" mG "); Serial.print("Y-mag field: "); Serial.print(myIMU.my); Serial.print(" mG "); Serial.print("Z-mag field: "); Serial.print(myIMU.mz); Serial.println(" mG"); *//* myIMU.tempCount = myIMU.readTempData(); // Read the adc values // Temperature in degrees Centigrade myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0; // Print temperature in degrees Centigrade
*// Serial.print("Temperature is "); Serial.print(myIMU.temperature, 1); Serial.println(" degrees C"); / }
/
ifdef LCD
display.clearDisplay(); display.setCursor(0, 0); display.print("MPU9250/AK8963"); display.setCursor(0, 8); display.print(" x y z ");
display.setCursor(0, 16); display.print((int)(1000myIMU.ax)); display.setCursor(24, 16); display.print((int)(1000myIMU.ay)); display.setCursor(48, 16); display.print((int)(1000*myIMU.az)); display.setCursor(72, 16); display.print("mg");
display.setCursor(0, 24); display.print((int)(myIMU.gx)); display.setCursor(24, 24); display.print((int)(myIMU.gy)); display.setCursor(48, 24); display.print((int)(myIMU.gz)); display.setCursor(66, 24); display.print("o/s");
display.setCursor(0, 32); display.print((int)(myIMU.mx)); display.setCursor(24, 32); display.print((int)(myIMU.my)); display.setCursor(48, 32); display.print((int)(myIMU.mz)); display.setCursor(72, 32); display.print("mG");
display.setCursor(0, 40); display.print("Gyro T "); display.setCursor(50, 40); display.print(myIMU.temperature, 1); display.print(" C"); display.display();
endif // LCD
*/
myIMU.count = millis(); digitalWrite(myLed, !digitalRead(myLed)); // toggle led } // if (myIMU.delt_t > 500)
} // if (!AHRS) else { // Serial print and/or display at 0.5 s rate independent of data rates myIMU.delt_t = millis() - myIMU.count;
// update LCD once per half-second independent of read rate
// if (myIMU.delt_t > 500) // { / Serial.print(""); Serial.print( getQ()); Serial.print("/"); Serial.print((getQ() + 1)); Serial.print("/"); Serial.print( (getQ() + 2)); Serial.print("/"); Serial.print((getQ() + 3)); Serial.print("/"); Serial.println(sensorReading); /
// Define output variables from updated quaternion---these are Tait-Bryan // angles, commonly used in aircraft orientation. In this coordinate system, // the positive z-axis is down toward Earth. Yaw is the angle between Sensor // x-axis and Earth magnetic North (or true North if corrected for local // declination, looking down on the sensor positive yaw is counterclockwise. // Pitch is angle between sensor x-axis and Earth ground plane, toward the // Earth is positive, up toward the sky is negative. Roll is angle between // sensor y-axis and Earth ground plane, y-axis up is positive roll. These // arise from the definition of the homogeneous rotation matrix constructed // from quaternions. Tait-Bryan angles as well as Euler angles are // non-commutative; that is, the get the correct orientation the rotations // must be applied in the correct order which for this configuration is yaw, // pitch, and then roll. // For more see // http://en.wikipedia.org/wiki/Conversion_between_ quaternions_and_Euler_angles // which has additional links.
myIMU.yaw = atan2(2.0f ((getQ()+1) (getQ()+2) + getQ() (getQ()+3)), getQ() getQ() + (getQ()+1) *(getQ()+1)
- (getQ()+2) (getQ()+2) - (getQ()+3) (getQ()+3)); myIMU.pitch = -asin(2.0f ((getQ()+1) (getQ()+3) - getQ() (getQ()+2))); myIMU.roll = atan2(2.0f (getQ() (getQ()+1) + (getQ()+2) (getQ()+3)), getQ() getQ() - (getQ()+1) (getQ()+1)
(getQ()+2) (getQ()+2) + (getQ()+3) (getQ()+3)); myIMU.pitch = RAD_TO_DEG; myIMU.yaw = RAD_TO_DEG; // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 // - http://www.ngdc.noaa.gov/geomag-web/#declination myIMU.yaw -= 8.5; myIMU.roll *= RAD_TO_DEG;
Serial.print(myIMU.roll); Serial.print("/"); Serial.print(myIMU.pitch); Serial.print("/"); Serial.print(myIMU.yaw); Serial.print("/"); Serial.print(sensorReading); Serial.print("/"); Serial.println(sensorReading);
/* if(SerialDebug) { Serial.print("Yaw, Pitch, Roll: "); Serial.print(myIMU.yaw, 2); Serial.print(", "); Serial.print(myIMU.pitch, 2); Serial.print(", "); Serial.println(myIMU.roll, 2);
Serial.print("rate = "); Serial.print((float)myIMU.sumCount/myIMU.sum, 2); Serial.println(" Hz");}
ifdef LCD
display.clearDisplay();
display.setCursor(0, 0); display.print(" x y z ");
display.setCursor(0, 8); display.print((int)(1000myIMU.ax)); display.setCursor(24, 8); display.print((int)(1000myIMU.ay)); display.setCursor(48, 8); display.print((int)(1000*myIMU.az)); display.setCursor(72, 8); display.print("mg");
display.setCursor(0, 16); display.print((int)(myIMU.gx)); display.setCursor(24, 16); display.print((int)(myIMU.gy)); display.setCursor(48, 16); display.print((int)(myIMU.gz)); display.setCursor(66, 16); display.print("o/s");
display.setCursor(0, 24); display.print((int)(myIMU.mx)); display.setCursor(24, 24); display.print((int)(myIMU.my)); display.setCursor(48, 24); display.print((int)(myIMU.mz)); display.setCursor(72, 24); display.print("mG");
display.setCursor(0, 32); display.print((int)(myIMU.yaw)); display.setCursor(24, 32); display.print((int)(myIMU.pitch)); display.setCursor(48, 32); display.print((int)(myIMU.roll)); display.setCursor(66, 32); display.print("ypr");
// With these settings the filter is updating at a ~145 Hz rate using the // Madgwick scheme and >200 Hz using the Mahony scheme even though the // display refreshes at only 2 Hz. The filter update rate is determined // mostly by the mathematical steps in the respective algorithms, the // processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: // an ODR of 10 Hz for the magnetometer produce the above rates, maximum // magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and // ~38 Hz for the Madgwick and Mahony schemes, respectively. This is // presumably because the magnetometer read takes longer than the gyro or // accelerometer reads. This filter update rate should be fast enough to // maintain accurate platform orientation for stabilization control of a // fast-moving robot or quadcopter. Compare to the update rate of 200 Hz // produced by the on-board Digital Motion Processor of Invensense's MPU6050 // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty // well! display.setCursor(0, 40); display.print("rt: "); display.print((float) myIMU.sumCount / myIMU.sum, 2); display.print(" Hz"); display.display();
endif // LCD
*/ myIMU.count = millis(); myIMU.sumCount = 0; myIMU.sum = 0; // } // if (myIMU.delt_t > 500) } // if (AHRS) delay(10); } // if (uduino.isInit()) delay(10);`
However the result is very very noisy and incorrect along certains axes.
(Uduino is the library to communicate between Arduino and Unity, and it requires to print only the information you need, which is why I negated the orther printings).
I also tried using Quaternions with : Serial.print(""); Serial.print(getQ()); Serial.print("/"); Serial.print((getQ() + 1)); Serial.print("/"); Serial.print((getQ() + 2)); Serial.print("/"); Serial.print((getQ() + 3)); Serial.print("/"); Serial.println(sensorReading);
But it didn't work either.
The Unity Code is the following one : string[] values = data.Split('/'); float w = float.Parse(values[0]); // ROLL float x = float.Parse(values[1]); // PITCH float y = float.Parse(values[2]); // YAW float z = float.Parse(values[3]); // SENSORREADING float translation = float.Parse(values[4]); rotation.x = y; // YAW rotation.y = x; // PITCH rotation.z = w; // ROLL this.transform.rotation = Quaternion.Euler(rotation);
I also tried with Quaternion with (w, x, y, z) and Quaternion.Inverse(w, x, y, z) but it was worse than using euler Angles, it didn't correspond to the movements.
Sorry if my question seems stupid or else, I am a newbie in electronics and Arduino and I am really struggling with this... I googled a read a lot of topics but I can't really understand the problem here. Do I need a filter ?
Here you can see the youtube video of me trying the sensor : https://www.youtube.com/watch?v=rs1C5n4_RBc
Thank you very much for you time
— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/179, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qvQx4Td_qujIj3jGt2vbn97Q_2Rlks5scum9gaJpZM4PEv0F .
FreakyMex
kosletr
Hello,
I am struggling with using the MPU9250. My goal is rotation tracking for a unity-based app. So far I was successful in communicating between the arduino micro and unity, and with using an MPU6050 with this code (using the MPU's DMP and quaternion output) :
`/ ============================================ Uduino IMU Project. Original MP-6050 library by Jeff Rowberg =============================================== /
include "Uduino.h"
Uduino uduino("IMU");
include "I2Cdev.h"
include "MPU6050_6Axis_MotionApps20.h"
if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
include "Wire.h"
endif
define INTERRUPT_PIN 2
MPU6050 mpu;
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 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
Quaternion q; // [w, x, y, z] quaternion container float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
define OUTPUT_READABLE_QUATERNION
//#define OUTPUT_READABLE_EULER //#define OUTPUT_READABLE_YAWPITCHROLL
void setup() {
if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
endif
Serial.begin(38400); while (!Serial);
mpu.initialize(); pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize(); if (devStatus == 0) { mpu.setDMPEnabled(true); attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); dmpReady = true; packetSize = mpu.dmpGetFIFOPacketSize(); Serial.print("DMP initialzed"); } }
void loop() { int sensorReading = analogRead(A0); uduino.readSerial();
if (uduino.isInit() && dmpReady) {
ifdef OUTPUT_READABLE_QUATERNION
endif
} }
void dmpDataReady() { mpuInterrupt = true; }`
The result was really smooth and nice, however rotation along the Z axis was too imprecise. After researching on the subject, it looks like the sensor can't do it appropriately without a magnetometer, which is why I bought a generic MPU9250.
However, I had a lot of trouble making it work.
First I tried this :
`/* MPU9250 Basic Example Code by: Kris Winer date: April 1, 2014 license: Beerware - Use this code however you'd like. If you find it useful you can buy me a beer some time. Modified by Brent Wilkins July 19, 2016
Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
SDA and SCL should have external pull-up resistors (to 3.3V). 10k resistors are on the EMSENSR-9250 breakout board.
Hardware setup: MPU9250 Breakout --------- Arduino VDD ---------------------- 3.3V VDDI --------------------- 3.3V SDA ----------------------- A4 SCL ----------------------- A5 GND ---------------------- GND */
include "quaternionFilters.h"
include "MPU9250.h"
include "Uduino.h" // Include Uduino library at the top of the sketch
Uduino uduino("IMU");
/*
ifdef LCD
include
include
// Using NOKIA 5110 monochrome 84 x 48 pixel display // pin 9 - Serial clock out (SCLK) // pin 8 - Serial data out (DIN) // pin 7 - Data/Command select (D/C) // pin 5 - LCD chip select (CS) // pin 6 - LCD reset (RST) Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
endif // LCD
*/
define AHRS true // Set to false for basic data read
define SerialDebug true // Set to true to get Serial output for debugging
// Pin definitions int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins int myLed = 13; // Set up pin 13 led for toggling
MPU9250 myIMU;
void setup() { Wire.begin(); // TWBR = 12; // 400 kbit/sec I2C speed Serial.begin(38400);
// Set up the interrupt pin, its set as active high, push-pull pinMode(intPin, INPUT); digitalWrite(intPin, LOW); pinMode(myLed, OUTPUT); digitalWrite(myLed, HIGH); /*
ifdef LCD
display.begin(); // Ini8ialize the display display.setContrast(58); // Set the contrast
// Start device display with ID of sensor display.clearDisplay(); display.setTextSize(2); display.setCursor(0,0); display.print("MPU9250"); display.setTextSize(1); display.setCursor(0, 20); display.print("9-DOF 16-bit"); display.setCursor(0, 30); display.print("motion sensor"); display.setCursor(20,40); display.print("60 ug LSB"); display.display(); delay(1000);
// Set up for data display display.setTextSize(1); // Set text size to normal, 2 is twice normal etc. display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen display.clearDisplay(); // clears the screen and buffer
endif // LCD
*/
// Read the WHO_AM_I register, this is a good test of communication byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); // Serial.print(" I should be "); Serial.println(0x71, HEX); /*
ifdef LCD
display.setCursor(20,0); display.print("MPU9250"); display.setCursor(0,10); display.print("I AM"); display.setCursor(0,20); display.print(c, HEX); display.setCursor(0,30); display.print("I Should Be"); display.setCursor(0,40); display.print(0x71, HEX); display.display(); delay(1000);
endif // LCD
*/ if (c == 0x71) // WHO_AM_I should always be 0x68 { // Serial.println("MPU9250 is online...");
/*
ifdef LCD
endif // LCD
*/
// Serial.println("MPU9250 initialized for active data mode....");
// Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); // Serial.print(" I should be "); Serial.println(0x48, HEX); /*
ifdef LCD
endif // LCD
*/
// Serial.println("AK8963 initialized for active data mode...."); if (SerialDebug) { // Serial.println("Calibration values: "); / Serial.print("X-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[0], 2); Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[1], 2); Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(myIMU.magCalibration[2], 2); / } /*
ifdef LCD
endif // LCD
*/ } // if (c == 0x71) else { // Serial.print("Could not connect to MPU9250: 0x"); // Serial.println(c, HEX); while(1) ; // Loop forever if communication doesn't happen } }
void loop() { int sensorReading = analogRead(A0); uduino.readSerial(); // This part is needed to be detected by Unity. if (uduino.isInit()) {
// If intPin goes high, all data registers have new data // On interrupt, check if data ready interrupt if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values myIMU.getAres();
} // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
// Must be called before updating quaternions! myIMU.updateTime();
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis // (+ up) of accelerometer and gyro! We have to make some allowance for this // orientationmismatch in feeding the output to the quaternion filter. For the // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward // along the x-axis just like in the LSM9DS0 sensor. This rotation can be // modified to allow any convenient orientation convention. This is ok by // aircraft orientation standards! Pass gyro rate as rad/s // MadgwickQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gzPI/180.0f, my, mx, mz); MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gxDEG_TO_RAD, myIMU.gyDEG_TO_RAD, myIMU.gzDEG_TO_RAD, myIMU.my, myIMU.mx, myIMU.mz, myIMU.deltat);
if (!AHRS) { myIMU.delt_t = millis() - myIMU.count; if (myIMU.delt_t > 500) { if(SerialDebug) { / // Print acceleration values in milligs! Serial.print("X-acceleration: "); Serial.print(1000myIMU.ax); Serial.print(" mg "); Serial.print("Y-acceleration: "); Serial.print(1000myIMU.ay); Serial.print(" mg "); Serial.print("Z-acceleration: "); Serial.print(1000myIMU.az); Serial.println(" mg ");
/* myIMU.tempCount = myIMU.readTempData(); // Read the adc values // Temperature in degrees Centigrade myIMU.temperature = ((float) myIMU.tempCount) / 333.87 + 21.0; // Print temperature in degrees Centigrade
/*
ifdef LCD
display.clearDisplay(); display.setCursor(0, 0); display.print("MPU9250/AK8963"); display.setCursor(0, 8); display.print(" x y z ");
display.setCursor(0, 16); display.print((int)(1000myIMU.ax)); display.setCursor(24, 16); display.print((int)(1000myIMU.ay)); display.setCursor(48, 16); display.print((int)(1000*myIMU.az)); display.setCursor(72, 16); display.print("mg");
display.setCursor(0, 24); display.print((int)(myIMU.gx)); display.setCursor(24, 24); display.print((int)(myIMU.gy)); display.setCursor(48, 24); display.print((int)(myIMU.gz)); display.setCursor(66, 24); display.print("o/s");
display.setCursor(0, 32); display.print((int)(myIMU.mx)); display.setCursor(24, 32); display.print((int)(myIMU.my)); display.setCursor(48, 32); display.print((int)(myIMU.mz)); display.setCursor(72, 32); display.print("mG");
display.setCursor(0, 40); display.print("Gyro T "); display.setCursor(50, 40); display.print(myIMU.temperature, 1); display.print(" C"); display.display();
endif // LCD
*/
} // if (!AHRS) else { // Serial print and/or display at 0.5 s rate independent of data rates myIMU.delt_t = millis() - myIMU.count;
// if (myIMU.delt_t > 500) // { / Serial.print(""); Serial.print(getQ()); Serial.print("/"); Serial.print((getQ() + 1)); Serial.print("/"); Serial.print((getQ() + 2)); Serial.print("/"); Serial.print((getQ() + 3)); Serial.print("/"); Serial.println(sensorReading); /
// Define output variables from updated quaternion---these are Tait-Bryan // angles, commonly used in aircraft orientation. In this coordinate system, // the positive z-axis is down toward Earth. Yaw is the angle between Sensor // x-axis and Earth magnetic North (or true North if corrected for local // declination, looking down on the sensor positive yaw is counterclockwise. // Pitch is angle between sensor x-axis and Earth ground plane, toward the // Earth is positive, up toward the sky is negative. Roll is angle between // sensor y-axis and Earth ground plane, y-axis up is positive roll. These // arise from the definition of the homogeneous rotation matrix constructed // from quaternions. Tait-Bryan angles as well as Euler angles are // non-commutative; that is, the get the correct orientation the rotations // must be applied in the correct order which for this configuration is yaw, // pitch, and then roll. // For more see // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles // which has additional links.
/* if(SerialDebug) { Serial.print("Yaw, Pitch, Roll: "); Serial.print(myIMU.yaw, 2); Serial.print(", "); Serial.print(myIMU.pitch, 2); Serial.print(", "); Serial.println(myIMU.roll, 2);
ifdef LCD
endif // LCD
*/ myIMU.count = millis(); myIMU.sumCount = 0; myIMU.sum = 0; // } // if (myIMU.delt_t > 500) } // if (AHRS) delay(10); } // if (uduino.isInit()) delay(10);`
However the result is very very noisy and incorrect along certains axes.
(Uduino is the library to communicate between Arduino and Unity, and it requires to print only the information you need, which is why I negated the orther printings).
I also tried using Quaternions with :
Serial.print(""); Serial.print(*getQ()); Serial.print("/"); Serial.print(*(getQ() + 1)); Serial.print("/"); Serial.print(*(getQ() + 2)); Serial.print("/"); Serial.print(*(getQ() + 3)); Serial.print("/"); Serial.println(sensorReading);But it didn't work either.
The Unity Code is the following one :
string[] values = data.Split('/'); float w = float.Parse(values[0]); // ROLL float x = float.Parse(values[1]); // PITCH float y = float.Parse(values[2]); // YAW float z = float.Parse(values[3]); // SENSORREADING float translation = float.Parse(values[4]); rotation.x = y; // YAW rotation.y = x; // PITCH rotation.z = w; // ROLL this.transform.rotation = Quaternion.Euler(rotation);I also tried with Quaternion with (w, x, y, z) and Quaternion.Inverse(w, x, y, z) but it was worse than using euler Angles, it didn't correspond to the movements.
Sorry if my question seems stupid or else, I am a newbie in electronics and Arduino and I am really struggling with this... I googled a read a lot of topics but I can't really understand the problem here. Do I need a filter ?
Here you can see the youtube video of me trying the sensor : https://www.youtube.com/watch?v=rs1C5n4_RBc
Thank you very much for you time