9- and 10-DoF LSM9DS0 shield for Teensy 3.1

Status
Not open for further replies.
I agree that current libraries don't make it easy to use alternate or multiple I2C buses.

The sketch is now using Wire1. However, the i2c_t3 library is unchanged and I'm pretty sure that it doesn't need to be, coz I've an LSM9DS0 microshield here running just fine using Wire1 and i2c_t3.

The device has 4k7 pull-ups. Pulses look good and square. I doubt that's the issue.
 
Sorry I lost track of this thread.

"MPU9250 I AM C0 I should be 71
Could not connect to MPU9250: 0xC0"

This is very weird. It looks like you are reading the wrong register. Can you verify that you get an ACK on I2C address 0x69 or 0x68 (you should probably choose to pull the ADO pin to GND for 0x68)? Can you simply try to read some MPU registers? If you can read then, then I am not sure what is happening. I suppose a short on 3V3 or GND or one of the other pins could cause this. Did you try to confirm function of the MPU9250 module before soldering? I do before I ship them out so it was working properly, I am just wondering if something happened in the process of soldering the module to the Teensy...
 
(For future reference, AD0 on the MPU-9250 microshield corresponds to pin 24 on the Teensy 3.1.)

Toggling pin 24/AD0 changes the device address from 0x68 to 0x69 and back, as expected. The I2C scanner recieves an ACK from the valid address.

Trying to read the register map (0-126, from https://strawberry-linux.com/pub/RM-MPU-9250A-00.pdf) gives me a return of FF for each register. I don't know if that's a problem with hardware or software.

Do you have test code for this specific setup?

I'm not sure how to test the microshield without soldering it onto the back of a Teensy. It's a tad tricky to connect to. I do have another one here, untouched, so I can try whatever you want to suggest.
 
Last edited:
I suspect there is something wrong with your I2C read function. Please post it here.

I use the spring-loaded hook jumpers to test all of the modules. It can be tricky, but it least it let's me find duds for rework before I inflict too much frustration on you the user (didn't succeed here apparently!).

I think the MPU9250 module must be working fin if you are getting i2C ACK and can change the I2C address. It's probably the wire1 business messing you up.

Here is what the readByte function should look like:

uint8_t readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
Wire1.beginTransmission(address); // Initialize the Tx buffer
Wire1.write(subAddress); // Put slave register address in Tx buffer
Wire1.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
Wire1.requestFrom(address, (size_t) 1); // Read one byte from slave register address
data = Wire1.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}

with the corresponding wire1 calls for the other readBytes and writeByte function and with wire1.begin at the top.

Please post the code you are using; maybe there is some obvious problem we can see...
 
I suspect there is something wrong with your I2C read function. Please post it here.

I use the spring-loaded hook jumpers to test all of the modules. It can be tricky, but it least it let's me find duds for rework before I inflict too much frustration on you the user (didn't succeed here apparently!).

I think the MPU9250 module must be working fin if you are getting i2C ACK and can change the I2C address. It's probably the wire1 business messing you up.

Here is what the readByte function should look like:

uint8_t readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
Wire1.beginTransmission(address); // Initialize the Tx buffer
Wire1.write(subAddress); // Put slave register address in Tx buffer
Wire1.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
Wire1.requestFrom(address, (size_t) 1); // Read one byte from slave register address
data = Wire1.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}

with the corresponding wire1 calls for the other readBytes and writeByte function and with wire1.begin at the top.

Please post the code you are using; maybe there is some obvious problem we can see...
 
Ah yes, that did the trick.

Changed every Wire.fn() to Wire1.fn() throughout the whole sketch and now I'm getting good comms and good data from the sensors. Thanks for helping with that.

To avoid someone else having this problem, would it be possible for you to provide a test sketch specifically for the microshield? The code at https://github.com/kriswiner/MPU-9250/blob/master/MPU9250_MS5637_AHRS_t3.ino only works for the minishield and doesn't say that it only works for the mini, not the micro.

Anyway, now that that's working, I can get on with the rest of the staff. Thanks again.
 
Last edited:
I Ressurect this discussion about the 9dof sensor.
I'm playing with a LSM9DS0 on the GPS board from smoking resistor, and i'm a bit puzzled about the behaviour of the behaviour of the tain bain angles.
The sketch is forked from onehorse, and all the math is the same.
I get stable quaternions but i've noticed that if i give a roll to the board i see also the yaw angle changing that is not supposed to happen, by changing i mean that if a start with a yaw angle roll and pitch about 0 and i roll the board of 45° i see the yaw angle changing by 57°, while the pitch angle stays the same as it's supposed to be, if i pitch the board the yaw stays the same (1° change) and the roll stays the same.
Do i miss something about tait bayn angles or there is something wrong (like magetometer calibration or something like that) ?
 
Could be poor sensor calibration (likely) or that you are passing the sensor data to the fusion algorithm inconsistently. Without a look at the board and code, it's hard to be sure.
 
Could be poor sensor calibration (likely) or that you are passing the sensor data to the fusion algorithm inconsistently. Without a look at the board and code, it's hard to be sure.

I'm updating the madgwick algorithm at 500hz and feeding it with data at 100hz, i will post the code.
 
In the main GPS sketch i call this 2 lines:

Code:
......................
Metro read_dof= Metro(10);
Metro update_quat=Metro(2);   
......................

if (read_dof.check())
      {
          sensor_9dof_read();
          bmp280_pressure = bme.readPressure()/100.0;
          read_dof.reset();
          graph_9dof_data();
      }
      if (update_quat.check())
      {
          update_quaternions();
          update_quat.reset();
      }

The AHRS (it's a bit messy, still contains several attempts of fixing the thing) code is:

Code:
#include "LSM9DS0.h"
#include <MovingAverageFilter.h>
#include <Filters.h>

#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW

#define ACC_RUNAVG   5 // Number of Run average samples for Accelerometers
#define GYR_RUNAVG   5 // Number of Run average samples for Gyros

int INT1XM = 16; // INT1XM tells us when accel data is ready
int INT2XM = 17; // INT2XM tells us when mag data is ready
int DRDYG  = 12; // DRDYG  tells us when gyro data is ready

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
//For Mahoni

#define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (1.0f / 180.0f)      // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
// 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.
#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 //0.5*(2.0f * 0.5f)// 2.0f  these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f //0.5*(2.0f * 0.1f) //0.005f


float eInt[3] = {0.0f, 0.0f, 0.0f};
volatile float integralFBx, integralFBy, integralFBz;
float sampleFreq=500;


MovingAverageFilter gx_avg(GYR_RUNAVG);
MovingAverageFilter gy_avg(GYR_RUNAVG);
MovingAverageFilter gz_avg(GYR_RUNAVG);

MovingAverageFilter ax_avg(ACC_RUNAVG);
MovingAverageFilter ay_avg(ACC_RUNAVG);
MovingAverageFilter az_avg(ACC_RUNAVG);
/*
FilterOnePole high_pass_gx( HIGHPASS, 0.5 ); 
FilterOnePole high_pass_gy( HIGHPASS, 0.5 ); 
FilterOnePole high_pass_gz( HIGHPASS, 0.5 ); 
*/
/*
FilterTwoPole high_pass_gx( HIGHPASS_BESSEL, 0.2 ); 
FilterTwoPole high_pass_gy( HIGHPASS_BESSEL, 0.2 ); 
FilterTwoPole high_pass_gz( HIGHPASS_BESSEL, 0.2 ); 
*/




static const int16_t dec_tbl[37][73] = \
{ \
    {150,145,140,135,130,125,120,115,110,105,100,95,90,85,80,75,70,65,60,55,50,45,40,35,30,25,20,15,10,5,0,-4,-9,-14,-19,-24,-29,-34,-39,-44,-49,-54,-59,-64,-69,-74,-79,-84,-89,-94,-99,104,109,114,119,124,129,134,139,144,149,154,159,164,169,174,179,175,170,165,160,155,150}, \
    {143,137,131,126,120,115,110,105,100,95,90,85,80,75,71,66,62,57,53,48,44,39,35,31,27,22,18,14,9,5,1,-3,-7,-11,-16,-20,-25,-29,-34,-38,-43,-47,-52,-57,-61,-66,-71,-76,-81,-86,-91,-96,101,107,112,117,123,128,134,140,146,151,157,163,169,175,178,172,166,160,154,148,143}, \
    {130,124,118,112,107,101,96,92,87,82,78,74,70,65,61,57,54,50,46,42,38,34,31,27,23,19,16,12,8,4,1,-2,-6,-10,-14,-18,-22,-26,-30,-34,-38,-43,-47,-51,-56,-61,-65,-70,-75,-79,-84,-89,-94,100,105,111,116,122,128,135,141,148,155,162,170,177,174,166,159,151,144,137,130}, \
    {111,104,99,94,89,85,81,77,73,70,66,63,60,56,53,50,46,43,40,36,33,30,26,23,20,16,13,10,6,3,0,-3,-6,-9,-13,-16,-20,-24,-28,-32,-36,-40,-44,-48,-52,-57,-61,-65,-70,-74,-79,-84,-88,-93,-98,103,109,115,121,128,135,143,152,162,172,176,165,154,144,134,125,118,111}, \
    {85,81,77,74,71,68,65,63,60,58,56,53,51,49,46,43,41,38,35,32,29,26,23,19,16,13,10,7,4,1,-1,-3,-6,-9,-13,-16,-19,-23,-26,-30,-34,-38,-42,-46,-50,-54,-58,-62,-66,-70,-74,-78,-83,-87,-91,-95,100,105,110,117,124,133,144,159,178,160,141,125,112,103,96,90,85 }, \
    {62,60,58,57,55,54,52,51,50,48,47,46,44,42,41,39,36,34,31,28,25,22,19,16,13,10,7,4,2,0,-3,-5,-8,-10,-13,-16,-19,-22,-26,-29,-33,-37,-41,-45,-49,-53,-56,-60,-64,-67,-70,-74,-77,-80,-83,-86,-89,-91,-94,-97,101,105,111,130,109,84,77,74,71,68,66,64,62 }, \
    {46,46,45,44,44,43,42,42,41,41,40,39,38,37,36,35,33,31,28,26,23,20,16,13,10,7,4,1,-1,-3,-5,-7,-9,-12,-14,-16,-19,-22,-26,-29,-33,-36,-40,-44,-48,-51,-55,-58,-61,-64,-66,-68,-71,-72,-74,-74,-75,-74,-72,-68,-61,-48,-25,2,22,33,40,43,45,46,47,46,46 }, \
    {36,36,36,36,36,35,35,35,35,34,34,34,34,33,32,31,30,28,26,23,20,17,14,10,6,3,0,-2,-4,-7,-9,-10,-12,-14,-15,-17,-20,-23,-26,-29,-32,-36,-40,-43,-47,-50,-53,-56,-58,-60,-62,-63,-64,-64,-63,-62,-59,-55,-49,-41,-30,-17,-4,6,15,22,27,31,33,34,35,36,36 }, \
    {30,30,30,30,30,30,30,29,29,29,29,29,29,29,29,28,27,26,24,21,18,15,11,7,3,0,-3,-6,-9,-11,-12,-14,-15,-16,-17,-19,-21,-23,-26,-29,-32,-35,-39,-42,-45,-48,-51,-53,-55,-56,-57,-57,-56,-55,-53,-49,-44,-38,-31,-23,-14,-6,0,7,13,17,21,24,26,27,29,29,30 }, \
    {25,25,26,26,26,25,25,25,25,25,25,25,25,26,25,25,24,23,21,19,16,12,8,4,0,-3,-7,-10,-13,-15,-16,-17,-18,-19,-20,-21,-22,-23,-25,-28,-31,-34,-37,-40,-43,-46,-48,-49,-50,-51,-51,-50,-48,-45,-42,-37,-32,-26,-19,-13,-7,-1,3,7,11,14,17,19,21,23,24,25,25 }, \
    {21,22,22,22,22,22,22,22,22,22,22,22,22,22,22,22,21,20,18,16,13,9,5,1,-3,-7,-11,-14,-17,-18,-20,-21,-21,-22,-22,-22,-23,-23,-25,-27,-29,-32,-35,-37,-40,-42,-44,-45,-45,-45,-44,-42,-40,-36,-32,-27,-22,-17,-12,-7,-3,0,3,7,9,12,14,16,18,19,20,21,21 }, \
    {18,19,19,19,19,19,19,19,19,19,19,19,19,19,19,19,18,17,16,14,10,7,2,-1,-6,-10,-14,-17,-19,-21,-22,-23,-24,-24,-24,-24,-23,-23,-23,-24,-26,-28,-30,-33,-35,-37,-38,-39,-39,-38,-36,-34,-31,-28,-24,-19,-15,-10,-6,-3,0,1,4,6,8,10,12,14,15,16,17,18,18 }, \
    {16,16,17,17,17,17,17,17,17,17,17,16,16,16,16,16,16,15,13,11,8,4,0,-4,-9,-13,-16,-19,-21,-23,-24,-25,-25,-25,-25,-24,-23,-21,-20,-20,-21,-22,-24,-26,-28,-30,-31,-32,-31,-30,-29,-27,-24,-21,-17,-13,-9,-6,-3,-1,0,2,4,5,7,9,10,12,13,14,15,16,16 }, \
    {14,14,14,15,15,15,15,15,15,15,14,14,14,14,14,14,13,12,11,9,5,2,-2,-6,-11,-15,-18,-21,-23,-24,-25,-25,-25,-25,-24,-22,-21,-18,-16,-15,-15,-15,-17,-19,-21,-22,-24,-24,-24,-23,-22,-20,-18,-15,-12,-9,-5,-3,-1,0,1,2,4,5,6,8,9,10,11,12,13,14,14 }, \
    {12,13,13,13,13,13,13,13,13,13,13,13,12,12,12,12,11,10,9,6,3,0,-4,-8,-12,-16,-19,-21,-23,-24,-24,-24,-24,-23,-22,-20,-17,-15,-12,-10,-9,-9,-10,-12,-13,-15,-17,-17,-18,-17,-16,-15,-13,-11,-8,-5,-3,-1,0,1,1,2,3,4,6,7,8,9,10,11,12,12,12 }, \
    {11,11,11,11,11,12,12,12,12,12,11,11,11,11,11,10,10,9,7,5,2,-1,-5,-9,-13,-17,-20,-22,-23,-23,-23,-23,-22,-20,-18,-16,-14,-11,-9,-6,-5,-4,-5,-6,-8,-9,-11,-12,-12,-12,-12,-11,-9,-8,-6,-3,-1,0,0,1,1,2,3,4,5,6,7,8,9,10,11,11,11 }, \
    {10,10,10,10,10,10,10,10,10,10,10,10,10,10,9,9,9,7,6,3,0,-3,-6,-10,-14,-17,-20,-21,-22,-22,-22,-21,-19,-17,-15,-13,-10,-8,-6,-4,-2,-2,-2,-2,-4,-5,-7,-8,-8,-9,-8,-8,-7,-5,-4,-2,0,0,1,1,1,2,2,3,4,5,6,7,8,9,10,10,10 }, \
    {9,9,9,9,9,9,9,10,10,9,9,9,9,9,9,8,8,6,5,2,0,-4,-7,-11,-15,-17,-19,-21,-21,-21,-20,-18,-16,-14,-12,-10,-8,-6,-4,-2,-1,0,0,0,-1,-2,-4,-5,-5,-6,-6,-5,-5,-4,-3,-1,0,0,1,1,1,1,2,3,3,5,6,7,8,8,9,9,9  }, \
    {9,9,9,9,9,9,9,9,9,9,9,9,8,8,8,8,7,5,4,1,-1,-5,-8,-12,-15,-17,-19,-20,-20,-19,-18,-16,-14,-11,-9,-7,-5,-4,-2,-1,0,0,1,1,0,0,-2,-3,-3,-4,-4,-4,-3,-3,-2,-1,0,0,0,0,0,1,1,2,3,4,5,6,7,8,8,9,9  }, \
    {9,9,9,8,8,8,9,9,9,9,9,8,8,8,8,7,6,5,3,0,-2,-5,-9,-12,-15,-17,-18,-19,-19,-18,-16,-14,-12,-9,-7,-5,-4,-2,-1,0,0,1,1,1,1,0,0,-1,-2,-2,-3,-3,-2,-2,-1,-1,0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,8,9  }, \
    {8,8,8,8,8,8,9,9,9,9,9,9,8,8,8,7,6,4,2,0,-3,-6,-9,-12,-15,-17,-18,-18,-17,-16,-14,-12,-10,-8,-6,-4,-2,-1,0,0,1,2,2,2,2,1,0,0,-1,-1,-1,-2,-2,-1,-1,0,0,0,0,0,0,0,0,0,1,2,3,4,5,6,7,8,8  }, \
    {8,8,8,8,9,9,9,9,9,9,9,9,9,8,8,7,5,3,1,-1,-4,-7,-10,-13,-15,-16,-17,-17,-16,-15,-13,-11,-9,-6,-5,-3,-2,0,0,0,1,2,2,2,2,1,1,0,0,0,-1,-1,-1,-1,-1,0,0,0,0,-1,-1,-1,-1,-1,0,0,1,3,4,5,7,7,8  }, \
    {8,8,9,9,9,9,10,10,10,10,10,10,10,9,8,7,5,3,0,-2,-5,-8,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8,-6,-4,-2,-1,0,0,1,2,2,3,3,2,2,1,0,0,0,0,0,0,0,0,0,0,-1,-1,-2,-2,-2,-2,-2,-1,0,0,1,3,4,6,7,8  }, \
    {7,8,9,9,9,10,10,11,11,11,11,11,10,10,9,7,5,3,0,-2,-6,-9,-11,-13,-15,-16,-16,-15,-14,-13,-11,-9,-7,-5,-3,-2,0,0,1,1,2,3,3,3,3,2,2,1,1,0,0,0,0,0,0,0,-1,-1,-2,-3,-3,-4,-4,-4,-3,-2,-1,0,1,3,5,6,7  }, \
    {6,8,9,9,10,11,11,12,12,12,12,12,11,11,9,7,5,2,0,-3,-7,-10,-12,-14,-15,-16,-15,-15,-13,-12,-10,-8,-7,-5,-3,-1,0,0,1,2,2,3,3,4,3,3,3,2,2,1,1,1,0,0,0,0,-1,-2,-3,-4,-4,-5,-5,-5,-5,-4,-2,-1,0,2,3,5,6  }, \
    {6,7,8,10,11,12,12,13,13,14,14,13,13,11,10,8,5,2,0,-4,-8,-11,-13,-15,-16,-16,-16,-15,-13,-12,-10,-8,-6,-5,-3,-1,0,0,1,2,3,3,4,4,4,4,4,3,3,3,2,2,1,1,0,0,-1,-2,-3,-5,-6,-7,-7,-7,-6,-5,-4,-3,-1,0,2,4,6  }, \
    {5,7,8,10,11,12,13,14,15,15,15,14,14,12,11,8,5,2,-1,-5,-9,-12,-14,-16,-17,-17,-16,-15,-14,-12,-11,-9,-7,-5,-3,-1,0,0,1,2,3,4,4,5,5,5,5,5,5,4,4,3,3,2,1,0,-1,-2,-4,-6,-7,-8,-8,-8,-8,-7,-6,-4,-2,0,1,3,5  }, \
    {4,6,8,10,12,13,14,15,16,16,16,16,15,13,11,9,5,2,-2,-6,-10,-13,-16,-17,-18,-18,-17,-16,-15,-13,-11,-9,-7,-5,-4,-2,0,0,1,3,3,4,5,6,6,7,7,7,7,7,6,5,4,3,2,0,-1,-3,-5,-7,-8,-9,-10,-10,-10,-9,-7,-5,-4,-1,0,2,4  }, \
    {4,6,8,10,12,14,15,16,17,18,18,17,16,15,12,9,5,1,-3,-8,-12,-15,-18,-19,-20,-20,-19,-18,-16,-15,-13,-11,-8,-6,-4,-2,-1,0,1,3,4,5,6,7,8,9,9,9,9,9,9,8,7,5,3,1,-1,-3,-6,-8,-10,-11,-12,-12,-11,-10,-9,-7,-5,-2,0,1,4  }, \
    {4,6,8,11,13,15,16,18,19,19,19,19,18,16,13,10,5,0,-5,-10,-15,-18,-21,-22,-23,-22,-22,-20,-18,-17,-14,-12,-10,-8,-5,-3,-1,0,1,3,5,6,8,9,10,11,12,12,13,12,12,11,9,7,5,2,0,-3,-6,-9,-11,-12,-13,-13,-12,-11,-10,-8,-6,-3,-1,1,4  }, \
    {3,6,9,11,14,16,17,19,20,21,21,21,19,17,14,10,4,-1,-8,-14,-19,-22,-25,-26,-26,-26,-25,-23,-21,-19,-17,-14,-12,-9,-7,-4,-2,0,1,3,5,7,9,11,13,14,15,16,16,16,16,15,13,10,7,4,0,-3,-7,-10,-12,-14,-15,-14,-14,-12,-11,-9,-6,-4,-1,1,3  }, \
    {4,6,9,12,14,17,19,21,22,23,23,23,21,19,15,9,2,-5,-13,-20,-25,-28,-30,-31,-31,-30,-29,-27,-25,-22,-20,-17,-14,-11,-9,-6,-3,0,1,4,6,9,11,13,15,17,19,20,21,21,21,20,18,15,11,6,2,-2,-7,-11,-13,-15,-16,-16,-15,-13,-11,-9,-7,-4,-1,1,4  }, \
    {4,7,10,13,15,18,20,22,24,25,25,25,23,20,15,7,-2,-12,-22,-29,-34,-37,-38,-38,-37,-36,-34,-31,-29,-26,-23,-20,-17,-13,-10,-7,-4,-1,2,5,8,11,13,16,18,21,23,24,26,26,26,26,24,21,17,12,5,0,-6,-10,-14,-16,-16,-16,-15,-14,-12,-10,-7,-4,-1,1,4  }, \
    {4,7,10,13,16,19,22,24,26,27,27,26,24,19,11,-1,-15,-28,-37,-43,-46,-47,-47,-45,-44,-41,-39,-36,-32,-29,-26,-22,-19,-15,-11,-8,-4,-1,2,5,9,12,15,19,22,24,27,29,31,33,33,33,32,30,26,21,14,6,0,-6,-11,-14,-15,-16,-15,-14,-12,-9,-7,-4,-1,1,4  }, \
    {6,9,12,15,18,21,23,25,27,28,27,24,17,4,-14,-34,-49,-56,-60,-60,-60,-58,-56,-53,-50,-47,-43,-40,-36,-32,-28,-25,-21,-17,-13,-9,-5,-1,2,6,10,14,17,21,24,28,31,34,37,39,41,42,43,43,41,38,33,25,17,8,0,-4,-8,-10,-10,-10,-8,-7,-4,-2,0,3,6  }, \
    {22,24,26,28,30,32,33,31,23,-18,-81,-96,-99,-98,-95,-93,-89,-86,-82,-78,-74,-70,-66,-62,-57,-53,-49,-44,-40,-36,-32,-27,-23,-19,-14,-10,-6,-1,2,6,10,15,19,23,27,31,35,38,42,45,49,52,55,57,60,61,63,63,62,61,57,53,47,40,33,28,23,21,19,19,19,20,22 }, \
    {168,173,178,176,171,166,161,156,151,146,141,136,131,126,121,116,111,106,101,-96,-91,-86,-81,-76,-71,-66,-61,-56,-51,-46,-41,-36,-31,-26,-21,-16,-11,-6,-1,3,8,13,18,23,28,33,38,43,48,53,58,63,68,73,78,83,88,93,98,103,108,113,118,123,128,133,138,143,148,153,158,163,168}, \
};


float deltat = 0.0f;        // integration interval for both filter schemes
uint32_t lastUpdate = 0;    // used to calculate integration interval
uint32_t now = 0;           // used to calculate integration interval
//float abias[3] = {0, 0, 0}, gbias[3] = {-0.7, -6, -4};

float abias[3] = {0, 0, 0}, gbias[3] = {0, 0, 0};

LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);
//Adafruit_BMP280 bme;

/// \fn get_declination
/// \brief Return declination from gps coordinates \n
/// 

uint32_t FreeMem(){ // for Teensy 3.0
    uint32_t stackTop;
    uint32_t heapTop;

    // current position of the stack.
    stackTop = (uint32_t) &stackTop;

    // current position of heap.
    void* hTop = malloc(1);
    heapTop = (uint32_t) hTop;
    free(hTop);

    // The difference is (approximately) the free, available ram.
    return stackTop - heapTop;
}

float get_declination(float lat, float lon)
{
    int16_t decSW, decSE, decNW, decNE, lonmin, latmin;
    float decmin, decmax;
    uint8_t latmin_index, lonmin_index;

    latmin = floor(lat/5)*5;
    lonmin = floor(lon/5)*5;

    latmin_index= (90+latmin)/5;
    lonmin_index= (180+lonmin)/5;

    decSW = dec_tbl[latmin_index][lonmin_index];
    decSE = dec_tbl[latmin_index][lonmin_index+1];
    decNE = dec_tbl[latmin_index+1][lonmin_index+1];
    decNW = dec_tbl[latmin_index+1][lonmin_index];

    decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW;
    decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW;
    return (lat - latmin) / 5 * (decmax - decmin) + decmin;
}

/// \fn MahonyQuaternionUpdate
/// \brief Return quaternions from 9 dof data \n
/// 
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
    {
        float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
        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;   

        // Normalise accelerometer measurement
        norm = invSqrt(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 = invSqrt(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 gravity
        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;
        }
        else
        {
            eInt[0] = 0.0f;     // prevent integral wind up
            eInt[1] = 0.0f;
            eInt[2] = 0.0f;
        }
        // Apply feedback terms
        gx = gx + Kp * ex + Ki * eInt[0];
        gy = gy + Kp * ey + Ki * eInt[1];
        gz = gz + Kp * ez + Ki * eInt[2];

        // 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;

    }
    
/// \fn MadgwickQuaternionUpdate
/// \brief Return quaternions from 9 dof data \n
/// 

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {

  float recipNorm;
  float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;
  if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
     // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
    float hx, hy, bx, bz;
    float halfwx, halfwy, halfwz;
    // Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;
        // Reference direction of Earth's magnetic field
    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    bx = sqrt(hx * hx + hy * hy);
    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
    
    // Estimated direction of magnetic field
    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
    
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex = (my * halfwz - mz * halfwy);
    halfey = (mz * halfwx - mx * halfwz);
    halfez = (mx * halfwy - my * halfwx);
  }
   if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;
    
    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // Compute and apply integral feedback if enabled
    if(Ki > 0.0f) {
      integralFBx += Ki * halfex * (deltat);  // integral error scaled by Ki
      integralFBy += Ki * halfey * (deltat);
      integralFBz += Ki * halfez * (deltat);
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += Kp * halfex;
    gy += Kp * halfey;
    gz += Kp * halfez;
  }
  
  // Integrate rate of change of quaternion
  gx *= (0.5f * (deltat));   // pre-multiply common factors
  gy *= (0.5f * (deltat));
  gz *= (0.5f * (deltat));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

   q[0] = q0;
   q[1] = q1;
   q[2] = q2;
   q[3] = q3;
    
}

void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // 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[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;
}

/// \fn sensor_9dof_configure
/// \brief Configure 9dof sensor \n
/// \detailed
/// 1. Set 4G diapason for accels \n
/// 2. Set 245 degree per second diapason for gyros \n
/// 3. Set 2 Gauss diapason for magnetometers \n
void sensor_9dof_configure()
{
  pinMode(INT1XM, INPUT);
  pinMode(INT2XM, INPUT);
  pinMode(DRDYG,  INPUT);
  dof.begin();
  dof.setAccelScale(dof.A_SCALE_6G);
  dof.setGyroScale(dof.G_SCALE_500DPS);
  dof.setMagScale(dof.M_SCALE_2GS);
  // Set output data rates                   
  dof.setAccelODR(dof.A_ODR_800); // Set accelerometer update rate at 100 Hz
  dof.setAccelABW(dof.A_ABW_50); // Choose lowest filter setting for low noise                            380 Hz (bandwidth 20, 25, 50, 100 Hz), or 760 Hz (bandwidth 30, 35, 50, 100 Hz)
  dof.setGyroODR(dof.G_ODR_95_BW_125);  // Set gyro update rate to 190 Hz with the smallest bandwidth for low noise
  dof.setMagODR(dof.M_ODR_100); // Set magnetometer to update every 80 ms
  dof.calLSM9DS0(gbias, abias);
}

/// \fn
/// \brief Scale accels data by factor 1.5 (see datasheet) \n
///
void scale_accel_16g(){  //need to set abias
  dof.ax = 1.5 * dof.ax;
  dof.ay = 1.5 * dof.ay;
  dof.az = 1.5 * dof.az;
}

/// \fn sensor_9dof_read
/// \brief Read 9dof sensor \n
/// 
void sensor_9dof_read()
{
  float declination;
  static float prev_yaw;
  float gx_q, gy_q, gz_q;
  if(digitalRead(DRDYG)){
    dof.readGyro();              // Read raw gyro data
    
    gx = dof.calcGyro(dof.gx) - gbias[0];   // Convert to degrees per seconds
    gy = dof.calcGyro(dof.gy) - gbias[1];
    gz = dof.calcGyro(dof.gz) - gbias[2];
    

    /*
    gx = high_pass_gx.input(gx_avg.process(dof.calcGyro(dof.gx) - gbias[0]));
    gy = high_pass_gy.input(gy_avg.process(dof.calcGyro(dof.gy) - gbias[1]));
    gz = high_pass_gz.input(gz_avg.process(dof.calcGyro(dof.gz) - gbias[2]));
    */
  }
  if(digitalRead(INT1XM)){
    dof.readAccel();              // Read raw accelerometer data
  #if defined ACCEL_16G
    scale_accel_16g();
  #endif
   /*
    ax = dof.calcAccel(dof.ax)- abias[0];   // Convert to g's
    ay = dof.calcAccel(dof.ay)- abias[1];
    az = dof.calcAccel(dof.az)- abias[2];   
   */
   ax = ax_avg.process(dof.calcAccel(dof.ax)- abias[0]);
   ay = ay_avg.process(dof.calcAccel(dof.ay)- abias[1]);
   az = az_avg.process(dof.calcAccel(dof.az)- abias[2]);
  }
  if(digitalRead(INT2XM)){
    dof.readMag();                // Read raw magnetometer data
    mx = dof.calcMag(dof.mx);     // Convert to Gauss
    my = dof.calcMag(dof.my);
    mz = dof.calcMag(dof.mz);
  }
  dof.readTemp();
  temp = 21.0 + (float) dof.temperature/8.0;
  //Normalize accelerometer raw values.
  float accXnorm = ax/sqrt(pow(ax,2) + pow(ay,2) + pow(az,2));
  float accYnorm = ay/sqrt(pow(ax,2) + pow(ay,2) + pow(az,2));
  //Calculate pitch and roll
  //pitch = asin(-accXnorm);
  //roll = asin(accYnorm/cos(pitch));
  float magXcomp = mx*cos(pitch) - mz*sin(pitch);
  float magYcomp = mx*sin(roll)*sin(pitch)+my*cos(roll)+mz*sin(roll)*cos(pitch);
  float magZcomp = mx*cos(roll)*sin(pitch)+my*sin(roll)-mz*cos(roll)*cos(pitch);
   declination = get_declination (gps.venus838data_filter.Latitude, gps.venus838data_raw.Longitude);
   heading = yaw + declination;
   inclination = 90 - 180*atan2f(sqrt(sq(magYcomp)+sq(magXcomp)),magZcomp)/M_PI;
   if (magZcomp < 0)
      inclination = - inclination;
  /* Removed to differentiate pid frequency from sample freq.

  //MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);  
  //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
  roll = atan2(2*(q[0]*q[1]+q[2]*q[3]),(1-2*(sq(q[1])+sq(q[2]))))*180.0/M_PI;
  pitch = asin(2*(q[0]*q[2]-q[3]*q[1]))*180.0/M_PI;
  yaw = atan2(2*(q[0]*q[3]+q[1]*q[2]), (1-2*(sq(q[2])+sq(q[3]))))*180.0/M_PI;
  if ((az < 0)&&(pitch > 0))
     pitch = 180 - pitch; // pitch change from 0 to 180 degree
  if ((az < 0)&&(pitch < 0))
     pitch = - 180 - pitch; // pitch change from 0 to -180 degree
  declination = get_declination (gps.venus838data_filter.Latitude, gps.venus838data_raw.Longitude);
  heading = yaw + declination;
  inclination = 90 - 180*atan2f(sqrt(sq(magYcomp)+sq(magXcomp)),magZcomp)/M_PI;
  if (magZcomp < 0)
     inclination = - inclination;
  now = micros();
  deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
  lastUpdate = now;
  Serial.println(deltat);
  //print_9dof_data();
  */
  
}

/// \fn print_9dof_data
/// \brief Print 9dof data in serial \n
///

void graph_9dof_data() {

      Serial.print(ax);
      Serial.print(" ");
      Serial.print(ay);
      Serial.print(" ");
      Serial.println(az);
      //Serial.print(" ");
      //Serial.println(q[3]);
      //Serial.print(" ");
      //Serial.print(gy);
      //Serial.print(" ");
      //Serial.println(gz);
  
   }


void update_quaternions(){  
  MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
  //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);  
  //MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
  //roll = atan2(2*(q[0]*q[1]+q[2]*q[3]),(1-2*(sq(q[1])+sq(q[2]))))*180.0/M_PI;
  //pitch = asin(2*(q[0]*q[2]-q[3]*q[1]))*180.0/M_PI;
  //yaw = atan2(2*(q[0]*q[3]+q[1]*q[2]), (1-2*(sq(q[2])+sq(q[3]))))*180.0/M_PI;

   //AHRSupdate(gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, ax, ay, az, mx, my, mz);

    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI;    
    roll  *= 180.0f / PI;
    
  
  if ((az < 0)&&(pitch > 0))
     pitch = 180 - pitch; // pitch change from 0 to 180 degree
  if ((az < 0)&&(pitch < 0))
     pitch = - 180 - pitch; // pitch change from 0 to -180 degree
  // declination = get_declination (gps.venus838data_filter.Latitude, gps.venus838data_raw.Longitude);
 // heading = yaw + declination;
 // inclination = 90 - 180*atan2f(sqrt(sq(magYcomp)+sq(magXcomp)),magZcomp)/M_PI;
 //  if (magZcomp < 0)
  //   inclination = - inclination;
  now = micros();
  deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
  lastUpdate = now;
  //Serial.println(deltat);
  //print_9dof_data();  
}



void print_9dof_data()
{
 
  
  Serial.print("Delta T: "); Serial.println(deltat, 4);

  // print out accelleration data
  Serial.print("Accel X: "); Serial.print(ax*ACC_SCALE, 3); Serial.print(" ");
  Serial.print("  \tY: "); Serial.print(ay*ACC_SCALE, 3);   Serial.print(" ");
  Serial.print("  \tZ: "); Serial.print(az*ACC_SCALE, 3);   Serial.println("  \mg");
  // print out magnetometer data
  Serial.print("Magn. X: "); Serial.print(mx, 3); Serial.print(" ");
  Serial.print("  \tY: "); Serial.print(my, 3);   Serial.print(" ");
  Serial.print("  \tZ: "); Serial.print(mz, 3);   Serial.println("  \tgauss");
  // print out gyroscopic data
  Serial.print("Gyro  X: "); Serial.print(gx, 3); Serial.print(" ");
  Serial.print("  \tY: "); Serial.print(gy, 3);   Serial.print(" ");
  Serial.print("  \tZ: "); Serial.print(gz, 3);   Serial.println("  \tdps");

  // print out temperature data
  Serial.print("Temp: "); Serial.print(temp); Serial.println(" *C");

  // 'orientation' should have valid .roll and .pitch fields 
  Serial.println(F("Orientation: "));
  Serial.print("Roll: ");
  Serial.println(roll, 7);
  Serial.print("Pitch: ");
  Serial.println(pitch, 7);
  Serial.print("Yaw: ");
  Serial.println(yaw, 7);
  Serial.print("Heading: ");
  Serial.println(heading, 7);
  Serial.print("Inclination: ");
  Serial.println(inclination, 7);
  Serial.print("Pressure: "); Serial.print(bmp280_pressure, 3); Serial.println("  \mBar");
  Serial.print(F(" "));
  Serial.println(FreeMem());

  Serial.print(q[0], 7);  
  Serial.print(","); 
  Serial.print(q[1], 7);
  Serial.print(",");
  Serial.print(q[2], 7);
  Serial.print(",");
  Serial.print(q[3], 7);
  Serial.println(",");
  
  
  
  Serial.print(ax,7);
  Serial.print("\t");
  Serial.print(ay,7);
  Serial.print("\t");
  Serial.print(az,7);
  Serial.print("\t");
  Serial.println(0);
  
  
  
}

float invSqrt(float number) {
  union {
    float f;
    int32_t i;
  } y;

  y.f = number;
  y.i = 0x5f375a86 - (y.i >> 1);
  y.f = y.f * ( 1.5f - ( number * 0.5f * y.f * y.f ) );
  return y.f;
}
 
I find it very hard to understand what you are doing here, but i notice you are calling this function:

MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

and passing it what I would assume are scaled sensor data. The problem is for the LSM9DS0 the accel/gyro +z is up, but +z for the mag is down, so the last entry should be -mz.

The sensor calibration isn't obvious to me, but at least the accel and gyro and mag offset biases need to be corrected, and the mag axes rescaled would improve accuracy.

Also, the sensor fusion rate isn't usually something set by the user; I do not understand this at all. It is usually determined by the processor loop speed and how many times it can iterate on the mahony function in between data reads. With the Teensy 3.2 you should have no trouble reaching 1 kHz and higher fusion rates.
 
Last edited:
yes is do pass the scaled sensor data and the calib is done in the setup of the main sketch calling
sensor_9dof_configure();
and in that function i do call
dof.calLSM9DS0(gbias, abias);
that should fill the values of the gbias and a bias arrays, but as you noticed the magnetometer is not calibrated.
Thanks for the tip about the mz, but it did not sovled the problem, actually looking at your sketch i've seen that the function is call with the same values, the code i'm using is actually heavily based on your code.
Regarding the update frequency i'm sure it can run it a 1khz, and i will update the frequency, but i don't have problems in quaternion convergence i'm not sure that would help in solving the issue.
 
Little update, i've used your github code (just changed the address and incorporated the madgwick function into the sketch) and i have the same result, that makes me beliving a sligtly little bit more in my terrible coding skills but frankly i was hoping for a stupid mistake from my side !
With the Board lying flat on the desk i have:

Code:
ax = -30.64 ay = -54.08 az = 1008.54 mg
gx = 0.04 gy = 0.31 gz = -0.20 deg/s
mx = 303.96 my = -41.75 mz = 306.52 mG
q0 = -1.00 qx = 0.03 qy = -0.01 qz = -0.04
Gyro temperature is 33.8 degrees C
Digital temperature value = 20.00 C
Digital temperature value = 68.00 F
Digital pressure value = 0.00 mbar
Altitude = 145366.45 feet
Yaw, Pitch, Roll: -9.14, 1.76, -3.05
rate = 1250.95 Hz
Mag raw data:
141
2
Mag raw data:
147
2
Mag raw data:
149
2

Rolling about 45°C the board i have:
Code:
ax = -14.95 ay = -756.90 az = 712.40 mg
gx = -0.67 gy = 0.04 gz = 0.30 deg/s
mx = 296.63 my = 231.08 mz = 168.09 mG
q0 = -0.85 qx = 0.36 qy = -0.16 qz = 0.35
Gyro temperature is 33.6 degrees C
Digital temperature value = 20.00 C
Digital temperature value = 68.00 F
Digital pressure value = 0.00 mbar
Altitude = 145366.45 feet
Yaw, Pitch, Roll: -58.84, 0.98, -46.66
rate = 1248.13 Hz
Mag raw data:
119
2
Mag raw data:
125
2
Mag raw data:
119
2
Mag raw data:
122
2

Pitching 45° the Board
Code:
ax = -721.13 ay = -32.17 az = 715.88 mg
gx = 0.05 gy = 0.15 gz = 3.14 deg/s
mx = 423.34 my = -5.49 mz = -26.37 mG
q0 = -0.92 qx = 0.03 qy = -0.38 qz = -0.03
Gyro temperature is 34.4 degrees C
Digital temperature value = 20.00 C
Digital temperature value = 68.00 F
Digital pressure value = 0.00 mbar
Altitude = 145366.45 feet
Yaw, Pitch, Roll: -11.46, 45.09, -2.75
rate = 1251.88 Hz
Mag raw data:
223
3
Mag raw data:
215
3
Mag raw data:
210
3

As in my sketch the yaw seems to be affected by the roll but not by the pitch :confused::confused::confused::confused:
Of course i made sure that the board did not get any yaw angle.

Here below the sketch i've slightly changed form your github.

Code:
/* LSM9DS0_MS5637_t3 Basic Example Code
 by: Kris Winer
 date: November 1, 2014
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 Demonstrate basic LSM9DS0 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.
 
 This sketch is intended specifically for the LSM9DS0+MS5637 Add-on shield for the Teensy 3.1.
 It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h.
 The MS5637 is a simple but high resolution pressure sensor, which can be used in its high resolution
 mode but with power consumption od 20 microAmp, or in a lower resolution mode with power consumption of
 only 1 microAmp. The choice will depend on the application.
 
 SDA and SCL should have external pull-up resistors (to 3.3V).
 4K7 resistors are on the LSM9DS0+MS5637 Teensy 3.1 add-on shield/breakout board.
 
 Hardware setup:
 LSM9DS0Breakout --------- Arduino
 VDD ---------------------- 3.3V
 VDDI --------------------- 3.3V
 SDA ----------------------- A4
 SCL ----------------------- A5
 GND ---------------------- GND
 
 Note: The LSM9DS0 is an I2C sensor and can use the Arduino Wire library. 
 Because the sensor is not 5V tolerant, we are using either a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
 */
//#include "Wire.h"   
#include <i2c_t3.h>
#include <SPI.h>
//#include <Adafruit_GFX.h>
//#include <Adafruit_PCD8544.h>

// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 7 - Serial clock out (SCLK)
// pin 6 - Serial data out (DIN)
// pin 5 - Data/Command select (D/C)
// pin 3 - LCD chip select (SCE)
// pin 4 - LCD reset (RST)
//Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 3, 4);

// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet
#define MS5637_RESET      0x1E
#define MS5637_CONVERT_D1 0x40
#define MS5637_CONVERT_D2 0x50
#define MS5637_ADC_READ   0x00

// See also LSM9DS0 Register Map and Descriptions,http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00087365.pdf
//
////////////////////////////
// LSM9DS0 Gyro Registers //
////////////////////////////
#define  LSM9DS0G_WHO_AM_I_G    0x0F
#define  LSM9DS0G_CTRL_REG1_G   0x20
#define  LSM9DS0G_CTRL_REG2_G   0x21
#define  LSM9DS0G_CTRL_REG3_G   0x22
#define  LSM9DS0G_CTRL_REG4_G   0x23
#define  LSM9DS0G_CTRL_REG5_G   0x24
#define  LSM9DS0G_REFERENCE_G   0x25
#define  LSM9DS0G_STATUS_REG_G    0x27
#define  LSM9DS0G_OUT_X_L_G   0x28
#define  LSM9DS0G_OUT_X_H_G   0x29
#define  LSM9DS0G_OUT_Y_L_G   0x2A
#define  LSM9DS0G_OUT_Y_H_G   0x2B
#define  LSM9DS0G_OUT_Z_L_G   0x2C
#define  LSM9DS0G_OUT_Z_H_G   0x2D
#define  LSM9DS0G_FIFO_CTRL_REG_G 0x2E
#define  LSM9DS0G_FIFO_SRC_REG_G  0x2F
#define  LSM9DS0G_INT1_CFG_G    0x30
#define  LSM9DS0G_INT1_SRC_G    0x31
#define  LSM9DS0G_INT1_THS_XH_G   0x32
#define  LSM9DS0G_INT1_THS_XL_G   0x33
#define  LSM9DS0G_INT1_THS_YH_G   0x34
#define  LSM9DS0G_INT1_THS_YL_G   0x35
#define  LSM9DS0G_INT1_THS_ZH_G   0x36
#define  LSM9DS0G_INT1_THS_ZL_G   0x37
#define  LSM9DS0G_INT1_DURATION_G 0x38

//////////////////////////////////////////
//  LSM9DS0XM Accel/Magneto (XM) Registers //
//////////////////////////////////////////
#define  LSM9DS0XM_OUT_TEMP_L_XM  0x05
#define  LSM9DS0XM_OUT_TEMP_H_XM  0x06
#define  LSM9DS0XM_STATUS_REG_M   0x07
#define  LSM9DS0XM_OUT_X_L_M    0x08
#define  LSM9DS0XM_OUT_X_H_M    0x09
#define  LSM9DS0XM_OUT_Y_L_M    0x0A
#define  LSM9DS0XM_OUT_Y_H_M    0x0B
#define  LSM9DS0XM_OUT_Z_L_M    0x0C
#define  LSM9DS0XM_OUT_Z_H_M    0x0D
#define  LSM9DS0XM_WHO_AM_I_XM    0x0F
#define  LSM9DS0XM_INT_CTRL_REG_M 0x12
#define  LSM9DS0XM_INT_SRC_REG_M  0x13
#define  LSM9DS0XM_INT_THS_L_M    0x14
#define  LSM9DS0XM_INT_THS_H_M    0x15
#define  LSM9DS0XM_OFFSET_X_L_M   0x16
#define  LSM9DS0XM_OFFSET_X_H_M   0x17
#define  LSM9DS0XM_OFFSET_Y_L_M   0x18
#define  LSM9DS0XM_OFFSET_Y_H_M   0x19
#define  LSM9DS0XM_OFFSET_Z_L_M   0x1A
#define  LSM9DS0XM_OFFSET_Z_H_M   0x1B
#define  LSM9DS0XM_REFERENCE_X    0x1C
#define  LSM9DS0XM_REFERENCE_Y    0x1D
#define  LSM9DS0XM_REFERENCE_Z    0x1E
#define  LSM9DS0XM_CTRL_REG0_XM   0x1F
#define  LSM9DS0XM_CTRL_REG1_XM   0x20
#define  LSM9DS0XM_CTRL_REG2_XM   0x21
#define  LSM9DS0XM_CTRL_REG3_XM   0x22
#define  LSM9DS0XM_CTRL_REG4_XM   0x23
#define  LSM9DS0XM_CTRL_REG5_XM   0x24
#define  LSM9DS0XM_CTRL_REG6_XM   0x25
#define  LSM9DS0XM_CTRL_REG7_XM   0x26
#define  LSM9DS0XM_STATUS_REG_A   0x27
#define  LSM9DS0XM_OUT_X_L_A    0x28
#define  LSM9DS0XM_OUT_X_H_A    0x29
#define  LSM9DS0XM_OUT_Y_L_A    0x2A
#define  LSM9DS0XM_OUT_Y_H_A    0x2B
#define  LSM9DS0XM_OUT_Z_L_A    0x2C
#define  LSM9DS0XM_OUT_Z_H_A    0x2D
#define  LSM9DS0XM_FIFO_CTRL_REG  0x2E
#define  LSM9DS0XM_FIFO_SRC_REG   0x2F
#define  LSM9DS0XM_INT_GEN_1_REG  0x30
#define  LSM9DS0XM_INT_GEN_1_SRC  0x31
#define  LSM9DS0XM_INT_GEN_1_THS  0x32
#define  LSM9DS0XM_INT_GEN_1_DURATION 0x33
#define  LSM9DS0XM_INT_GEN_2_REG  0x34
#define  LSM9DS0XM_INT_GEN_2_SRC  0x35
#define  LSM9DS0XM_INT_GEN_2_THS  0x36
#define  LSM9DS0XM_INT_GEN_2_DURATION 0x37
#define  LSM9DS0XM_CLICK_CFG    0x38
#define  LSM9DS0XM_CLICK_SRC    0x39
#define  LSM9DS0XM_CLICK_THS    0x3A
#define  LSM9DS0XM_TIME_LIMIT   0x3B
#define  LSM9DS0XM_TIME_LATENCY   0x3C
#define  LSM9DS0XM_TIME_WINDOW    0x3D
#define  LSM9DS0XM_ACT_THS    0x3E
#define  LSM9DS0XM_ACT_DUR    0x3F

// Using the LSM9DS0+MS5637 Teensy 3.1 Add-On shield, ADO is set to 0 
#define ADO 0
#if ADO
#define LSM9DS0XM_ADDRESS  0x1E // Address of accel/magnetometer when ADO = 1
#define LSM9DS0G_ADDRESS   0x6B // Address of gyro when ADO = 1
#define MS5637_ADDRESS 0x76      // Address of altimeter
#else 
#define LSM9DS0XM_ADDRESS  0x1D // Address of accel/magnetometer when ADO = 0
#define LSM9DS0G_ADDRESS   0x6B // Address of gyro when ADO = 0
#define MS5637_ADDRESS 0x76      // Address of altimeter
#endif

#define SerialDebug true  // set to true to get Serial output for debugging

// Set initial input parameters
enum Ascale {  // set of allowable accel full scale settings
  AFS_2G = 0,
  AFS_4G,
  AFS_6G,
  AFS_8G,
  AFS_16G
};

enum Aodr {  // set of allowable gyro sample rates
  AODR_PowerDown = 0,
  AODR_3_125Hz,
  AODR_6_25Hz,
  AODR_12_5Hz,
  AODR_25Hz,
  AODR_50Hz,
  AODR_100Hz,
  AODR_200Hz,
  AODR_400Hz,
  AODR_800Hz,
  AODR_1600Hz
};

enum Abw {  // set of allowable accewl bandwidths
   ABW_773Hz = 0,
   ABW_194Hz,
   ABW_362Hz,
   ABW_50Hz
};

enum Gscale {  // set of allowable gyro full scale settings
  GFS_245DPS = 0,
  GFS_500DPS,
  GFS_NoOp,
  GFS_2000DPS
};

enum Godr {  // set of allowable gyro sample rates
  GODR_95Hz = 0,
  GODR_190Hz,
  GODR_380Hz,
  GODR_760Hz
};

enum Gbw {   // set of allowable gyro data bandwidths
  GBW_low = 0,  // 12.5 Hz at Godr = 95 Hz, 12.5 Hz at Godr = 190 Hz,  30 Hz at Godr = 760 Hz
  GBW_med,      // 25 Hz   at Godr = 95 Hz, 25 Hz   at Godr = 190 Hz,  35 Hz at Godr = 760 Hz
  GBW_high,     // 25 Hz   at Godr = 95 Hz, 50 Hz   at Godr = 190 Hz,  50 Hz at Godr = 760 Hz
  GBW_highest   // 25 Hz   at Godr = 95 Hz, 70 Hz   at Godr = 190 Hz, 100 Hz at Godr = 760 Hz
};

enum Mscale {  // set of allowable mag full scale settings
  MFS_2G = 0,
  MFS_4G,
  MFS_8G,
  MFS_12G
};

enum Mres {
  MRES_LowResolution = 0, 
  MRES_NoOp,
  MRES_HighResolution
};

enum Modr {  // set of allowable mag sample rates
  MODR_3_125Hz = 0,
  MODR_6_25Hz,
  MODR_12_5Hz,
  MODR_25Hz,
  MODR_50Hz,
  MODR_100Hz
};

#define ADC_256  0x00 // define pressure and temperature conversion rates
#define ADC_512  0x02
#define ADC_1024 0x04
#define ADC_2048 0x06
#define ADC_4096 0x08
#define ADC_8192 0x0A
#define ADC_D1   0x40
#define ADC_D2   0x50

// Specify sensor full scale
uint8_t OSR = ADC_8192;      // set pressure amd temperature oversample rate
uint8_t Gscale = GFS_245DPS; // gyro full scale
uint8_t Godr = GODR_190Hz;   // gyro data sample rate
uint8_t Gbw = GBW_low;       // gyro data bandwidth
uint8_t Ascale = AFS_2G;     // accel full scale
uint8_t Aodr = AODR_200Hz;   // accel data sample rate
uint8_t Abw = ABW_50Hz;      // accel data bandwidth
uint8_t Mscale = MFS_12G;     // mag full scale
uint8_t Modr = MODR_6_25Hz;    // mag data sample rate
uint8_t Mres = MRES_LowResolution;  // magnetometer operation mode
float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors
  
// Pin definitions
int intPin = 15;  // These can be changed, 2 and 3 are the Arduinos ext int pins
int myLed  = 13;

uint16_t Pcal[8];         // calibration constants from MS5637 PROM registers
unsigned char nCRC;       // calculated check sum to ensure PROM integrity
uint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data
double dT, OFFSET, SENS, T2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data
int16_t accelCount[3], gyroCount[3], magCount[3];  // Stores the 16-bit signed accelerometer, gyro, and mag sensor output
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0},  magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and magnetometer
int16_t tempCount;            // temperature raw count output
float   temperature;          // Stores the LSM9DS0gyro internal chip temperature in degrees Celsius
double Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
float GyroMeasError = PI * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)
float GyroMeasDrift = PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
// 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.
float beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta
float 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

uint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate
float pitch, yaw, roll;
float deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0;                         // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method


void setup()
{
//  Wire.begin();
//  TWBR = 12;  // 400 kbit/sec I2C speed for Pro Mini
  // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1
  Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400);
  delay(4000);
  Serial.begin(38400);
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(intPin, INPUT);
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);
/*  
  display.begin(); // Initialize the display
  display.setContrast(40); // Set the contrast
  
// Start device display with ID of sensor
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0,0); display.print("LSM9DS0");
  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
*/

  // Read the WHO_AM_I registers, this is a good test of communication
  Serial.println("LSM9DS0 9-axis motion sensor...");
  byte c = readByte(LSM9DS0G_ADDRESS, LSM9DS0G_WHO_AM_I_G);  // Read WHO_AM_I register for LSM9DS0 gyro
  Serial.println("LSM9DS0 accel/gyro"); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0xD4, HEX);
  byte d = readByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_WHO_AM_I_XM);  // Read WHO_AM_I register for LSM9DS0 accel/magnetometer
  Serial.println("LSM9DS0 magnetometer"); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x49, HEX);
/*
  display.setCursor(20,0); display.print("LSM9DS0");
  display.setCursor(0,10); display.print("I AM"); display.print(c, HEX);  
  display.setCursor(0,20); display.print("I Should Be"); display.print(0xD4, HEX); 
  display.setCursor(0,30); display.print("I AM"); display.print(d, HEX);  
  display.setCursor(0,40); display.print("I Should Be"); display.print(0x49, HEX); 
  display.display();
  delay(1000); 
  */

  if (c == 0xD4 && d == 0x49) // WHO_AM_I should always be 0xD4 for the gyro and 0x49 for the accel/mag
  {  
    Serial.println("LSM9DS0 is online...");
 
   initLSM9DS0(); 
   Serial.println("LSM9DS0 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

   // get sensor resolutions, only need to do this once
   getAres();
   getGres();
   getMres();
   Serial.print("accel sensitivity is "); Serial.print(1./(1000.*aRes)); Serial.println(" LSB/mg");
   Serial.print("gyro sensitivity is "); Serial.print(1./(1000.*gRes)); Serial.println(" LSB/mdps");
   Serial.print("mag sensitivity is "); Serial.print(1./(1000.*mRes)); Serial.println(" LSB/mGauss");

  accelgyrocalLSM9DS0(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
  Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]);
  Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);
 
  magcalLSM9DS0(magBias);
  Serial.println("mag biases (mG)"); Serial.println(1000.*magBias[0]); Serial.println(1000.*magBias[1]); Serial.println(1000.*magBias[2]);
  
  /* display.clearDisplay();
     
  display.setCursor(0, 0); display.print("LSM9DS0bias");
  display.setCursor(0, 8); display.print(" x   y   z  ");
  display.setCursor(0,  16); display.print((int)(1000*accelBias[0])); 
  display.setCursor(24, 16); display.print((int)(1000*accelBias[1])); 
  display.setCursor(48, 16); display.print((int)(1000*accelBias[2])); 
  display.setCursor(72, 16); display.print("mg");
    
  display.setCursor(0,  24); display.print(gyroBias[0], 1); 
  display.setCursor(24, 24); display.print(gyroBias[1], 1); 
  display.setCursor(48, 24); display.print(gyroBias[2], 1); 
  display.setCursor(66, 24); display.print("o/s");   
 
  display.display();
  delay(1000); 
 */ 
 
  // Reset the MS5637 pressure sensor
  MS5637Reset();
  delay(100);
  Serial.println("MS5637 pressure sensor reset...");
  // Read PROM data from MS5637 pressure sensor
  MS5637PromRead(Pcal);
  Serial.println("PROM data read:");
  Serial.print("C0 = "); Serial.println(Pcal[0]);
  unsigned char refCRC = Pcal[0]  >> 12;
  Serial.print("C1 = "); Serial.println(Pcal[1]);
  Serial.print("C2 = "); Serial.println(Pcal[2]);
  Serial.print("C3 = "); Serial.println(Pcal[3]);
  Serial.print("C4 = "); Serial.println(Pcal[4]);
  Serial.print("C5 = "); Serial.println(Pcal[5]);
  Serial.print("C6 = "); Serial.println(Pcal[6]);
  Serial.print("C7 = "); Serial.println(Pcal[7]);
  
  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data
  Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);  
  
/*  display.clearDisplay();
  display.setCursor(20,0); display.print("MS5637");
  display.setCursor(0,10); display.print("CRC is "); display.setCursor(50,10); display.print(nCRC);
  display.setCursor(0,20); display.print("Should be "); display.setCursor(50,30); display.print(refCRC);
  display.display();
  delay(1000);  
 */      
  }
  else
  {
    Serial.print("Could not connect to LSM9DS0: 0x");
    Serial.println(c, HEX);
    while(1) ; // Loop forever if communication doesn't happen
  }
}

void loop()
{  
  if (readByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_STATUS_REG_A) & 0x08) {  // check if new accel data is ready  
    readAccelData(accelCount);  // Read the x/y/z adc values
 
    // Now we'll calculate the accleration value into actual g's
    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
    ay = (float)accelCount[1]*aRes - accelBias[1];   
    az = (float)accelCount[2]*aRes - accelBias[2]; 
  } 
   
  if (readByte(LSM9DS0G_ADDRESS, LSM9DS0G_STATUS_REG_G) & 0x08) {  // check if new gyro data is ready  
    readGyroData(gyroCount);  // Read the x/y/z adc values

    // Calculate the gyro value into actual degrees per second
    gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
    gy = (float)gyroCount[1]*gRes - gyroBias[1];  
    gz = (float)gyroCount[2]*gRes - gyroBias[2];   
  }
  
  if (readByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_STATUS_REG_M) & 0x08) {  // check if new mag data is ready  
    readMagData(magCount);  // Read the x/y/z adc values
    
    // Calculate the magnetometer values in milliGauss
    // Include factory calibration per data sheet and user environmental corrections
    mx = (float)magCount[0]*mRes - magBias[0];  // get actual magnetometer value, this depends on scale being set
    my = (float)magCount[1]*mRes - magBias[1];  
    mz = (float)magCount[2]*mRes - magBias[2];   
  }
  
  Now = micros();
  deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
  lastUpdate = Now;

  sum += deltat; // sum for averaging filter update rate
  sumCount++;
  
  // Sensors x and yaxes of the accelerometer/magnetometer and gyro are aligned. The magnetometer  
  // z-axis (+ updown) is anti-aligned with the z-axis (+ up) of accelerometer and gyro
  // We have to make no allowance for this orientation match in feeding the output to the quaternion filter.
  // For the LSM9DS0, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis  
  // 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, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  mx,  my, mz);
//  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

    // Serial print and/or display at 0.5 s rate independent of data rates
    delt_t = millis() - count;
    if (delt_t > 500) { // update LCD once per half-second independent of read rate

    if(SerialDebug) {
    Serial.print("ax = "); Serial.print((int)1000*ax);  
    Serial.print(" ay = "); Serial.print((int)1000*ay); 
    Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
    Serial.print("gx = "); Serial.print( gx, 2); 
    Serial.print(" gy = "); Serial.print( gy, 2); 
    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
    Serial.print("mx = "); Serial.print( (int)1000*mx ); 
    Serial.print(" my = "); Serial.print( (int)1000*my ); 
    Serial.print(" mz = "); Serial.print( (int)1000*mz ); Serial.println(" mG");
    
    Serial.print("q0 = "); Serial.print(q[0]);
    Serial.print(" qx = "); Serial.print(q[1]); 
    Serial.print(" qy = "); Serial.print(q[2]); 
    Serial.print(" qz = "); Serial.println(q[3]); 
    }               
    tempCount = readTempData();  // Read the gyro adc values
    temperature = ((float) tempCount/8. + 25.0); // Gyro chip temperature in degrees Centigrade
   // Print temperature in degrees Centigrade      
    Serial.print("Gyro temperature is ");  Serial.print(temperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of s degree C
 
    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value
    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value
    
    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference
    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);
    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);
 
    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade
//
// Second order corrections
    if(Temperature > 20) 
    {
      T2 = 5*dT*dT/pow(2, 38); // correction for high temperatures
      OFFSET2 = 0;
      SENS2 = 0;
    }
    if(Temperature < 20)                   // correction for low temperature
    {
      T2      = 3*dT*dT/pow(2, 33); 
      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
    } 
    if(Temperature < -15)                      // correction for very low temperature
    {
      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);
      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);
    }
 // End of second order corrections
 //
     Temperature = Temperature - T2/100;
     OFFSET = OFFSET - OFFSET2;
     SENS = SENS - SENS2;
 
     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or Pa/100
  
    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));
    if(SerialDebug) {
    Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius
    Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit
    Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2);  Serial.println(" mbar");// pressure in millibar
    Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
    }
    
  // 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.
    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI; 
    yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    roll  *= 180.0f / PI;
     
    if(SerialDebug) {
    Serial.print("Yaw, Pitch, Roll: ");
    Serial.print(yaw, 2);
    Serial.print(", ");
    Serial.print(pitch, 2);
    Serial.print(", ");
    Serial.println(roll, 2);
    
    Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
    }
/*   
    display.clearDisplay();    
 
    display.setCursor(0, 0); display.print(" x   y   z ");
    display.setCursor(0,  8); display.print((int)(1000*ax)); 
    display.setCursor(24, 8); display.print((int)(1000*ay)); 
    display.setCursor(48, 8); display.print((int)(1000*az)); 
    display.setCursor(72, 8); display.print("mg");
    
    display.setCursor(0,  16); display.print((int)(gx)); 
    display.setCursor(24, 16); display.print((int)(gy)); 
    display.setCursor(48, 16); display.print((int)(gz)); 
    display.setCursor(66, 16); display.print("o/s");    
    display.setCursor(0,  24); display.print((int)(mx)); 
    display.setCursor(24, 24); display.print((int)(my)); 
    display.setCursor(48, 24); display.print((int)(mz)); 
    display.setCursor(72, 24); display.print("mG");    
 
    display.setCursor(0,  32); display.print((int)(yaw)); 
    display.setCursor(24, 32); display.print((int)(pitch)); 
    display.setCursor(48, 32); display.print((int)(roll)); 
    display.setCursor(66, 32); display.print("ypr");  
 
/*    display.setCursor(0, 40); display.print(altitude, 0); display.print("ft"); 
    display.setCursor(68, 0); display.print(9.*Temperature/5. + 32., 0); 
    display.setCursor(42, 40); display.print((float) sumCount / (1000.*sum), 2); display.print("kHz"); 
    display.display();
*/


    digitalWrite(myLed, !digitalRead(myLed));
    count = millis(); 
    sumCount = 0;
    sum = 0;    
    }

}

//===================================================================================================================
//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data
//===================================================================================================================

void getMres() {
  switch (Mscale)
  {
  // Possible magnetometer scales (and their register bit settings) are:
  // 2 Gauss (00), 4 Gauss (01), 8 Gauss (10) and 12 Gauss (11)
    case MFS_2G:
          mRes = 2.0/32768.0;
          break;
    case MFS_4G:
          mRes = 4.0/32768.0;
          break;
    case MFS_8G:
          mRes = 8.0/32768.0;
          break;
    case MFS_12G:
          mRes = 12.0/32768.0;
          break;
  }
}

void getGres() {
  switch (Gscale)
  {
  // Possible gyro scales (and their register bit settings) are:
  // 245 DPS (00), 500 DPS (01), and 2000 DPS  (11). 
    case GFS_245DPS:
          gRes = 245.0/32768.0;
          break;
    case GFS_500DPS:
          gRes = 500.0/32768.0;
          break;
    case GFS_2000DPS:
          gRes = 2000.0/32768.0;
          break;
  }
}

void getAres() {
  switch (Ascale)
  {
  // Possible accelerometer scales (and their register bit settings) are:
  // 2 Gs (000), 4 Gs (001), 6 gs (010), 8 Gs (011), and 16 gs (100). 
    case AFS_2G:
          aRes = 2.0/32768.0;
          break;
    case AFS_4G:
          aRes = 4.0/32768.0;
          break;
    case AFS_6G:
          aRes = 6.0/32768.0;
          break;
    case AFS_8G:
          aRes = 8.0/32768.0;
          break;
    case AFS_16G:
          aRes = 16.0/32768.0;
          break;
  }
}


void readAccelData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z accel register data stored here
  readBytes(LSM9DS0XM_ADDRESS, LSM9DS0XM_OUT_X_L_A, 6, &rawData[0]);  // Read the six raw data registers into data array
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
}


void readGyroData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z gyro register data stored here
  readBytes(LSM9DS0G_ADDRESS, LSM9DS0G_OUT_X_L_G, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
}

void readMagData(int16_t * destination)
{
    uint8_t rawData[6];  // x/y/z gyro register data stored here
    readBytes(LSM9DS0XM_ADDRESS, LSM9DS0XM_OUT_X_L_M, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
    destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value
    destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  // Data stored as little Endian
    destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
    Serial.println("Mag raw data:");
    Serial.println(rawData[0]); Serial.println(rawData[1]);
}

int16_t readTempData()
{
  uint8_t rawData[2];  // x/y/z gyro register data stored here
  readBytes(LSM9DS0XM_ADDRESS, LSM9DS0XM_OUT_TEMP_L_XM, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array 
  return (((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a 16-bit signed value
}
       

void initLSM9DS0()
{  
   // configure the gyroscope, enable normal mode = power on
   writeByte(LSM9DS0G_ADDRESS, LSM9DS0G_CTRL_REG1_G, Godr << 6 | Gbw << 4 | 0x0F);
   writeByte(LSM9DS0G_ADDRESS, LSM9DS0G_CTRL_REG4_G, Gscale << 4 | 0x80); // enable bloack data update
   // configure the accelerometer-specify ODR (sample rate) selection with Aodr, enable block data update
   writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG1_XM, Aodr << 4 | 0x0F);
   // configure the accelerometer-specify bandwidth and full-scale selection with Abw, Ascale 
   writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG2_XM, Abw << 6 | Ascale << 3);
    // enable temperature sensor, set magnetometer ODR (sample rate) and resolution mode
   writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG5_XM, 0x80 | Mres << 5 | Modr << 2);
   // set magnetometer full scale
   writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG6_XM, Mscale << 5 & 0x60);
   writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG7_XM, 0x00); // select continuous conversion mode
 }


// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void accelgyrocalLSM9DS0(float * dest1, float * dest2)
{  
  uint8_t data[6] = {0, 0, 0, 0, 0, 0};
  int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
  uint16_t samples, ii;
  
  Serial.println("Calibrating gyro...");
 
  // First get gyro bias
  byte c = readByte(LSM9DS0G_ADDRESS, LSM9DS0G_CTRL_REG5_G);
  writeByte(LSM9DS0G_ADDRESS, LSM9DS0G_CTRL_REG5_G, c | 0x40);     // Enable gyro FIFO  
  delay(200);                                                       // Wait for change to take effect
  writeByte(LSM9DS0G_ADDRESS, LSM9DS0G_FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
  delay(1000);  // delay 1000 milliseconds to collect FIFO samples
  
  samples = (readByte(LSM9DS0G_ADDRESS, LSM9DS0G_FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples

  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
    int16_t gyro_temp[3] = {0, 0, 0};
    readBytes(LSM9DS0G_ADDRESS, LSM9DS0G_OUT_X_L_G, 6, &data[0]);
    gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
    gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
    gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);

    gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    gyro_bias[1] += (int32_t) gyro_temp[1]; 
    gyro_bias[2] += (int32_t) gyro_temp[2]; 
  }  

  gyro_bias[0] /= samples; // average the data
  gyro_bias[1] /= samples; 
  gyro_bias[2] /= samples; 
  
  dest1[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
  dest1[1] = (float)gyro_bias[1]*gRes;
  dest1[2] = (float)gyro_bias[2]*gRes;
  
  c = readByte(LSM9DS0G_ADDRESS, LSM9DS0G_CTRL_REG5_G);
  writeByte(LSM9DS0G_ADDRESS, LSM9DS0G_CTRL_REG5_G, c & ~0x40);   //Disable gyro FIFO  
  delay(200);
  writeByte(LSM9DS0G_ADDRESS, LSM9DS0G_FIFO_CTRL_REG_G, 0x00);  // Enable gyro bypass mode
 
   Serial.println("Calibrating accel...");
 
  // now get the accelerometer bias
  c = readByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG0_XM);
  writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG0_XM, c | 0x40);     // Enable gyro FIFO  
  delay(200);                                                       // Wait for change to take effect
  writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
  delay(1000);  // delay 1000 milliseconds to collect FIFO samples
  
  samples = (readByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_FIFO_SRC_REG) & 0x1F); // Read number of stored samples

  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
    int16_t accel_temp[3] = {0, 0, 0};
    readBytes(LSM9DS0XM_ADDRESS, LSM9DS0XM_OUT_X_L_A, 6, &data[0]);
    accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
    accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
    accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);

    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[1] += (int32_t) accel_temp[1]; 
    accel_bias[2] += (int32_t) accel_temp[2]; 
  }  

  accel_bias[0] /= samples; // average the data
  accel_bias[1] /= samples; 
  accel_bias[2] /= samples; 
  
  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32_t) (1.0/aRes);}
  
  dest2[0] = (float)accel_bias[0]*aRes;  // Properly scale the data to get g
  dest2[1] = (float)accel_bias[1]*aRes;
  dest2[2] = (float)accel_bias[2]*aRes;
  
  c = readByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG0_XM);
  writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_CTRL_REG0_XM, c & ~0x40);   //Disable accel FIFO  
  delay(200);
  writeByte(LSM9DS0XM_ADDRESS, LSM9DS0XM_FIFO_CTRL_REG, 0x00);  // Enable accel bypass mode
}

void magcalLSM9DS0(float * dest1) 
{
  uint8_t data[6]; // data array to hold mag x, y, z, data
  uint16_t ii = 0, sample_count = 0;
  int32_t mag_bias[3] = {0, 0, 0};
  int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0};
 
  Serial.println("Mag Calibration: Wave device in a figure eight until done!");
  delay(4000);
  
   sample_count = 128;
   for(ii = 0; ii < sample_count; ii++) {
    int16_t mag_temp[3] = {0, 0, 0};
    readBytes(LSM9DS0XM_ADDRESS, LSM9DS0XM_OUT_X_L_M, 6, &data[0]);  // Read the six raw data registers into data array
    mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;   // Form signed 16-bit integer for each sample in FIFO
    mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
    mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
    for (int jj = 0; jj < 3; jj++) {
      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
    }
    delay(105);  // at 10 Hz ODR, new mag data is available every 100 ms
   }

//    Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);
//    Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);
//    Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);

    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
    
    dest1[0] = (float) mag_bias[0]*mRes;  // save mag biases in G for main program
    dest1[1] = (float) mag_bias[1]*mRes;   
    dest1[2] = (float) mag_bias[2]*mRes;          

 /* //write biases to accelerometermagnetometer offset registers as counts);
  writeByte(LSM9DS0M_ADDRESS, LSM9DS0M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0]  & 0xFF);
  writeByte(LSM9DS0M_ADDRESS, LSM9DS0M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF);
  writeByte(LSM9DS0M_ADDRESS, LSM9DS0M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF);
  writeByte(LSM9DS0M_ADDRESS, LSM9DS0M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF);
  writeByte(LSM9DS0M_ADDRESS, LSM9DS0M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF);
  writeByte(LSM9DS0M_ADDRESS, LSM9DS0M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF);
 */
   Serial.println("Mag Calibration done!");
}

// I2C communication with the MS5637 is a little different from that with the LSM9DS0and most other sensors
// For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading
// MS5637 registers

        void MS5637Reset()
        {
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
  Wire.write(MS5637_RESET);                // Put reset command in Tx buffer
  Wire.endTransmission();                  // Send the Tx buffer
        }
        
        void MS5637PromRead(uint16_t * destination)
        {
        uint8_t data[2] = {0,0};
        for (uint8_t ii = 0; ii <8; ii++) {
          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer
          Wire.endTransmission(I2C_NOSTOP);        // Send the Tx buffer, but send a restart to keep connection alive
    uint8_t i = 0;
          Wire.requestFrom(MS5637_ADDRESS, 2);   // Read two bytes from slave PROM address 
    while (Wire.available()) {
          data[i++] = Wire.read(); }               // Put read results in the Rx buffer
          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program
        }
        }

        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read
        {
        uint8_t data[3] = {0,0,0};
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(CMD | OSR);                  // Put pressure conversion command in Tx buffer
        Wire.endTransmission(I2C_NOSTOP);        // Send the Tx buffer, but send a restart to keep connection alive
        
        switch (OSR)
        {
          case ADC_256: delay(1); break;  // delay for conversion to complete
          case ADC_512: delay(3); break;
          case ADC_1024: delay(4); break;
          case ADC_2048: delay(6); break;
          case ADC_4096: delay(10); break;
          case ADC_8192: delay(20); break;
        }
       
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(0x00);                        // Put ADC read command in Tx buffer
        Wire.endTransmission(I2C_NOSTOP);        // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address 
  while (Wire.available()) {
        data[i++] = Wire.read(); }               // Put read results in the Rx buffer
        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program
        }



unsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents
{
  int cnt;
  unsigned int n_rem = 0;
  unsigned char n_bit;
  
  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation
  n_prom[7] = 0;
  for(cnt = 0; cnt < 16; cnt++)
  {
    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);
    for(n_bit = 8; n_bit > 0; n_bit--)
    {
        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;
        else                  n_rem = (n_rem<<1);
    }
  }
  n_rem = ((n_rem>>12) & 0x000F);
  return (n_rem ^ 0x00);
}



// I2C read/write functions for the LSM9DS0and AK8963 sensors

        void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
  Wire.beginTransmission(address);  // Initialize the Tx buffer
  Wire.write(subAddress);           // Put slave register address in Tx buffer
  Wire.write(data);                 // Put data in Tx buffer
  Wire.endTransmission();           // Send the Tx buffer
}

        uint8_t readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data; // `data` will store the register data   
  Wire.beginTransmission(address);         // Initialize the Tx buffer
  Wire.write(subAddress);                  // Put slave register address in Tx buffer
  Wire.endTransmission(I2C_NOSTOP);        // Send the Tx buffer, but send a restart to keep connection alive
//  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
//  Wire.requestFrom(address, 1);  // Read one byte from slave register address 
  Wire.requestFrom(address, (size_t) 1);   // Read one byte from slave register address 
  data = Wire.read();                      // Fill Rx buffer with result
  return data;                             // Return data read from slave register
}

        void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  Wire.beginTransmission(address);   // Initialize the Tx buffer
  Wire.write(0x80 | subAddress);     // Put slave register address in Tx buffer, include 0x80 for LSM9DS0 multiple byte read
  Wire.endTransmission(I2C_NOSTOP);  // Send the Tx buffer, but send a restart to keep connection alive
//  Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
//        Wire.requestFrom(address, count);  // Read bytes from slave register address 
        Wire.requestFrom(address, (size_t) count);  // Read bytes from slave register address 
  while (Wire.available()) {
        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer
}


void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // 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[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;

        }
  
  
  
 // Similar to Madgwick scheme but 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 q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            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;   

            // 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 gravity
            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;
            }
            else
            {
                eInt[0] = 0.0f;     // prevent integral wind up
                eInt[1] = 0.0f;
                eInt[2] = 0.0f;
            }

            // Apply feedback terms
            gx = gx + Kp * ex + Ki * eInt[0];
            gy = gy + Kp * ey + Ki * eInt[1];
            gz = gz + Kp * ez + Ki * eInt[2];

            // 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;
 
}
 
Magnetometer calibration is essential if you hope to have any kind of
reasonable orientation estimation. I am not surprised you are seeing these
effects of no mag calibration then.
 
Magnetometer calibration is essential if you hope to have any kind of
reasonable orientation estimation. I am not surprised you are seeing these
effects of no mag calibration then.
well actually your sketch that i've used does calibrate the magnetometer but the problem is still there as you see from my last post...i'm just haveing troubles in storing the calibration in the magnetometer offset register to avoid calibrate it anytime i switch the board on, but this is not the issue riight now.
Thanks for help.
 
I'd recommend the mag calibration GUI used for Paul's prop shield to be sure...

This kind of cross talk is normal using an uncalibrated or insufficiently calibrated LSM9DS0.

I would also switch to the MPU9250, but that is just me...
 
Last edited:
mmm looks like a big job for me to port the calibration routine to LSM9DS0.
How did you solved the calibration for your minishield woth LSM9DS0 onehorse ?
As you can see i've tryed your sketch but it doesn't seem to solve the problem....
 
Last edited:
A little update.
I knife and forked the code i was playing to ouput raw data rescaled to 14bit (NXP dof works at 14, the LSM9 at 16) and i gather the data with paul's motion sensor calibration tool.
Of course the calibration part doesn't work becouse is not implemented, but what is strange is that using that sw there is no cross correlation between the yaw and roll as it was, but as far as i know the mgnetomenter still ouputs non calibrated data therfore i'm still dubtfull about the math of the yaw calculation....any clever idea ?!?!?!

View attachment 8372
 
I just suggested you use the visualization GUI so you could see how well your mag calibration is working. I didn't expect you to port the NXP code over...

Here is what I am using lately:

Code:
    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);
    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);
    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);
    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    pitch = -asinf(a32);
    roll  = atan2f(a31, a33);
    yaw   = atan2f(a12, a22);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI; 
    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360
    roll  *= 180.0f / PI;
    lin_ax = ax + a31;
    lin_ay = ay + a32;
    lin_az = az - a33;
    if(SerialDebug) {
    Serial.print("Yaw, Pitch, Roll: ");
    Serial.print(yaw, 2);
    Serial.print(", ");
    Serial.print(pitch, 2);
    Serial.print(", ");
    Serial.println(roll, 2);

    Serial.print("Grav_x, Grav_y, Grav_z: ");
    Serial.print(-a31*1000, 2);
    Serial.print(", ");
    Serial.print(-a32*1000, 2);
    Serial.print(", ");
    Serial.print(a33*1000, 2);  Serial.println(" mg");
    Serial.print("Lin_ax, Lin_ay, Lin_az: ");
    Serial.print(lin_ax*1000, 2);
    Serial.print(", ");
    Serial.print(lin_ay*1000, 2);
    Serial.print(", ");
    Serial.print(lin_az*1000, 2); Serial.println(" mg");

Maybe you can just compare with what is being used in the NXP code.
 
It's quite strange, the magnetometer calibration doesn't not seem too bad as per the picture of the last post, with Paul's visualizer there is no crosscorrelation between yaw and roll, the nxp code used the rotation matrix instead of the quaternions so the porting doesn't look to easy.
i'm srtaching my head so much that i'm about to reach the skull
 
The above code snippet also uses the rotation matrix constructed from the Madgwick (or Mahony) quaternions to get yaw, pitch, and roll. There really is only one way to do this so I am somewhat surprised the Prop Shield code gives a different result.
 
The above code snippet also uses the rotation matrix constructed from the Madgwick (or Mahony) quaternions to get yaw, pitch, and roll. There really is only one way to do this so I am somewhat surprised the Prop Shield code gives a different result.

Here is what i got with your snippet, huge correlation between roll and yaw, small between pitch and yaw, almost null between pitch nd roll.
I would be very curious if you could do the same graph and see the difference.
Paul's prop shield just uses a very different notation and is quite difficould for me to adapt his code to mine, if you have a quick look to his git you will understand what i mean.
Immagine.jpg
 
This is the function that paul calls in his repo to ouptut euler angles

Code:
static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg,
	float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
{
	// calculate the pitch angle -90.0 <= Theta <= 90.0 deg
	*pfTheDeg = fasin_deg(-R[X][Z]);

	// calculate the roll angle range -180.0 <= Phi < 180.0 deg
	*pfPhiDeg = fatan2_deg(R[Y][Z], R[Z][Z]);

	// map +180 roll onto the functionally equivalent -180 deg roll
	if (*pfPhiDeg == 180.0F) {
		*pfPhiDeg = -180.0F;
	}

	// calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
	if (*pfTheDeg == 90.0F) {
		// vertical upwards gimbal lock case
		*pfPsiDeg = fatan2_deg(R[Z][Y], R[Y][Y]) + *pfPhiDeg;
	} else if (*pfTheDeg == -90.0F) {
		// vertical downwards gimbal lock case
		*pfPsiDeg = fatan2_deg(-R[Z][Y], R[Y][Y]) - *pfPhiDeg;
	} else {
		// general case
		*pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]);
	}

	// map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
	if (*pfPsiDeg < 0.0F) {
		*pfPsiDeg += 360.0F;
	}

	// check for rounding errors mapping small negative angle to 360 deg
	if (*pfPsiDeg >= 360.0F) {
		*pfPsiDeg = 0.0F;
	}

	// for NED, the compass heading Rho equals the yaw angle Psi
	*pfRhoDeg = *pfPsiDeg;

	// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
	*pfChiDeg = facos_deg(R[Z][Z]);

	return;
}
 
I finally got some time to check this using the LSM9DS0 that has been stuck on the back of my workhorse Teensy 3.1 for more than two years. I used this sketch, where I had to correct an error in the FIFO initialization (FIFO mode to the intended STREAM mode) in the gyro calibration routine but the key change is this:

MadgwickQuaternionUpdate(ax, ay, -az, gx*PI/180.0f, gy*PI/180.0f, -gz*PI/180.0f, mx, my, mz);

where I have negated the z-components of the accel and gyro entries into the Madgwick (or Mahony) algorithm. For the LSM9DS0 (it will be different for different IMUs) the mag +z-axis is opposite to the +z axes of the accel and gyro and if this orientation isn't properly accounted for, I get wicked cross talk between the yaw and pitch. With the above I get this:

LSM9DS0.yawcrosstalk.png

which shows the yaw more or less steady at -50 degrees from true north as I tilt the breadboard with the Teensy and LSM9DS0 attached for increasing roll and then pitch. I rotated the board to change yaw somewhere in the middle to see if the pitch or roll would change and, in general, the sensor fusion responds properly to orientation changes with no cross talk between the directions to the limit that I can hold the breadboard still by hand.

For some reason quite a few people have been complaining about this kind of cross talk lately and the solution is always two fold; 1) proper sensor calibration and 2) proper alignment of the sensor data passed to the fusion engine.

The Teensy 3.1 is managing 1300 Hz fusion rate with polling; I would expect to get closer to 2 kHz with interrupts. I also get complaints from people that they get crappy results with my sketches when it turns out they are using an 8 MHz Arduino Pro Mini (which I love) to do the fusion which might squeek out a 100 Hz fusion rate, which is just not fast enough to get any kind of accuracy.

Once more, the rule of thumb with iterative fusion algorithms is bandwidth (low pass filter) at 10 - 20% of the data sample rate and fusion at 4 - 5 times the sample rate. This means for the LSM9DS0 running at ~200 Hz sample rate the bandwidth should be ~40 Hz and the MCU should be capable of running the fusion algorithms at >1 kHz. The Teensy 3.X can, the Teensy LC not so much.
 
Last edited:
Status
Not open for further replies.
Back
Top