Not completely working for me

[ghost]

Hi Kris, I have been trying to make my LSM9DS1 with your code. I just have a few questions.

I am using madwick filter on an arduino uno. MadgwickQuaternionUpdate(ax, ay, az, gx * PI / 180.0f, gy * PI / 180.0f, gz * PI / 180.0f, -mx, my, mz);

`if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_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];   

}`

In the above code why do you comment our the bias on magnetometre?

Secondly with my yaw has a lot of problems. I place the board on a breadboard and leave it on a place surface. i then rotate it by the z axis. but i can not see any changes.

below pic shows me moving in an eight shape and then putting the sensor to rest.

the format is ax,ay,az,gx,gy,gz,mx,my,mz, roll,pitch, yaw. The sensor is flat on a breadboard on a tale and i am rotating it about the z axis.

Is there something i am doing wrong or some registers not outputting something? is there a way to factory reset the sensor?

finally i am using this sketch to visualise my imu. https://github.com/PaulStoffregen/NXPMotionSense/blob/master/OrientationVisualiser/OrientationVisualiser.pde

it just doesnt seem to be doing anything.I rotate it then put it to rest and it reacts so slow. when i move it, it gets confused, goes the wrong way then back to the the other way.

Hope you can help fix this.

Kind Regards

[kriswiner]

Not sure what is wrong but you are not getting correct mag values so everything else is moot.

Are you using Sparkfun's code or what?

On Wed, Aug 16, 2017 at 7:37 AM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Hi Kris, I have been trying to make my LSM9DS1 with your code. I just have a few questions.

I am using madwick filter on an arduino uno. MadgwickQuaternionUpdate(ax, ay, az, gx PI / 180.0f, gy PI / 180.0f, gz * PI / 180.0f, -mx, my, mz);

`if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_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];

}`

In the above code why do you comment our the bias on magnetometre?

Secondly with my yaw has a lot of problems. I place the board on a breadboard and leave it on a place surface. i then rotate it by the z axis. but i can not see any changes.

below pic shows me moving in an eight shape and then putting the sensor to rest. [image: screenshot from 2017-08-16 15-17-13] https://user-images.githubusercontent.com/8300457/29367831-0d225286-8296-11e7-9029-901af4ac34d2.png

the format is ax,ay,az,gx,gy,gz,mx,my,mz, roll,pitch, yaw. The sensor is flat on a breadboard on a tale and i am rotating it about the z axis.

Is there something i am doing wrong or some registers not outputting something? is there a way to factory reset the sensor?

finally i am using this sketch to visualise my imu. https://github.com/PaulStoffregen/NXPMotionSense/blob/master/ OrientationVisualiser/OrientationVisualiser.pde http://url

it just doesnt seem to be doing anything.I rotate it then put it to rest and it reacts so slow. when i move it, it gets confused, goes the wrong way then back to the the other way.

Hope you can help fix this.

Kind Regards

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qnl1jp0C21KqM_sZTN6HKnRTzMQ9ks5sYv7GgaJpZM4O5A5T .

[ghost]

Hi Kris, i have also used the wire library and made changes to user fast mode.

Thanks for your quick response. I am using a sparkfun breakout board but i have made tiny changes to your code. I have commented out most of the MS5611 as the board doesn't have it. If you can help that will be great. The code is below. Why do you comment out the magnetometer bias?

The formatting isn't great below but i hope it makes sense where i made tiny changes.

`/** LSM9DS1_MS5611_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 LSM9DS1 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 LSM9DS1+MS5611 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 MS5611 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 LSM9DS1+MS5611 Teensy 3.1 add-on shield/breakout board.

Hardware setup: LSM9DS1Breakout --------- Arduino VDD ---------------------- 3.3V VDDI --------------------- 3.3V SDA ----------------------- A4 SCL ----------------------- A5 GND ---------------------- GND

Note: The LSM9DS1 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

include

//#include //#include

// 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 MS5611-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet

define MS5611_RESET 0x1E

define MS5611_CONVERT_D1 0x40

define MS5611_CONVERT_D2 0x50

define MS5611_ADC_READ 0x00

// See also LSM9DS1 Register Map and Descriptions, http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00103319.pdf // // Accelerometer and Gyroscope registers

define LSM9DS1XG_ACT_THS 0x04

define LSM9DS1XG_ACT_DUR 0x05

define LSM9DS1XG_INT_GEN_CFG_XL 0x06

define LSM9DS1XG_INT_GEN_THS_X_XL 0x07

define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08

define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09

define LSM9DS1XG_INT_GEN_DUR_XL 0x0A

define LSM9DS1XG_REFERENCE_G 0x0B

define LSM9DS1XG_INT1_CTRL 0x0C

define LSM9DS1XG_INT2_CTRL 0x0D

define LSM9DS1XG_WHO_AM_I 0x0F // should return 0x68

define LSM9DS1XG_CTRL_REG1_G 0x10

define LSM9DS1XG_CTRL_REG2_G 0x11

define LSM9DS1XG_CTRL_REG3_G 0x12

define LSM9DS1XG_ORIENT_CFG_G 0x13

define LSM9DS1XG_INT_GEN_SRC_G 0x14

define LSM9DS1XG_OUT_TEMP_L 0x15

define LSM9DS1XG_OUT_TEMP_H 0x16

define LSM9DS1XG_STATUS_REG 0x17

define LSM9DS1XG_OUT_X_L_G 0x18

define LSM9DS1XG_OUT_X_H_G 0x19

define LSM9DS1XG_OUT_Y_L_G 0x1A

define LSM9DS1XG_OUT_Y_H_G 0x1B

define LSM9DS1XG_OUT_Z_L_G 0x1C

define LSM9DS1XG_OUT_Z_H_G 0x1D

define LSM9DS1XG_CTRL_REG4 0x1E

define LSM9DS1XG_CTRL_REG5_XL 0x1F

define LSM9DS1XG_CTRL_REG6_XL 0x20

define LSM9DS1XG_CTRL_REG7_XL 0x21

define LSM9DS1XG_CTRL_REG8 0x22

define LSM9DS1XG_CTRL_REG9 0x23

define LSM9DS1XG_CTRL_REG10 0x24

define LSM9DS1XG_INT_GEN_SRC_XL 0x26

//#define LSM9DS1XG_STATUS_REG 0x27 // duplicate of 0x17!

define LSM9DS1XG_OUT_X_L_XL 0x28

define LSM9DS1XG_OUT_X_H_XL 0x29

define LSM9DS1XG_OUT_Y_L_XL 0x2A

define LSM9DS1XG_OUT_Y_H_XL 0x2B

define LSM9DS1XG_OUT_Z_L_XL 0x2C

define LSM9DS1XG_OUT_Z_H_XL 0x2D

define LSM9DS1XG_FIFO_CTRL 0x2E

define LSM9DS1XG_FIFO_SRC 0x2F

define LSM9DS1XG_INT_GEN_CFG_G 0x30

define LSM9DS1XG_INT_GEN_THS_XH_G 0x31

define LSM9DS1XG_INT_GEN_THS_XL_G 0x32

define LSM9DS1XG_INT_GEN_THS_YH_G 0x33

define LSM9DS1XG_INT_GEN_THS_YL_G 0x34

define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35

define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36

define LSM9DS1XG_INT_GEN_DUR_G 0x37

// // Magnetometer registers

define LSM9DS1M_OFFSET_X_REG_L_M 0x05

define LSM9DS1M_OFFSET_X_REG_H_M 0x06

define LSM9DS1M_OFFSET_Y_REG_L_M 0x07

define LSM9DS1M_OFFSET_Y_REG_H_M 0x08

define LSM9DS1M_OFFSET_Z_REG_L_M 0x09

define LSM9DS1M_OFFSET_Z_REG_H_M 0x0A

define LSM9DS1M_WHO_AM_I 0x0F // should be 0x3D

define LSM9DS1M_CTRL_REG1_M 0x20

define LSM9DS1M_CTRL_REG2_M 0x21

define LSM9DS1M_CTRL_REG3_M 0x22

define LSM9DS1M_CTRL_REG4_M 0x23

define LSM9DS1M_CTRL_REG5_M 0x24

define LSM9DS1M_STATUS_REG_M 0x27

define LSM9DS1M_OUT_X_L_M 0x28

define LSM9DS1M_OUT_X_H_M 0x29

define LSM9DS1M_OUT_Y_L_M 0x2A

define LSM9DS1M_OUT_Y_H_M 0x2B

define LSM9DS1M_OUT_Z_L_M 0x2C

define LSM9DS1M_OUT_Z_H_M 0x2D

define LSM9DS1M_INT_CFG_M 0x30

define LSM9DS1M_INT_SRC_M 0x31

define LSM9DS1M_INT_THS_L_M 0x32

define LSM9DS1M_INT_THS_H_M 0x33

// Using the LSM9DS1+MS5611 Teensy 3.1 Add-On shield, ADO is set to 1 // Seven-bit device address of accel/gyro is 110101 for ADO = 0 and 110101 for ADO = 1

define ADO 1

if ADO

define LSM9DS1XG_ADDRESS 0x6B // Device address when ADO = 1

define LSM9DS1M_ADDRESS 0x1E // Address of magnetometer

define MS5611_ADDRESS 0x77 // Address of altimeter

else

define LSM9DS1XG_ADDRESS 0x6A // Device address when ADO = 0

define LSM9DS1M_ADDRESS 0x1D // Address of magnetometer

define MS5611_ADDRESS 0x77 // 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_16G, AFS_4G, AFS_8G };

enum Aodr { // set of allowable gyro sample rates AODR_PowerDown = 0, AODR_10Hz, AODR_50Hz, AODR_119Hz, AODR_238Hz, AODR_476Hz, AODR_952Hz };

enum Abw { // set of allowable accewl bandwidths ABW_408Hz = 0, ABW_211Hz, ABW_105Hz, 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_PowerDown = 0, GODR_14_9Hz, GODR_59_5Hz, GODR_119Hz, GODR_238Hz, GODR_476Hz, GODR_952Hz };

enum Gbw { // set of allowable gyro data bandwidths GBW_low = 0, // 14 Hz at Godr = 238 Hz, 33 Hz at Godr = 952 Hz GBW_med, // 29 Hz at Godr = 238 Hz, 40 Hz at Godr = 952 Hz GBW_high, // 63 Hz at Godr = 238 Hz, 58 Hz at Godr = 952 Hz GBW_highest // 78 Hz at Godr = 238 Hz, 100 Hz at Godr = 952 Hz };

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

enum Mmode { MMode_LowPower = 0, MMode_MedPerformance, MMode_HighPerformance, MMode_UltraHighPerformance };

enum Modr { // set of allowable mag sample rates MODR_0_625Hz = 0, MODR_1_25Hz, MODR_2_5Hz, MODR_5Hz, MODR_10Hz, MODR_20Hz, MODR_80Hz };

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_D1 0x40

define ADC_D2 0x50

// Specify sensor full scale uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate uint8_t Gscale = GFS_245DPS; // gyro full scale uint8_t Godr = GODR_238Hz; // gyro data sample rate uint8_t Gbw = GBW_med; // gyro data bandwidth uint8_t Ascale = AFS_16G; // accel full scale uint8_t Aodr = AODR_238Hz; // accel data sample rate uint8_t Abw = ABW_50Hz; // accel data bandwidth uint8_t Mscale = MFS_4G; // mag full scale uint8_t Modr = MODR_10Hz; // mag data sample rate uint8_t Mmode = MMode_HighPerformance; // 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

uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers unsigned char nCRC; // calculated check sum to ensure PROM integrity uint32_t D1 = 0, D2 = 0; // raw MS5611 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 LSM9DS1gyro internal chip temperature in degrees Celsius double Temperature, Pressure; // stores MS5611 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

unsigned int run_time = 0; unsigned int calib_time = 0; unsigned int madwick_time = 0; unsigned int mahony_time = 0;

//

//#define sampleFreq 512.0f // sample frequency in Hz //#define betaDef 0.1f // 2 * proportional gain

//volatile float beta = betaDef; // 2 * proportional gain (Kp) //volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame

//// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" //// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) //// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute //// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. //// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms //// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) {

madwick_time = millis();

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;

madwick_time = millis() - madwick_time; // Serial.print("madwick_time (ms)"); // Serial.println(madwick_time, DEC); // Serial.print("madwick_time (s)"); // Serial.println(madwick_time / 1000, DEC);

}

//--------------------------------------------------------------------------------------------------- // Function declarations //--------------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root // //float invSqrt(float x) { // float halfx = 0.5f x; // float y = x; // long i = (long)&y; // i = 0x5f3759df - (i >> 1); // y = (float)&i; // y = y (1.5f - (halfx y y)); // return y; //}

//==================================================================================================== // Functions

//--------------------------------------------------------------------------------------------------- // AHRS algorithm update // //void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { // // madwick_time = millis(); // // float recipNorm; // float s0, s1, s2, s3; // float qDot1, qDot2, qDot3, qDot4; // float hx, hy; // float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; // // // // Rate of change of quaternion from gyroscope // qDot1 = 0.5f (-q1 gx - q2 gy - q3 gz); // qDot2 = 0.5f (q0 gx + q2 gz - q3 gy); // qDot3 = 0.5f (q0 gy - q1 gz + q3 gx); // qDot4 = 0.5f (q0 gz + q1 gy - q2 gx); // // // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) // if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // // // Normalise accelerometer measurement // recipNorm = invSqrt(ax ax + ay ay + az az); // ax = recipNorm; // ay = recipNorm; // az = recipNorm; // // // Normalise magnetometer measurement // recipNorm = invSqrt(mx mx + my my + mz mz); // mx = recipNorm; // my = recipNorm; // mz = recipNorm; // // // Auxiliary variables to avoid repeated arithmetic // _2q0mx = 2.0f q0 mx; // _2q0my = 2.0f q0 my; // _2q0mz = 2.0f q0 mz; // _2q1mx = 2.0f q1 mx; // _2q0 = 2.0f q0; // _2q1 = 2.0f q1; // _2q2 = 2.0f q2; // _2q3 = 2.0f q3; // _2q0q2 = 2.0f q0 q2; // _2q2q3 = 2.0f q2 q3; // 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; // // // Reference direction of Earth's magnetic field // hx = mx q0q0 - _2q0my q3 + _2q0mz q2 + mx q1q1 + _2q1 my q2 + _2q1 mz q3 - mx q2q2 - mx q3q3; // hy = _2q0mx q3 + my q0q0 - _2q0mz q1 + _2q1mx q2 - my q1q1 + my q2q2 + _2q2 mz q3 - my q3q3; // _2bx = sqrt(hx hx + hy hy); // _2bz = -_2q0mx q2 + _2q0my q1 + mz q0q0 + _2q1mx q3 - mz q1q1 + _2q2 my q3 - mz q2q2 + mz q3q3; // _4bx = 2.0f _2bx; // _4bz = 2.0f _2bz; // // // Gradient decent algorithm corrective step // s0 = -_2q2 (2.0f q1q3 - _2q0q2 - ax) + _2q1 (2.0f q0q1 + _2q2q3 - ay) - _2bz q2 (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (-_2bx q3 + _2bz q1) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + _2bx q2 (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // s1 = _2q3 (2.0f q1q3 - _2q0q2 - ax) + _2q0 (2.0f q0q1 + _2q2q3 - ay) - 4.0f q1 (1 - 2.0f q1q1 - 2.0f q2q2 - az) + _2bz q3 (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (_2bx q2 + _2bz q0) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + (_2bx q3 - _4bz q1) (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // s2 = -_2q0 (2.0f q1q3 - _2q0q2 - ax) + _2q3 (2.0f q0q1 + _2q2q3 - ay) - 4.0f q2 (1 - 2.0f q1q1 - 2.0f q2q2 - az) + (-_4bx q2 - _2bz q0) (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (_2bx q1 + _2bz q3) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + (_2bx q0 - _4bz q2) (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // s3 = _2q1 (2.0f q1q3 - _2q0q2 - ax) + _2q2 (2.0f q0q1 + _2q2q3 - ay) + (-_4bx q3 + _2bz q1) (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (-_2bx q0 + _2bz q2) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + _2bx q1 (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // recipNorm = invSqrt(s0 s0 + s1 s1 + s2 s2 + s3 s3); // normalise step magnitude // s0 = recipNorm; // s1 = recipNorm; // s2 = recipNorm; // s3 = recipNorm; // // // Apply feedback step // qDot1 -= beta s0; // qDot2 -= beta s1; // qDot3 -= beta s2; // qDot4 -= beta s3; // } // // // Integrate rate of change of quaternion to yield quaternion // q0 += qDot1 (1.0f / sampleFreq); // q1 += qDot2 (1.0f / sampleFreq); // q2 += qDot3 (1.0f / sampleFreq); // q3 += qDot4 (1.0f / sampleFreq); // // // Normalise quaternion // recipNorm = invSqrt(q0 q0 + q1 q1 + q2 q2 + q3 q3); // q0 = recipNorm; // q1 = recipNorm; // q2 = recipNorm; // q3 = recipNorm; // // madwick_time = millis() - madwick_time; // // Serial.print("madwick_time (ms)"); // // Serial.println(madwick_time, DEC); // // Serial.print("madwick_time (s)"); // // Serial.println(madwick_time / 1000, DEC); //}

// // //// 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) //{ // // mahony_time = millis(); // 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; // // mahony_time = millis() - mahony_time; // // Serial.print("mahony_time (ms): "); // // Serial.println(mahony_time, DEC); //}

void setup() { calib_time = millis(); Wire.begin(); //start the Wire library to enable i2c on arduino Wire.setClock(400000L);

delay(4000); //Serial.begin(115200); //Serial.begin(1000000); Serial.begin(9600);

// Set up the interrupt pin, its set as active high, push-pull pinMode(intPin, INPUT);

Serial.println("**Calibrating**");

// Read the WHO_AM_I registers, this is a good test of communication Serial.println("LSM9DS1 9-axis motion sensor..."); byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 accel/gyro Serial.print("LSM9DS1 accel/gyro"); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x68, HEX); byte d = readByte(LSM9DS1M_ADDRESS, LSM9DS1M_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 magnetometer Serial.print("LSM9DS1 magnetometer"); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x3D, HEX);

if (c == 0x68 && d == 0x3D) // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag { Serial.println("LSM9DS1 is online...");

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

Serial.println("Perform gyro and accel self test");
selftestLSM9DS1(); // check function of gyro and accelerometer via self test

Serial.println(" Calibrate gyro and accel");
accelgyrocalLSM9DS1(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]);

magcalLSM9DS1(magBias);
Serial.println("mag biases (mG)"); Serial.println(1000.*magBias[0]); Serial.println(1000.*magBias[1]); Serial.println(1000.*magBias[2]);
delay(2000); // add delay to see results before serial spew of data

initLSM9DS1();
Serial.println("LSM9DS1 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

// // Reset the MS5611 pressure sensor
//  MS5611Reset();
//  delay(100);
//  Serial.println("MS5611 pressure sensor reset...");
//  // Read PROM data from MS5611 pressure sensor
//  MS5611PromRead(Pcal);
//  Serial.println("PROM data read:");
//  Serial.print("C0 = "); Serial.println(Pcal[0]);
//  unsigned char refCRC = Pcal[7] & 0x0F;
//  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 = MS5611checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5611 calibration data
//  Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);

} else { Serial.print("Could not connect to LSM9DS1: 0x"); Serial.println(c, HEX); while (1) ; // Loop forever if communication doesn't happen }

calib_time = millis() - calib_time; // Serial.print("calib_time (ms): "); // Serial.println(calib_time, DEC); Serial.print("calib_time (s)"); Serial.println(calib_time / 1000, DEC); }

void loop() { // Serial.println("**");

run_time = millis();

if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x01) { // 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) { // 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(LSM9DS1M_ADDRESS, LSM9DS1M_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];

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

// Serial.print("ax = "); Serial.print(ax); // Serial.print(" ay = "); Serial.print(ay); // Serial.print(" az = "); Serial.print(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( mx ); // Serial.print(" my = "); Serial.print( my ); // Serial.print(" mz = "); Serial.print( mz ); Serial.println(" mG");

// Serial.print("ax = "); Serial.print((int)1000ax); // Serial.print(" ay = "); Serial.print((int)1000ay); // Serial.print(" az = "); Serial.print((int)1000az); 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)1000mx ); // Serial.print(" my = "); Serial.print( (int)1000my ); // Serial.print(" mz = "); Serial.print( (int)1000mz ); Serial.println(" mG"); // // ax = 1000 ax; // ax = 1000 ax; // ax = 1000 ax; // mx = 1000 mx; // my = 1000 my; // mz = 1000 mz;

// Sensors x, y, and z axes of the accelerometer and gyro are aligned. The magnetometer // the magnetometer z-axis (+ up) is aligned with the z-axis (+ up) of accelerometer and gyro, but the magnetometer // x-axis is aligned with the -x axis of the gyro and the magnetometer y axis is aligned with the y axis of the gyro! // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. // For the LSM9DS1, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. // This is ok by aircraft orientation standards! // Pass gyro rate as rad/s MadgwickQuaternionUpdate(ax, ay, az, gx PI / 180.0f, gy PI / 180.0f, gz PI / 180.0f, -mx, my, mz); // MadgwickAHRSupdate(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("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 / 256. + 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 = MS5611Read(ADC_D1, OSR); // get raw pressure value // D2 = MS5611Read(ADC_D2, OSR); // get raw temperature value // dT = D2 - Pcal[5]pow(2,8); // calculate temperature difference from reference // OFFSET = Pcal[2]pow(2, 16) + dTPcal[4]/pow(2,7); // SENS = Pcal[1]pow(2,15) + dTPcal[3]/pow(2,8); // // Temperature = (2000 + (dTPcal[6])/pow(2, 23))/100; // First-order Temperature in degrees Centigrade // // Second order corrections if (Temperature > 20) { T2 = 0; // correction for high temperatures OFFSET2 = 0; SENS2 = 0; } if (Temperature < 20) // correction for low temperature { T2 = dT dT / pow(2, 31); OFFSET2 = 5 (100 Temperature - 2000) (100 Temperature - 2000) / 2; SENS2 = 5 (100 Temperature - 2000) (100 Temperature - 2000) / 4; } if (Temperature < -15) // correction for very low temperature { OFFSET2 = OFFSET2 + 7 (100 Temperature + 1500) (100 Temperature + 1500); SENS2 = SENS2 + 11 (100 Temperature + 1500) (100 * Temperature + 1500) / 2; } // 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]); // // yaw = atan2(double(2 q1 q2 + 2 q0 q3), double(q0 q0 + q1 q1 - q2 q2 - q3 q3)); //yaw // pitch = -asin(2 q0 q2 - 2 q1 q3); // pitch // roll = -atan2(2 q2 q3 + 2 q0 q1, -q0 q0 + q1 q1 + q2 q2 - q3 * q3); //roll

pitch = 180.0f / PI; yaw = (180.0f / PI); // Declination london is 30 degrees ; //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 yaw += 30.8f; // Declination london is 30 degrees roll *= 180.0f / PI;

// Serial.print("Orientation: "); // Serial.print(yaw); // Serial.print(" "); // Serial.print(pitch);// // Serial.print(" "); // Serial.println(roll);

//keep loop below. NJ // if(SerialDebug) { // // Serial.print("Yaw"); // Serial.print("\t"); // Serial.print("Pitch"); // Serial.print("\t"); // Serial.println("Roll"); // // Serial.print(yaw, 2); // Serial.print("\t"); // Serial.print(pitch, 2); // Serial.print("\t"); // 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");

/ // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. // This filter update rate should be fast enough to maintain accurate platform orientation for // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. // The 3.3 V 8 MHz Pro Mini is doing pretty well! / display.setCursor(0, 40); display.print(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(); */

count = millis(); sumCount = 0; sum = 0; // }

run_time = millis() - run_time; printAHRS(); // Serial.print("run_time (ms): "); // Serial.println(run_time, DEC); // // Serial.print("run_time (s): "); // // Serial.println(run_time/1000, DEC); // Serial.println("**");

}

//=================================================================================================================== //====== 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: // 4 Gauss (00), 8 Gauss (01), 12 Gauss (10) and 16 Gauss (11) 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; case MFS_16G: mRes = 16.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 (00), 16 Gs (01), 4 Gs (10), and 8 Gs (11). caseAFS_16G: aRes = 2.0 / 32768.0; break; case AFS_16G: aRes = 16.0 / 32768.0; break; case AFS_4G: aRes = 4.0 / 32768.0; break; case AFS_8G: aRes = 8.0 / 32768.0; break; } }

void readAccelData(int16_t * destination) { uint8_t rawData[6]; // x/y/z accel register data stored here readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_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(LSM9DS1M_ADDRESS, LSM9DS1M_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] ; }

int16_t readTempData() { uint8_t rawData[2]; // x/y/z gyro register data stored here readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_TEMP_L, 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 initLSM9DS1() { // enable the 3-axes of the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // configure the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); delay(200); // enable the three axes of the accelerometer writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // configure the accelerometer-specify bandwidth selection with Abw writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 | Abw); delay(200); // enable block data update, allow auto-increment during multiple byte read writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); // configure the magnetometer-enable temperature compensation of mag data writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode }

void selftestLSM9DS1() { float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.}; float gyro_noST[3] = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.};

writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test accelgyrocalLSM9DS1(gyro_noST, accel_noST); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // enable gyro/accel self test accelgyrocalLSM9DS1(gyro_ST, accel_ST);

float gyrodx = (gyro_ST[0] - gyro_noST[0]); float gyrody = (gyro_ST[1] - gyro_noST[1]); float gyrodz = (gyro_ST[2] - gyro_noST[2]);

Serial.println("Gyro self-test results: "); Serial.print("x-axis = "); Serial.print(gyrodx); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); Serial.print("y-axis = "); Serial.print(gyrody); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); Serial.print("z-axis = "); Serial.print(gyrodz); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps");

float accdx = 1000.(accel_ST[0] - accel_noST[0]); float accdy = 1000.(accel_ST[1] - accel_noST[1]); float accdz = 1000.*(accel_ST[2] - accel_noST[2]);

Serial.println("Accelerometer self-test results: "); Serial.print("x-axis = "); Serial.print(accdx); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); Serial.print("y-axis = "); Serial.print(accdy); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); Serial.print("z-axis = "); Serial.print(accdz); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg");

writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test delay(200); } // 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 accelgyrocalLSM9DS1(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; // enable the 3-axes of the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // configure the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw);

delay(200); // enable the three axes of the accelerometer writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // configure the accelerometer-specify bandwidth selection with Abw writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 | Abw); delay(200); // enable block data update, allow auto-increment during multiple byte read writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);

// First get gyro bias byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable gyro FIFO delay(50); // Wait for change to take effect writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples

samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_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(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO delay(50); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode

// now get the accelerometer bias c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable accel FIFO delay(50); // Wait for change to take effect writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples

samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples

for (ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO int16_t accel_temp[3] = {0, 0, 0}; readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO delay(50); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode }

void magcalLSM9DS1(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};

// configure the magnetometer-enable temperature compensation of mag data writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode

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(LSM9DS1M_ADDRESS, LSM9DS1M_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(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF);

# Serial.println("Mag Calibration done!"); }

//// I2C communication with the MS5611 is a little different from that with the LSM9DS1and most other sensors //// For the MS5611, we write commands, and the MS5611 sends data in response, rather than directly reading //// MS5611 registers // // void MS5611Reset() // { // Wire.beginTransmission(MS5611_ADDRESS); // Initialize the Tx buffer // Wire.write(MS5611_RESET); // Put reset command in Tx buffer // Wire.endTransmission(); // Send the Tx buffer // } // // void MS5611PromRead(uint16_t destination) // { // uint8_t data[2] = {0,0}; // for (uint8_t ii = 0; ii <8; ii++) { // Wire.beginTransmission(MS5611_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 // Wire.endTransmission(); // Send the Tx buffer, but send a restart to keep connection alive // uint8_t i = 0; // Wire.requestFrom(MS5611_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 MS5611Read(uint8_t CMD, uint8_t OSR) // temperature data read // { // uint8_t data[3] = {0,0,0}; // Wire.beginTransmission(MS5611_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 // Wire.endTransmission(); // 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; // } // // Wire.beginTransmission(MS5611_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 // Wire.endTransmission(); // Send the Tx buffer, but send a restart to keep connection alive // uint8_t i = 0; // Wire.requestFrom(MS5611_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 MS5611checkCRC(uint16_t n_prom) // calculate checksum from PROM register contents //{ //int cnt; // simple counter //unsigned int n_rem; // crc reminder //unsigned int crc_read; // original value of the crc //unsigned char n_bit; //n_rem = 0x00; //crc_read=n_prom[7]; //save read CRC //n_prom[7]=(0xFF00 & (n_prom[7])); // CRC byte is replaced by 0 // //for (cnt = 0; cnt < 16; cnt++) // operation is performed on bytes //{// choose LSB or MSB //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= (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code //n_prom[7]=crc_read; // restore the crc_read to its original place //return (n_rem ^ 0x0); //} //

// I2C read/write functions for the LSM9DS1and 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(); // 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(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(); // 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 printAHRS() {

Serial.print("acc(g): "); Serial.print("\t"); Serial.print(ax); Serial.print("\t"); Serial.print(ay); Serial.print("\t"); Serial.print(az); Serial.print("\t"); Serial.print("gyro(d/s): "); Serial.print("\t"); Serial.print(gx); Serial.print("\t"); Serial.print(gy); Serial.print("\t"); Serial.print(gz); Serial.print("\t"); Serial.print("mag(G): "); Serial.print("\t"); Serial.print(mx); Serial.print("\t"); Serial.print(my); Serial.print("\t"); Serial.print(mz); Serial.print("\t");

Serial.print("Orientation: "); Serial.print("\t"); Serial.print(yaw); Serial.print("\t"); Serial.print(pitch); Serial.print("\t"); Serial.print(roll); Serial.print("\t");

Serial.print("Quat: "); Serial.print("\t"); Serial.print(q[0]); Serial.print("\t"); Serial.print(q[1]); Serial.print("\t"); Serial.print(q[2]); Serial.print("\t"); Serial.println(q[3]); //Serial.print(q0); Serial.print("\t"); Serial.print(q1); Serial.print("\t"); Serial.print(q2); Serial.print("\t"); Serial.println(q3); } `

[kriswiner]

Not sure what to make of this, I can;t diagnose your code for you. You are outputting accel in gs and mag in Gauss. hard to tell what is going on.

On Wed, Aug 16, 2017 at 9:18 AM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Hi Kris, i have also used the wire library and made changes to user fast mode.

Thanks for your quick response. I am using a sparkfun breakout board but i have made tiny changes to your code. I have commented out most of the MS5611 as the board doesn't have it. If you can help that will be great. The code is below. Why do you comment out the magnetometer bias?

The formatting isn't great below but i hope it makes sense where i made tiny changes.

`/** LSM9DS1_MS5611_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 LSM9DS1 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 LSM9DS1+MS5611 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 MS5611 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 LSM9DS1+MS5611 Teensy 3.1 add-on shield/breakout board.

Hardware setup: LSM9DS1Breakout --------- Arduino VDD ---------------------- 3.3V VDDI --------------------- 3.3V SDA ----------------------- A4 SCL ----------------------- A5 GND ---------------------- GND

Note: The LSM9DS1 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

include

//#include //#include

// 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 MS5611-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet

define MS5611_RESET 0x1E

define MS5611_CONVERT_D1 0x40

define MS5611_CONVERT_D2 0x50

define MS5611_ADC_READ 0x00

// See also LSM9DS1 Register Map and Descriptions, http://www.st.com/st-web-ui/static/active/en/resource/ technical/document/datasheet/DM00103319.pdf // // Accelerometer and Gyroscope registers

define LSM9DS1XG_ACT_THS 0x04

define LSM9DS1XG_ACT_DUR 0x05

define LSM9DS1XG_INT_GEN_CFG_XL 0x06

define LSM9DS1XG_INT_GEN_THS_X_XL 0x07

define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08

define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09

define LSM9DS1XG_INT_GEN_DUR_XL 0x0A

define LSM9DS1XG_REFERENCE_G 0x0B

define LSM9DS1XG_INT1_CTRL 0x0C

define LSM9DS1XG_INT2_CTRL 0x0D

define LSM9DS1XG_WHO_AM_I 0x0F // should return 0x68

define LSM9DS1XG_CTRL_REG1_G 0x10

define LSM9DS1XG_CTRL_REG2_G 0x11

define LSM9DS1XG_CTRL_REG3_G 0x12

define LSM9DS1XG_ORIENT_CFG_G 0x13

define LSM9DS1XG_INT_GEN_SRC_G 0x14

define LSM9DS1XG_OUT_TEMP_L 0x15

define LSM9DS1XG_OUT_TEMP_H 0x16

define LSM9DS1XG_STATUS_REG 0x17

define LSM9DS1XG_OUT_X_L_G 0x18

define LSM9DS1XG_OUT_X_H_G 0x19

define LSM9DS1XG_OUT_Y_L_G 0x1A

define LSM9DS1XG_OUT_Y_H_G 0x1B

define LSM9DS1XG_OUT_Z_L_G 0x1C

define LSM9DS1XG_OUT_Z_H_G 0x1D

define LSM9DS1XG_CTRL_REG4 0x1E

define LSM9DS1XG_CTRL_REG5_XL 0x1F

define LSM9DS1XG_CTRL_REG6_XL 0x20

define LSM9DS1XG_CTRL_REG7_XL 0x21

define LSM9DS1XG_CTRL_REG8 0x22

define LSM9DS1XG_CTRL_REG9 0x23

define LSM9DS1XG_CTRL_REG10 0x24

define LSM9DS1XG_INT_GEN_SRC_XL 0x26

//#define LSM9DS1XG_STATUS_REG 0x27 // duplicate of 0x17!

define LSM9DS1XG_OUT_X_L_XL 0x28

define LSM9DS1XG_OUT_X_H_XL 0x29

define LSM9DS1XG_OUT_Y_L_XL 0x2A

define LSM9DS1XG_OUT_Y_H_XL 0x2B

define LSM9DS1XG_OUT_Z_L_XL 0x2C

define LSM9DS1XG_OUT_Z_H_XL 0x2D

define LSM9DS1XG_FIFO_CTRL 0x2E

define LSM9DS1XG_FIFO_SRC 0x2F

define LSM9DS1XG_INT_GEN_CFG_G 0x30

define LSM9DS1XG_INT_GEN_THS_XH_G 0x31

define LSM9DS1XG_INT_GEN_THS_XL_G 0x32

define LSM9DS1XG_INT_GEN_THS_YH_G 0x33

define LSM9DS1XG_INT_GEN_THS_YL_G 0x34

define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35

define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36

define LSM9DS1XG_INT_GEN_DUR_G 0x37

// // Magnetometer registers

define LSM9DS1M_OFFSET_X_REG_L_M 0x05

define LSM9DS1M_OFFSET_X_REG_H_M 0x06

define LSM9DS1M_OFFSET_Y_REG_L_M 0x07

define LSM9DS1M_OFFSET_Y_REG_H_M 0x08

define LSM9DS1M_OFFSET_Z_REG_L_M 0x09

define LSM9DS1M_OFFSET_Z_REG_H_M 0x0A

define LSM9DS1M_WHO_AM_I 0x0F // should be 0x3D

define LSM9DS1M_CTRL_REG1_M 0x20

define LSM9DS1M_CTRL_REG2_M 0x21

define LSM9DS1M_CTRL_REG3_M 0x22

define LSM9DS1M_CTRL_REG4_M 0x23

define LSM9DS1M_CTRL_REG5_M 0x24

define LSM9DS1M_STATUS_REG_M 0x27

define LSM9DS1M_OUT_X_L_M 0x28

define LSM9DS1M_OUT_X_H_M 0x29

define LSM9DS1M_OUT_Y_L_M 0x2A

define LSM9DS1M_OUT_Y_H_M 0x2B

define LSM9DS1M_OUT_Z_L_M 0x2C

define LSM9DS1M_OUT_Z_H_M 0x2D

define LSM9DS1M_INT_CFG_M 0x30

define LSM9DS1M_INT_SRC_M 0x31

define LSM9DS1M_INT_THS_L_M 0x32

define LSM9DS1M_INT_THS_H_M 0x33

// Using the LSM9DS1+MS5611 Teensy 3.1 Add-On shield, ADO is set to 1 // Seven-bit device address of accel/gyro is 110101 for ADO = 0 and 110101 for ADO = 1

define ADO 1

if ADO

define LSM9DS1XG_ADDRESS 0x6B // Device address when ADO = 1

define LSM9DS1M_ADDRESS 0x1E // Address of magnetometer

define MS5611_ADDRESS 0x77 // Address of altimeter

else

define LSM9DS1XG_ADDRESS 0x6A // Device address when ADO = 0

define LSM9DS1M_ADDRESS 0x1D // Address of magnetometer

define MS5611_ADDRESS 0x77 // 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_16G, AFS_4G, AFS_8G };

enum Aodr { // set of allowable gyro sample rates AODR_PowerDown = 0, AODR_10Hz, AODR_50Hz, AODR_119Hz, AODR_238Hz, AODR_476Hz, AODR_952Hz };

enum Abw { // set of allowable accewl bandwidths ABW_408Hz = 0, ABW_211Hz, ABW_105Hz, 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_PowerDown = 0, GODR_14_9Hz, GODR_59_5Hz, GODR_119Hz, GODR_238Hz, GODR_476Hz, GODR_952Hz };

enum Gbw { // set of allowable gyro data bandwidths GBW_low = 0, // 14 Hz at Godr = 238 Hz, 33 Hz at Godr = 952 Hz GBW_med, // 29 Hz at Godr = 238 Hz, 40 Hz at Godr = 952 Hz GBW_high, // 63 Hz at Godr = 238 Hz, 58 Hz at Godr = 952 Hz GBW_highest // 78 Hz at Godr = 238 Hz, 100 Hz at Godr = 952 Hz };

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

enum Mmode { MMode_LowPower = 0, MMode_MedPerformance, MMode_HighPerformance, MMode_UltraHighPerformance };

enum Modr { // set of allowable mag sample rates MODR_0_625Hz = 0, MODR_1_25Hz, MODR_2_5Hz, MODR_5Hz, MODR_10Hz, MODR_20Hz, MODR_80Hz };

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_D1 0x40

define ADC_D2 0x50

// Specify sensor full scale uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate uint8_t Gscale = GFS_245DPS; // gyro full scale uint8_t Godr = GODR_238Hz; // gyro data sample rate uint8_t Gbw = GBW_med; // gyro data bandwidth uint8_t Ascale = AFS_16G; // accel full scale uint8_t Aodr = AODR_238Hz; // accel data sample rate uint8_t Abw = ABW_50Hz; // accel data bandwidth uint8_t Mscale = MFS_4G; // mag full scale uint8_t Modr = MODR_10Hz; // mag data sample rate uint8_t Mmode = MMode_HighPerformance; // 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

uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers unsigned char nCRC; // calculated check sum to ensure PROM integrity uint32_t D1 = 0, D2 = 0; // raw MS5611 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 LSM9DS1gyro internal chip temperature in degrees Celsius double Temperature, Pressure; // stores MS5611 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

unsigned int run_time = 0; unsigned int calib_time = 0; unsigned int madwick_time = 0; unsigned int mahony_time = 0;

//

//#define sampleFreq 512.0f // sample frequency in Hz //#define betaDef 0.1f // 2 * proportional gain

//volatile float beta = betaDef; // 2 * proportional gain (Kp) //volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame

//// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" //// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) //// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute //// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. //// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms //// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) {

madwick_time = millis();

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;

madwick_time = millis() - madwick_time; // Serial.print("madwick_time (ms)"); // Serial.println(madwick_time, DEC); // Serial.print("madwick_time (s)"); // Serial.println(madwick_time / 1000, DEC);

}

//----------------------------------------------------------

// Function declarations //----------------------------------------------------------

// Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root // //float invSqrt(float x) { // float halfx = 0.5f x; // float y = x; // long i = (long)&y; // i = 0x5f3759df - (i >> 1); // y = (float)&i; // y = y (1.5f - (halfx y y)); // return y; //}

//==========================================================

// Functions

//----------------------------------------------------------

// AHRS algorithm update // //void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { // // madwick_time = millis(); // // float recipNorm; // float s0, s1, s2, s3; // float qDot1, qDot2, qDot3, qDot4; // float hx, hy; // float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; // // // // Rate of change of quaternion from gyroscope // qDot1 = 0.5f (-q1 gx - q2 gy - q3 gz); // qDot2 = 0.5f (q0 gx + q2 gz - q3 gy); // qDot3 = 0.5f (q0 gy - q1 gz + q3 gx); // qDot4 = 0.5f (q0 gz + q1 gy - q2 gx); // // // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) // if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // // // Normalise accelerometer measurement // recipNorm = invSqrt(ax ax + ay ay + az az); // ax = recipNorm; // ay = recipNorm; // az = recipNorm; // // // Normalise magnetometer measurement // recipNorm = invSqrt(mx mx + my my + mz mz); // mx = recipNorm; // my = recipNorm; // mz = recipNorm; // // // Auxiliary variables to avoid repeated arithmetic // _2q0mx = 2.0f q0 mx; // _2q0my = 2.0f q0 my; // _2q0mz = 2.0f q0 mz; // _2q1mx = 2.0f q1 mx; // _2q0 = 2.0f q0; // _2q1 = 2.0f q1; // _2q2 = 2.0f q2; // _2q3 = 2.0f q3; // _2q0q2 = 2.0f q0 q2; // _2q2q3 = 2.0f q2 q3; // 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; // // // Reference direction of Earth's magnetic field // hx = mx q0q0 - _2q0my q3 + _2q0mz q2 + mx q1q1 + _2q1 my q2

• _2q1 mz q3 - mx q2q2 - mx q3q3; // hy = _2q0mx q3 + my q0q0 - _2q0mz q1 + _2q1mx q2 - my q1q1 + my q2q2 + _2q2 mz q3 - my q3q3; // _2bx = sqrt(hx hx + hy hy); // _2bz = -_2q0mx q2 + _2q0my q1 + mz q0q0 + _2q1mx q3 - mz q1q1
• _2q2 my q3 - mz q2q2 + mz q3q3; // _4bx = 2.0f _2bx; // _4bz = 2.0f _2bz; // // // Gradient decent algorithm corrective step // s0 = -_2q2 (2.0f q1q3 - _2q0q2 - ax) + _2q1 (2.0f q0q1 + _2q2q3
• ay) - _2bz q2 (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (-_2bx q3 + _2bz q1) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + _2bx q2 (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // s1 = _2q3 (2.0f q1q3 - _2q0q2 - ax) + _2q0 (2.0f * q0q1 + _2q2q3
• ay) - 4.0f q1 (1 - 2.0f q1q1 - 2.0f q2q2 - az) + _2bz q3 (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (_2bx q2 + _2bz q0) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + (_2bx q3 - _4bz q1) (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // s2 = -_2q0 (2.0f q1q3 - _2q0q2 - ax) + _2q3 (2.0f q0q1 + _2q2q3
• ay) - 4.0f q2 (1 - 2.0f q1q1 - 2.0f q2q2 - az) + (-_4bx q2 - _2bz q0) (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (_2bx q1 + _2bz q3) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + (_2bx q0 - _4bz q2) (_2bx (q0q2 + q1q3) + _2bz * (0.5f - q1q1
• q2q2) - mz); // s3 = _2q1 (2.0f q1q3 - _2q0q2 - ax) + _2q2 (2.0f q0q1 + _2q2q3
• ay) + (-_4bx q3 + _2bz q1) (_2bx (0.5f - q2q2 - q3q3) + _2bz (q1q3 - q0q2) - mx) + (-_2bx q0 + _2bz q2) (_2bx (q1q2 - q0q3) + _2bz (q0q1 + q2q3) - my) + _2bx q1 (_2bx (q0q2 + q1q3) + _2bz (0.5f - q1q1 - q2q2) - mz); // recipNorm = invSqrt(s0 s0 + s1 s1 + s2 s2 + s3 s3); // normalise step magnitude // s0 = recipNorm; // s1 = recipNorm; // s2 = recipNorm; // s3 = recipNorm; // // // Apply feedback step // qDot1 -= beta s0; // qDot2 -= beta s1; // qDot3 -= beta s2; // qDot4 -= beta s3; // } // // // Integrate rate of change of quaternion to yield quaternion // q0 += qDot1 (1.0f / sampleFreq); // q1 += qDot2 (1.0f / sampleFreq); // q2 += qDot3 (1.0f / sampleFreq); // q3 += qDot4 (1.0f / sampleFreq); // // // Normalise quaternion // recipNorm = invSqrt(q0 q0 + q1 q1 + q2 q2 + q3 q3); // q0 = recipNorm; // q1 = recipNorm; // q2 = recipNorm; // q3 = recipNorm; // // madwick_time = millis() - madwick_time; // // Serial.print("madwick_time (ms)"); // // Serial.println(madwick_time, DEC); // // Serial.print("madwick_time (s)"); // // Serial.println(madwick_time / 1000, DEC); //}

// // //// 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) //{ // // mahony_time = millis(); // 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; // // mahony_time = millis() - mahony_time; // // Serial.print("mahony_time (ms): "); // // Serial.println(mahony_time, DEC); //}

void setup() { calib_time = millis(); Wire.begin(); //start the Wire library to enable i2c on arduino Wire.setClock(400000L);

delay(4000); //Serial.begin(115200); //Serial.begin(1000000); Serial.begin(9600);

// Set up the interrupt pin, its set as active high, push-pull pinMode(intPin, INPUT);

Serial.println("*Calibrating*****");

// Read the WHO_AM_I registers, this is a good test of communication Serial.println("LSM9DS1 9-axis motion sensor..."); byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 accel/gyro Serial.print("LSM9DS1 accel/gyro"); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x68, HEX); byte d = readByte(LSM9DS1M_ADDRESS, LSM9DS1M_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 magnetometer Serial.print("LSM9DS1 magnetometer"); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x3D, HEX);

if (c == 0x68 && d == 0x3D) // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag { Serial.println("LSM9DS1 is online...");

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

Serial.println("Perform gyro and accel self test"); selftestLSM9DS1(); // check function of gyro and accelerometer via self test

Serial.println(" Calibrate gyro and accel"); accelgyrocalLSM9DS1(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]);

magcalLSM9DS1(magBias); Serial.println("mag biases (mG)"); Serial.println(1000.magBias[0]); Serial.println(1000.magBias[1]); Serial.println(1000.*magBias[2]); delay(2000); // add delay to see results before serial spew of data

initLSM9DS1(); Serial.println("LSM9DS1 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

// // Reset the MS5611 pressure sensor // MS5611Reset(); // delay(100); // Serial.println("MS5611 pressure sensor reset..."); // // Read PROM data from MS5611 pressure sensor // MS5611PromRead(Pcal); // Serial.println("PROM data read:"); // Serial.print("C0 = "); Serial.println(Pcal[0]); // unsigned char refCRC = Pcal[7] & 0x0F; // 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 = MS5611checkCRC(Pcal); //calculate checksum to ensure integrity of MS5611 calibration data // Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);

} else { Serial.print("Could not connect to LSM9DS1: 0x"); Serial.println(c, HEX); while (1) ; // Loop forever if communication doesn't happen }

calib_time = millis() - calib_time; // Serial.print("calib_time (ms): "); // Serial.println(calib_time, DEC); Serial.print("calib_time (s)"); Serial.println(calib_time / 1000, DEC); }

void loop() { // Serial.println("****

---

**");

run_time = millis();

if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x01) { // 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) { // 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(LSM9DS1M_ADDRESS, LSM9DS1M_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];

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

// Serial.print("ax = "); Serial.print(ax); // Serial.print(" ay = "); Serial.print(ay); // Serial.print(" az = "); Serial.print(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( mx ); // Serial.print(" my = "); Serial.print( my ); // Serial.print(" mz = "); Serial.print( mz ); Serial.println(" mG");

// Serial.print("ax = "); Serial.print((int)1000 ax); // Serial.print(" ay = "); Serial.print((int)1000ay); // 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)1000mx ); // Serial.print(" my = "); Serial.print( (int)1000 my ); // Serial.print(" mz = "); Serial.print( (int)1000mz ); Serial.println(" mG"); // // ax = 1000 ax; // ax = 1000 ax; // ax = 1000 ax; // mx = 1000 mx; // my = 1000 my; // mz = 1000 mz;

// Sensors x, y, and z axes of the accelerometer and gyro are aligned. The magnetometer // the magnetometer z-axis (+ up) is aligned with the z-axis (+ up) of accelerometer and gyro, but the magnetometer // x-axis is aligned with the -x axis of the gyro and the magnetometer y axis is aligned with the y axis of the gyro! // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. // For the LSM9DS1, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. // This is ok by aircraft orientation standards! // Pass gyro rate as rad/s MadgwickQuaternionUpdate(ax, ay, az, gx PI / 180.0f, gy PI / 180.0f, gz PI / 180.0f, -mx, my, mz); // MadgwickAHRSupdate(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("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 / 256. + 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 = MS5611Read(ADC_D1, OSR); // get raw pressure value // D2 = MS5611Read(ADC_D2, OSR); // get raw temperature value // dT = D2 - Pcal[5]

pow(2,8); // calculate temperature difference from reference // OFFSET = Pcal[2]pow(2, 16) + dTPcal[4]/pow(2,7); // SENS = Pcal[1]pow(2,15) + dTPcal[3]/pow(2,8); // // Temperature = (2000 + (dTPcal[6])/pow(2, 23))/100; // First-order Temperature in degrees Centigrade // // Second order corrections if (Temperature > 20) { T2 = 0; // correction for high temperatures OFFSET2 = 0; SENS2 = 0; } if (Temperature < 20) // correction for low temperature { T2 = dT dT / pow(2, 31); OFFSET2 = 5 (100 Temperature - 2000) (100 Temperature - 2000) / 2; SENS2 = 5 (100 Temperature - 2000) (100 Temperature - 2000) / 4; } if (Temperature < -15) // correction for very low temperature { OFFSET2 = OFFSET2 + 7 (100 Temperature + 1500) (100 Temperature + 1500); SENS2 = SENS2 + 11 (100 Temperature + 1500) (100 * Temperature + 1500) / 2; } // 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]); // // yaw = atan2(double(2 q1 q2 + 2 q0 q3), double(q0 q0 + q1 q1
• q2 q2 - q3 q3)); //yaw // pitch = -asin(2 q0 q2 - 2 q1 q3); // pitch // roll = -atan2(2 q2 q3 + 2 q0 q1, -q0 q0 + q1 q1 + q2 q2 - q3 q3); //roll

pitch = 180.0f / PI; yaw = (180.0f / PI); // Declination london is 30 degrees ; //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 yaw += 30.8f; // Declination london is 30 degrees roll *= 180.0f / PI;

// Serial.print("Orientation: "); // Serial.print(yaw); // Serial.print(" "); // Serial.print(pitch);// // Serial.print(" "); // Serial.println(roll);

//keep loop below. NJ // if(SerialDebug) { // // Serial.print("Yaw"); // Serial.print("\t"); // Serial.print("Pitch"); // Serial.print("\t"); // Serial.println("Roll"); // // Serial.print(yaw, 2); // Serial.print("\t"); // Serial.print(pitch, 2); // Serial.print("\t"); // 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)(1000ax)); display.setCursor(24, 8); display.print((int)(1000ay)); 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");

/ // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. // This filter update rate should be fast enough to maintain accurate platform orientation for // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. // The 3.3 V 8 MHz Pro Mini is doing pretty well! / display.setCursor(0, 40); display.print(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(); */

count = millis(); sumCount = 0; sum = 0; // }

run_time = millis() - run_time; printAHRS(); // Serial.print("run_time (ms): "); // Serial.println(run_time, DEC); // // Serial.print("run_time (s): "); // // Serial.println(run_time/1000, DEC); // Serial.println("****

---

**");

}

//==========================================================

//====== 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: // 4 Gauss (00), 8 Gauss (01), 12 Gauss (10) and 16 Gauss (11) 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; case MFS_16G: mRes = 16.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 (00), 16 Gs (01), 4 Gs (10), and 8 Gs (11). caseAFS_16G: aRes = 2.0 / 32768.0; break; case AFS_16G: aRes = 16.0 / 32768.0; break; case AFS_4G: aRes = 4.0 / 32768.0; break; case AFS_8G: aRes = 8.0 / 32768.0; break; } }

void readAccelData(int16_t * destination) { uint8_t rawData[6]; // x/y/z accel register data stored here readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_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(LSM9DS1M_ADDRESS, LSM9DS1M_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] ; }

int16_t readTempData() { uint8_t rawData[2]; // x/y/z gyro register data stored here readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_TEMP_L, 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 initLSM9DS1() { // enable the 3-axes of the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // configure the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); delay(200); // enable the three axes of the accelerometer writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // configure the accelerometer-specify bandwidth selection with Abw writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 | Abw); delay(200); // enable block data update, allow auto-increment during multiple byte read writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); // configure the magnetometer-enable temperature compensation of mag data writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode }

void selftestLSM9DS1() { float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.}; float gyro_noST[3] = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.};

writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test accelgyrocalLSM9DS1(gyro_noST, accel_noST); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // enable gyro/accel self test accelgyrocalLSM9DS1(gyro_ST, accel_ST);

float gyrodx = (gyro_ST[0] - gyro_noST[0]); float gyrody = (gyro_ST[1] - gyro_noST[1]); float gyrodz = (gyro_ST[2] - gyro_noST[2]);

Serial.println("Gyro self-test results: "); Serial.print("x-axis = "); Serial.print(gyrodx); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); Serial.print("y-axis = "); Serial.print(gyrody); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); Serial.print("z-axis = "); Serial.print(gyrodz); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps");

float accdx = 1000. (accel_ST[0] - accel_noST[0]); float accdy = 1000.(accel_ST[1] - accel_noST[1]); float accdz = 1000.*(accel_ST[2] - accel_noST[2]);

Serial.println("Accelerometer self-test results: "); Serial.print("x-axis = "); Serial.print(accdx); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); Serial.print("y-axis = "); Serial.print(accdy); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); Serial.print("z-axis = "); Serial.print(accdz); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg");

writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test delay(200); } // 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 accelgyrocalLSM9DS1(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; // enable the 3-axes of the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); // configure the gyroscope writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw);

delay(200); // enable the three axes of the accelerometer writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); // configure the accelerometer-specify bandwidth selection with Abw writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 | Abw); delay(200); // enable block data update, allow auto-increment during multiple byte read writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44);

// First get gyro bias byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable gyro FIFO delay(50); // Wait for change to take effect writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples

samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_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(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO delay(50); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode

// now get the accelerometer bias c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable accel FIFO delay(50); // Wait for change to take effect writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples delay(1000); // delay 1000 milliseconds to collect FIFO samples

samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples

for (ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO int16_t accel_temp[3] = {0, 0, 0}; readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 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(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO delay(50); writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode }

void magcalLSM9DS1(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};

// configure the magnetometer-enable temperature compensation of mag data writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode

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(LSM9DS1M_ADDRESS, LSM9DS1M_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(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF); writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF);

Serial.println("Mag Calibration done!"); }

//// I2C communication with the MS5611 is a little different from that with the LSM9DS1and most other sensors //// For the MS5611, we write commands, and the MS5611 sends data in response, rather than directly reading //// MS5611 registers // // void MS5611Reset() // { // Wire.beginTransmission(MS5611_ADDRESS); // Initialize the Tx buffer // Wire.write(MS5611_RESET); // Put reset command in Tx buffer // Wire.endTransmission(); // Send the Tx buffer // } // // void MS5611PromRead(uint16_t destination) // { // uint8_t data[2] = {0,0}; // for (uint8_t ii = 0; ii <8; ii++) { // Wire.beginTransmission(MS5611_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 // Wire.endTransmission(); // Send the Tx buffer, but send a restart to keep connection alive // uint8_t i = 0; // Wire.requestFrom(MS5611_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 MS5611Read(uint8_t CMD, uint8_t OSR) // temperature data read // { // uint8_t data[3] = {0,0,0}; // Wire.beginTransmission(MS5611_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 // Wire.endTransmission(); // 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; // } // // Wire.beginTransmission(MS5611_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 // Wire.endTransmission(); // Send the Tx buffer, but send a restart to keep connection alive // uint8_t i = 0; // Wire.requestFrom(MS5611_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 MS5611checkCRC(uint16_t n_prom) // calculate checksum from PROM register contents //{ //int cnt; // simple counter //unsigned int n_rem; // crc reminder //unsigned int crc_read; // original value of the crc //unsigned char n_bit; //n_rem = 0x00; //crc_read=n_prom[7]; //save read CRC //n_prom[7]=(0xFF00 & (n_prom[7])); // CRC byte is replaced by 0 // //for (cnt = 0; cnt < 16; cnt++) // operation is performed on bytes //{// choose LSB or MSB //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= (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code //n_prom[7]=crc_read; // restore the crc_read to its original place //return (n_rem ^ 0x0); //} //

// I2C read/write functions for the LSM9DS1and 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(); // 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(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(); // 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 printAHRS() {

Serial.print("acc(g): "); Serial.print("\t"); Serial.print(ax); Serial.print("\t"); Serial.print(ay); Serial.print("\t"); Serial.print(az); Serial.print("\t"); Serial.print("gyro(d/s): "); Serial.print("\t"); Serial.print(gx); Serial.print("\t"); Serial.print(gy); Serial.print("\t"); Serial.print(gz); Serial.print("\t"); Serial.print("mag(G): "); Serial.print("\t"); Serial.print(mx); Serial.print("\t"); Serial.print(my); Serial.print("\t"); Serial.print(mz); Serial.print("\t");

Serial.print("Orientation: "); Serial.print("\t"); Serial.print(yaw); Serial.print("\t"); Serial.print(pitch); Serial.print("\t"); Serial.print(roll); Serial.print("\t");

Serial.print("Quat: "); Serial.print("\t"); Serial.print(q[0]); Serial.print("\t"); Serial.print(q[1]); Serial.print("\t"); Serial.print(q[2]); Serial.print("\t"); Serial.println(q[3]); //Serial.print(q0); Serial.print("\t"); Serial.print(q1); Serial.print("\t"); Serial.print(q2); Serial.print("\t"); Serial.println(q3); } `

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-322823879, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qkRjsVqJKRIQOCjdp6chRKIaxGB-ks5sYxZSgaJpZM4O5A5T .

[ghost]

is there a particular reason for working in mg and mG? I thought the Madwick needed the values in standard units?

Any idea how i can reset this to factory settings? please

[kriswiner]

The madwick filter uses normalized units for accel and mag (but no gyro), so it doesn't matter, but if you want a human to diagnose your problem better to use units that humans can understand.

Reset to factory settings??? Ask Sparkfun...

On Wed, Aug 16, 2017 at 10:12 AM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

is there a particular reason for working in mg and mG? I thought the Madwick needed the values in standard units?

Any idea how i can reset this to factory settings? please

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-322838778, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qiAxNTkS3dps3qZ6W-m1ooQjU5sPks5sYyMOgaJpZM4O5A5T .

[ghost]

Thank you Kris, i will try to run your exact code and post the results with sensor at rest after calibration. Thank you for your help.

[ghost]

Hi Kris,

I have run the your code and just formatted the order of printing. it is printing in mg and mG. I perform the calibration and put the sensor at rest.

Also my magnetometer calibration results, the bias can all be negative and can be a mix of positive and negative results. not sure if this has any impact at all.

I am also scratching my head about doing a self test for the magnetometer. using CTRL_REG1_M (20h). There is no information on how to go about this and what to expect, any ideas?

Kind Regards

[ghost]

Just to add print format for about results is ax, ay, az, gx, gy, gz, mx, my, mz, yaw, pitch, roll, q[0], q[1], q[2], q[3]

[kriswiner]

OK, that looks much better.

On Thu, Aug 17, 2017 at 5:09 AM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Just to add print format for about results is ax, ay, az, gx, gy, gz, mx, my, mz, yaw, pitch, roll, q[0], q[1], q[2], q[3]

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-323053589, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qpqrNhdTZA6SiU2ylxTLIG2LhVQYks5sZC2VgaJpZM4O5A5T .

[ghost]

Hello Kris,

I have updated from Arduino Uno to a Teensy 3.6. I am thinking i am doing the right calibration as it is your code i am using.

below are my calibration results. Am i doing something wrong? The yaw is just not very responsive and seems off.

In one on of your responses you mentioned my magnetometer readings are wrong, could you please elaborate on this.?

Also in your code, you do not apply any bias to the reading. is it beacuse you qwrite the biases to the sensors?

Here when you read the magbias is commented out. `if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_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];`

This one is after calibration and placing the sensor down

this is with the sensor at rest after calibratin.

[kriswiner]

How did you verify that the mag is calibrated?

On Wed, Aug 23, 2017 at 9:29 AM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Hello Kris,

I have updated from Arduino Uno to a Teensy 3.6. I am thinking i am doing the right calibration as it is your code i am using.

below are my calibration results. Am i doing something wrong? The yaw is just not very responsive and seems off.

In your on of your responses you mentioned my magnetometer readings are wrong, could you please elaborate on this.?

Also in your code, you do not apply any bias to the reading. is it beacuse you qwrite the biases to the sensors?

Here when you read the magbias is commented out. `if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_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];`

This one is after calibration and placing the sensor down [image: screenshot from 2017-08-23 17-22-27] https://user-images.githubusercontent.com/8300457/29626883-1c523b6c-8828-11e7-9e53-d469a46bfec0.png

this is with the sensor at rest after calibratin. [image: screenshot from 2017-08-23 17-23-37] https://user-images.githubusercontent.com/8300457/29626897-28d4a69a-8828-11e7-8243-09c8b7f4b6ce.png

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-324390231, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qpLdTKZrSZDepuDftkdDAzG-mH6hks5sbFNvgaJpZM4O5A5T .

[ghost]

I haven't used any external user interface. My apologies for my ignorance and assumptions. Do you have any suggestions?

Another questions is you do not do the soft iron calibrations like you do for the 9250. any particular reason why?

[kriswiner]

You need to do something like this:

https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration

Might not be in my sketch since it was posted years ago before I started doing mag calibration in the sketches.

On Wed, Aug 23, 2017 at 9:52 AM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

I haven't used any external user interface. My apologies for my ignorance and assumptions. Do you have any suggestions?

Another questions is you do not do the soft iron calibrations like you do for the 9250. any particular reason why?

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-324396337, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qgozJLCVe7hvYAEUdDKJ7FdQv7Rcks5sbFi_gaJpZM4O5A5T .

[ghost]

Hi Kris, while i was looking for a tool to do verify my calibration i found this tool. I have used this tool to calibrate and i got these values (completely different from what i what getting from the code). https://www.pjrc.com/store/prop_shield.html

My issue is now to follow this example https://github.com/adafruit/Adafruit_AHRS/blob/master/examples/ahrs_fusion_usb/ahrs_fusion_usb.ino

In your code you apply offset to registers OFFSET_X_REG, OFFSET_Y_REG and OFFSET_Z_REG. Can i write the values of this registers to Zero and apply the offsets via software as done in the adafruit example? Can this eliminate the whole magnetometre calibration in your code?

[kriswiner]

Yes and yes.

On Fri, Sep 1, 2017 at 5:48 AM Nyasha Lloyd Japondo < notifications@github.com> wrote:

Hi Kris, while i was looking for a tool to do verify my calibration i found this tool. I have used this tool to calibrate and i got these values (completely different from what i what getting from the code). https://www.pjrc.com/store/prop_shield.html

[image: screenshot from 2017-09-01 13-34-39] https://user-images.githubusercontent.com/8300457/29970177-6f7ab2e8-8f1b-11e7-8f38-7f92ec663ea6.png

My issue is now to follow this example https://github.com/adafruit/Adafruit_AHRS/blob/master/examples/ahrs_fusion_usb/ahrs_fusion_usb.ino

In your code you apply offset to registers OFFSET_X_REG, OFFSET_Y_REG and OFFSET_Z_REG. Can i write the values of this registers to Zero and apply the offsets via software as done in the adafruit example? Can this eliminate the whole magnetometre calibration in your code?

— You are receiving this because you commented.

Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326571446, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qkKxMt7motRmC47-bNq6qXPe7FBDks5sd_0jgaJpZM4O5A5T .

[ghost]

Thanks you very much will let you know how it goes

[ghost]

Hi Kris, So after testing the calibration and replacing the tools i have still not managed to get any sensible results. My yaw is very unstable, slow to respond and not rotating well still. a 90 degree turn for instant can not be produced. I have tried to put the results from the calibration and tried to do the calibration at start as well. Results are not that different.

after twitching with the figures in your code i am getting a rate between 711Hz to 2040Hz. Here is how i am setting some parametres from your code (rather the only lines i have played with changing stuff back and forth).

Please help if you can spot any errors.

If possible i can email the whole code with tiny changes i made but it is mainly based on yours. I am using the Teensy 3.6.

any wise words of advice please?

float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta

uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate uint8_t Gscale = GFS_245DPS; // gyro full scale uint8_t Godr = GODR_238Hz; // gyro data sample rate uint8_t Gbw = GBW_med; // gyro data bandwidth uint8_t Ascale = AFS_2G; // accel full scale uint8_t Aodr = AODR_238Hz; // accel data sample rate uint8_t Abw = ABW_50Hz; // accel data bandwidth uint8_t Mscale = MFS_4G; // mag full scale uint8_t Modr = MODR_20Hz; // mag data sample rate uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode float aRes, gRes, mRes; // scale resolutions per LSB for the sensors

deltat = ((Now - lastUpdate) / (1000000.0f)); // set integration time by time elapsed since last filter update

if (delt_t > 0.25) { // update LCD once per half-second independent of read rate//25ns I have tried both filters.

How can i make the filter react quicker and settle quicker? I have tried to play with beta sticking random values from 0.041 to 0.6.

[kriswiner]

With the Teensy 3.6 you should be getting fusion rates of ~50 kHz. Something is wrong...

On Sun, Sep 3, 2017 at 1:38 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Hi Kris, So after testing the calibration and replacing the tools i have still not managed to get any sensible results. My yaw is very unstable, slow to respond and not rotating well still. a 90 degree turn for instant can not be produced. I have tried to put the results from the calibration and tried to do the calibration at start as well. Results are not that different.

after twitching with the figures in your code i am getting a rate between 711Hz to 2040Hz. Here is how i am setting some parametres from your code (rather the only lines i have played with changing stuff back and forth).

Please help if you can spot any errors.

If possible i can email the whole code with tiny changes i made but it is mainly based on yours. I am using the Teensy 3.6.

any wise words of advice please?

float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta

uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate uint8_t Gscale = GFS_245DPS; // gyro full scale uint8_t Godr = GODR_238Hz; // gyro data sample rate uint8_t Gbw = GBW_med; // gyro data bandwidth uint8_t Ascale = AFS_2G; // accel full scale uint8_t Aodr = AODR_238Hz; // accel data sample rate uint8_t Abw = ABW_50Hz; // accel data bandwidth uint8_t Mscale = MFS_4G; // mag full scale uint8_t Modr = MODR_20Hz; // mag data sample rate uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode float aRes, gRes, mRes; // scale resolutions per LSB for the sensors

deltat = ((Now - lastUpdate) / (1000000.0f)); // set integration time by time elapsed since last filter update

if (delt_t > 0.25) { // update LCD once per half-second independent of read rate//25ns I have tried both filters.

How can i make the filter react quicker and settle quicker? I have tried to play with beta sticking random values from 0.041 to 0.6.

[image: screenshot from 2017-09-03 21-37-32] https://user-images.githubusercontent.com/8300457/30006426-2017627e-90f0-11e7-8abc-58b176e5f204.png

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326829750, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qrqj3cHg12knfFEEH81Z48thiWOIks5sew5PgaJpZM4O5A5T .

[ghost]

can i email you my piece of code so you can try it your end? please.

[kriswiner]

No thanks

On Sun, Sep 3, 2017 at 1:52 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

can i email you my piece of code so you can try it your end? please.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326830491, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qoZSxSRxVL7GCG8253wvC4yuANWiks5sexGbgaJpZM4O5A5T .

[ghost]

Ok, thank you for the help i will keep on trying.

[kriswiner]

What happens if you just run one of my sketches without fiddling with it?

On Sun, Sep 3, 2017 at 1:56 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Ok, thank you for the help i will keep on trying.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326830671, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qlCyKvC6SXTXEHltnw0jpTaEmW-Uks5sexJsgaJpZM4O5A5T .

[ghost]

Thank you for all the help, i appreciate

This is what i get, please see screenshots below.

I have taken the filter code and stuck it right at the bottom. Only addition i have made. I only get 3.6 max from running this sketch. I am using the sparkfun board and it only contains the lsm9ds1 only. I have 3 of these and have swapped them multiple times

[ghost]

I am using this code here https://github.com/kriswiner/LSM9DS1

[kriswiner]

OK, this looks pretty good to me.

Did you calibrate the sensors?

I mean I see that the accel data is not 0, 0, 1 as I would expect for lying flat on a table motionless.

If you turn the sensor first face up and then face down, do you get Mz up = -Mz down?

The fusion rate is rather low, but maybe you are not using the interrupts? On the other hand, it is fast enough you should be getting pretty accurate and stable heading.

The only way I know not to is:

1) not properly calibrated 2) not feeding sensor data into Madgwick algorithm properly.

If

On Sun, Sep 3, 2017 at 2:04 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

Thank you for all the help, i appreciate

This is what i get, please see screenshots below.

I have taken the filter code and stuck it right at the bottom. Only addition i have made. I only get 3.6 max from running this sketch. I am using the sparkfun board and it only contains the lsm9ds1 only. I have 3 of these and have swapped them multiple times

[image: screenshot from 2017-09-03 22-00-28] https://user-images.githubusercontent.com/8300457/30006601-c91716fa-90f3-11e7-8fe6-5e51ab6da0d9.png

[image: screenshot from 2017-09-03 22-01-00] https://user-images.githubusercontent.com/8300457/30006602-cbcc78c2-90f3-11e7-845d-24b6c53e2464.png

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326831125, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qr2PcVvyU5XPgnFpPCtLMhUTpFqWks5sexRbgaJpZM4O5A5T .

[kriswiner]

That code has no magnetometer calibration...

On Sun, Sep 3, 2017 at 2:05 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

I am using this code here https://github.com/kriswiner/LSM9DS1

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326831173, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qqxVtMfeTa2TBqL4GdMFuDZx-T7Nks5sexSNgaJpZM4O5A5T .

[ghost]

I am slightly confused when you say that code has no calibration. At start up it asks you to move the sensor in 8 position which i understand is hard iron calibration. I can not see any soft iron correction though.

do you have another version of this code for this particular device that uses interrupts and the calibration you expect?

No i had a basic I2C setup. No interrupts.

I can see an int intPin = 15; line which i am not sure where this pin connects to.

When i turn my sensor Mz up does not equal - Mz Down. Do i need to calibrate first to achieve this or this should be expected behaviour without calibration?

[kriswiner]

I just noticed that the offset bias correction seems to be commented out for the mag data in that sketch.

I suspect your mag is not calibrated, and without mag calibration your heading will be garbage.

This is why I asked if you had calibrated your sensors and how did you know.

On Sun, Sep 3, 2017 at 2:29 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

I am slightly confused when you say that code has no calibration. At start up it asks you to move the sensor in 8 position which i understand is hard iron calibration. I can not see any soft iron correction though.

do you have another version of this code for this particular device that uses interrupts and the calibration you expect?

No i had a basic I2C setup. No interrupts.

I can see an int intPin = 15; line which i am not sure where this pin connects to.

When i turn my sensor Mz up does not equal - Mz Down. Do i need to calibrate first to achieve this or this should be expected behaviour without calibration?

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326832413, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qm9WbZPW5yvzmCuGQjOVpvUqsfmCks5sexpCgaJpZM4O5A5T .

[ghost]

From lines 897 to 902 in the calibration function for the magnetometre you write the bias to the offset registers. My assumption was this is how you calibrate the magnetometre? Should i uncomment the bias even when you write to the offset registers?

[kriswiner]

You should check the calibration of your sensors.

If you place the sensor flat and motionless you should see:

0 mg, 0 mg, 1000 mg for xyz accel

and

0 dps, 0 dps, 0 dps for xyz gyro.

Do you?

On the mag, plot Mx vs. My, Mx vs. Mz, and My vs Mx while moving the sensor all around.

You should see circular clouds of points centered on the origin.

Do you>

Until you can answer yes to these questions your sensors are not calibrated.

On Sun, Sep 3, 2017 at 2:42 PM, Nyasha Lloyd Japondo < notifications@github.com> wrote:

From lines 897 to 902 in the calibration function for the magnetometre you write the bias to the offset registers. My assumption was this is how you calibrate the magnetometre? Should i uncomment the bias even when you write to the offset registers?

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/8#issuecomment-326833078, or mute the thread https://github.com/notifications/unsubscribe-auth/AGY1qtSX27uKH6yAk-CXYeguuAXwLaPxks5sex1fgaJpZM4O5A5T .

[ghost]

I see that for the acc and the gyro. I will give another go at the mag

[ghost]

In one of my posts above i showed calibration using the tool, perhaps it is important to note that results are similar?

[ghost]

Hi Kris,

So i have reset my offset registers and Mz up = -Mz down. I am applying the offsets in the software rather than writing to the offset registers.

My calibration has been verified

The problem i have now is with the filter part. it goes to the right angle then quickly goes back up to 360. Any idea what may be happening?