Teensy, IMU & Madgick Algorithm [kind of a strange bug]

Status
Not open for further replies.
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 :

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
 
Hi and thanks a lot for your answers,

I'll take a try with the code you linked, I should have old version of them. My present code come from the AHRS example of Sparkfun.
To be honest I should be able to deal to get a working code starting with clean examples.

Anyhow I would like to understand how my above code could be valid and working on a 328p and valid and not working on Teensy. It's the first time I meet this kind of problem and it totally bugs my mind to understand how it could be possible.
 
Hi and thanks a lot for your answers,

I'll take a try with the code you linked, I should have old version of them. My present code come from the AHRS example of Sparkfun.
To be honest I should be able to deal to get a working code starting with clean examples.

Anyhow I would like to understand how my above code could be valid and working on a 328p and valid and not working on Teensy. It's the first time I meet this kind of problem and it totally bugs my mind to understand how it could be possible.

one thing you might fix is that micros() returns uint32_t -- you need to fix your now lastUpdate declarations.

some other MPU9250 speed numbers using onehorse's sketches
https://forum.pjrc.com/threads/24633-Any-Chance-of-a-Teensy-3-1?p=108823&viewfull=1#post108823
 
one thing you might fix is that micros() returns uint32_t -- you need to fix your now lastUpdate declarations.
This may very well be the problem. C and C++ have brain-damaged integer auto promotion rules. On AVR Arduino, int is 16 bits, so uint16_t math "(now - lastUpdate)" is done without integer promotion (the full range of uint16_t doesn't fit in int). This calculation relies on uint16_t overflow handling/wrapping to work right.

On ARM, int is 32-bits, so in "(now - lastUpdate)" "now" and "lastUpdate" are promoted to 32-bit signed ints and the overflow doesn't cancel out anymore.
 
This may very well be the problem. C and C++ have brain-damaged integer auto promotion rules. On AVR Arduino, int is 16 bits, so uint16_t math "(now - lastUpdate)" is done without integer promotion (the full range of uint16_t doesn't fit in int). This calculation relies on uint16_t overflow handling/wrapping to work right.

On ARM, int is 32-bits, so in "(now - lastUpdate)" "now" and "lastUpdate" are promoted to 32-bit signed ints and the overflow doesn't cancel out anymore.

Thanks a lot to both of you it actually solved the problem. It seems very clear to me now. I will take more time to explore the link you gave me.
I'm very impressed by the refresh rates from onehorse sketches.

I noticed that everybody focused on I2C communication over SPI (which MPU9250 is capable of). Is this really the best way to do ?
 
I hope you don't mind if I got some more question about IMU, I feel like there is experimented people here and that it would clear my minds from a lot of future misunderstanding.
So here is some random question I have right now :

- SPI vs I2C speed for MPU9250
- efficiency and reliability the internal Invense Fusion algorythm over home made algorythm from the mpu side ?
- is interrupt implementation for the MPU compatible with timing methods in the program (micros, millis, elapsedMicros/Millis), I've read somewhere that interupt pauses function like delays and timers ?
- what about fixed point maths to optimize the calculation process of madgwick algorithm over floating point (like Paul did with the Audio library which relies on q15) ?
- any thought over MPU9250 alternative like the BNO055 which also have ebemded fusion algorithm ?
- how does calibration work ? good tutorial, papers to start with ?
 
I noticed that everybody focused on I2C communication over SPI (which MPU9250 is capable of). Is this really the best way to do ?

Glad it's working.

You might be constrained to I2C by the shield or breakout, or you may already have other I2C sensors in your project. SPI requires more wires, but it would improve performance. I2C status query @400khz takes 120us, SPI@1MHz would take 20 us. if SPI clocked at 20MHz, query would be 2 us. Your API might also support SPI DMA, greatly reducing sensor data collection times, though I2C DMA has been demonstrated on dragonfly sketch. Your application may not require such high speeds.

Kris (onehorse) has written about IMUs, e.g. https://github.com/kriswiner/MPU-6050/wiki/Affordable-9-DoF-Sensor-Fusion
and https://github.com/kriswiner/MPU-9250

you might also search the teensy propshield beta test thread for mention of Kalman filters vs Madgwick ...
 
Last edited:
Status
Not open for further replies.
Back
Top