Forum Rule: Always post complete source code & details to reproduce any issue!
Page 5 of 5 FirstFirst ... 3 4 5
Results 101 to 124 of 124

Thread: 9- and 10-DoF LSM9DS0 shield for Teensy 3.1

  1. #101
    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.

  2. #102
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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...

  3. #103
    (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 by happyinmotion; 12-10-2015 at 03:43 AM.

  4. #104
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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...

  5. #105
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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...

  6. #106
    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-925...37_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 by happyinmotion; 12-10-2015 at 05:09 AM.

  7. #107
    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) ?

  8. #108
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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.

  9. #109
    Quote Originally Posted by onehorse View Post
    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.

  10. #110
    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;
    }

  11. #111
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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 by onehorse; 10-06-2016 at 05:51 AM.

  12. #112
    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.

  13. #113
    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 45C 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
    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;
     
    }

  14. #114
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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.

  15. #115
    Quote Originally Posted by onehorse View Post
    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.

  16. #116
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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 by onehorse; 10-06-2016 at 07:41 PM.

  17. #117
    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 by riodda; 10-07-2016 at 02:40 PM.

  18. #118
    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 ?!?!?!

    Attachment 8372

  19. #119
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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.

  20. #120
    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

  21. #121
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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.

  22. #122
    Quote Originally Posted by onehorse View Post
    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.
    Click image for larger version. 

Name:	Immagine.jpg 
Views:	110 
Size:	48.6 KB 
ID:	8392

  23. #123
    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;
    }

  24. #124
    Senior Member onehorse's Avatar
    Join Date
    Apr 2014
    Location
    Danville, California
    Posts
    921
    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:

    Click image for larger version. 

Name:	LSM9DS0.yawcrosstalk.png 
Views:	125 
Size:	32.1 KB 
ID:	8424

    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 by onehorse; 10-09-2016 at 06:59 PM.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •