siteswapjuggler
Member
Hello,
I'm actually working on a project which include a MPU9150 (and is recent version the MPU9250) from Invensense. The project is a migration from an Arduino UNO to a Teensy 3.2 which provide faster calculation over my program. Well here is my problem I've a test implementation of the Madgick Algorithm that works perfectly on the Arduino UNO. Appart from being "slow" it output plausible quaternion and data. The same code is compiled without problem with Teensy, BUT the quaternion output stay almost constant whatever the move I'm doing.
Same code -> very different result, and I really don't know where to start to solve the problem.
So far I tried to change the IMU, to test the code on other plateform (Arduino Due get the same problem as described above with Teensy, Uno and other 328p card work as expected).
Here is the code :
Further details :
Source Code : see above
Error Messages : no error messages just warnings
Wiring : GND, 3.3v for VCC, SDA & SCL (respond well to I2C Scanner tests)
Software : Arduino 1.6.11, Teensyduino 1.30 (also tried with 1.6.10 and 1.29)
Libraries : MPU6050 (https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050) and Wire
Any help would save my day
Sylvain
artist, juggler and maker
I'm actually working on a project which include a MPU9150 (and is recent version the MPU9250) from Invensense. The project is a migration from an Arduino UNO to a Teensy 3.2 which provide faster calculation over my program. Well here is my problem I've a test implementation of the Madgick Algorithm that works perfectly on the Arduino UNO. Appart from being "slow" it output plausible quaternion and data. The same code is compiled without problem with Teensy, BUT the quaternion output stay almost constant whatever the move I'm doing.
Same code -> very different result, and I really don't know where to start to solve the problem.
So far I tried to change the IMU, to test the code on other plateform (Arduino Due get the same problem as described above with Teensy, Uno and other 328p card work as expected).
Here is the code :
Code:
/*****************************************************************
How to calculate actual acceleration, rotation speed, magnetic
field strength using the specified ranges as described in the data sheet:
http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/IMU/PS-MPU-9150A.pdf
and
http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/IMU/RM-MPU-9150A-00.pdf.
*****************************************************************/
#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>
#include <helper_3dmath.h>
// Declare device MPU6050 class
MPU6050 mpu;
Quaternion q(1.,0,0,0);
int16_t a1, a2, a3, g1, g2, g3, m1, m2, m3; // raw data arrays reading
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
float deltat = 0.0f; // integration interval for both filter schemes
uint16_t lastUpdate = 0; // used to calculate integration interval
uint16_t now = 0; // used to calculate integration interval
// Full-scale range of the gyro sensors:
// 0 = +/- 250 degrees/sec, 1 = +/- 500 degrees/sec, 2 = +/- 1000 degrees/sec, 3 = +/- 2000 degrees/sec
byte gyroRange = 0;
float aScale = 250.0f * (1<<gyroRange) / 32768.0f;
// Full-scale accelerometer range.
// The full-scale range of the accelerometer: 0 = +/- 2g, 1 = +/- 4g, 2 = +/- 8g, 3 = +/- 16g
byte accelRange = 0;
float gScale = 2.0f * (1<<accelRange) / 32768.0f;
float d2r = PI / 180.0f;
float mScale = 10.0f * 1229.0f / 4096.0f;
void setup()
{
Wire.begin();
Wire.setClock(400000L); // très bonne astuce pour accélérer l'acquisition en I2C
Serial.begin(115200);
mpu.initialize();
mpu.setMasterClockSpeed(13);
// Set up the accelerometer, gyro, and magnetometer for data output
mpu.setRate(7); // set gyro rate to 8 kHz/(1 * rate) shows 1 kHz, accelerometer ODR is fixed at 1 KHz
// Digital low pass filter configuration.
// It also determines the internal sampling rate used by the device as shown in the table below.
// The accelerometer output rate is fixed at 1kHz. This means that for a Sample
// Rate greater than 1kHz, the same accelerometer sample may be output to the
// FIFO, DMP, and sensor registers more than once.
/*
| ACCELEROMETER | GYROSCOPE
DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate
---------+-----------+--------+-----------+--------+-------------
0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz
1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz
2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz
3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz
4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz
5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz
6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz
*/
//mpu.setDLPFMode(0); // set bandwidth of both gyro and accelerometer to ~20 Hz
mpu.setFullScaleGyroRange(gyroRange); // set gyro range to 250 degrees/sec
mpu.setFullScaleAccelRange(accelRange); // set accelerometer to 2 g range
}
void loop() {
long test = micros();
mpu.getAcceleration(&a1,&a2,&a3);
mpu.getRotation(&g1,&g2,&g3);
mpu.getMagnetometer(&m1,&m2,&m3);
Serial.println("acquisition "+String(micros()-test));
test = micros();
float ax = a1 * aScale; //
float ay = a2 * aScale; //
float az = a3 * aScale; //
float gx = g1 * gScale * d2r; //
float gy = g2 * gScale * d2r; //
float gz = g3 * gScale * d2r; //
float mx = m1 * mScale + 18.0f; // milliGauss (1229 microTesla per 2^12 bits, 10 mG per microTesla)
float my = m2 * mScale + 70.0f; // apply calibration offsets in mG that correspond to your environment and magnetometer
float mz = m3 * mScale + 270.0f; //
Serial.println("calculst "+String(micros()-test));
now = micros(); //
deltat = ((now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = now; //
test = micros();
MadgwickQuaternionUpdate(&q,ax,ay,az,gx,gy,gz,my,mx,mz); // Madgwick quaternion update
Serial.println("madgwickt "+String(micros()-test));
test = micros();
VectorInt16 world; // create a vector to store world acceleration
VectorInt16 accel(a1, a2, a3); // create a vector with acceleration parameters
getWorldAccel(&world, &accel, &q); // adjust acceleration vector with the quaternion
Serial.println("worldt "+String(micros()-test));
test = micros();
Serial.println("quat " + String(q.w) + " " + String(q.x) + " " + String(q.y) + " " + String(q.y)); // teta, x, y, z
Serial.println("accel " + String(a1) + " " + String(a2) + " " + String(a3)); // x, y, z
Serial.println("magnet " + String(m1) + " " + String(m2) + " " + String(m3)); // x, y, z
Serial.println("gyro " + String(g1) + " " + String(g2) + " " + String(g3)); // x, y, z
Serial.println("world " + String(world.x) + " " + String(world.y) + " " + String(world.z)); // x, y, z
Serial.println("printt "+String(micros()-test));
delay(25);
}
// CALIBRATION
//
// The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally
// pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis.
// The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors.
// The magnetometer is a different thing. Most magnetometers will be sensitive to circuit currents, computers, and
// other both man-made and natural sources of magnetic field. The rough way to calibrate the magnetometer is to record
// the maximum and minimum readings (generally achieved at the North magnetic direction). The average of the sum divided by two
// should provide a pretty good calibration offset. Don't forget that for the MPU9150, the magnetometer x- and y-axes are switched
// compared to the gyro and accelerometer!
// ORIENTATION CORRECTION
//
// 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-9150, 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
// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
#define GyroMeasError PI * (40.0f / 180.0f) // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f) // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
#define beta sqrt(3.0f / 4.0f) * GyroMeasError // compute beta
#define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void MadgwickQuaternionUpdate(Quaternion *q, float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q->w, q2 = q->x, q3 = q->y, q4 = q->z; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
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;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm;
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;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f / norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f / norm;
q->w = q1 * norm;
q->x = q2 * norm;
q->y = q3 * norm;
q->z = q4 * norm;
}
uint8_t getWorldAccel(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
memcpy(v, vReal, sizeof(VectorInt16));
v -> rotate(q);
return 0;
}
Further details :
Source Code : see above
Error Messages : no error messages just warnings
Wiring : GND, 3.3v for VCC, SDA & SCL (respond well to I2C Scanner tests)
Software : Arduino 1.6.11, Teensyduino 1.30 (also tried with 1.6.10 and 1.29)
Libraries : MPU6050 (https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050) and Wire
Any help would save my day
Sylvain
artist, juggler and maker