LSM6DS3 with madgewick flter to find tilt angle, problems with accelerometer output - arduino

Question

I am trying to find the tilt angle of an object using LSM6DS3 in arduino mega board, using Madgwick filter algorithm. But the accelero meter values drops suddenly to zero readings, so that the filter gives error output. Attaching my codes with this post.
/* Sensor SDI/SDA - Arduino Digital Pin 51 or MOSI (Master Out Slave In)
* Sensor SCK/SCL - Arduino Digital Pin 52 or SCK (Serial Clock)
* Sensor SDO/ADR - Arduino Digital Pin 50 or MISO (Master In Slave Out)
* Sensor CS - Arduino Digital Pin 10 or 53 (Slave Select or Chip Select)
*/
#include <SPI.h> // Include Arduino SPI library
#include <math.h>
#define OUT_X_L_G 0x22 // Register addresses from sensor datasheet.
#define OUT_X_H_G 0x23 // Only the registers that are used
#define OUT_Y_L_G 0x24 // in this program are defined here.
#define OUT_Y_H_G 0x25
#define OUT_Z_L_G 0x26
#define OUT_Z_H_G 0x27
#define CTRL1_A 0x10
#define CTRL2_G 0x11
#define CTRL3_C 0x12
#define CTRL4_C 0x13
#define CTRL9_A 0x18
#define CTRL10_C 0x19
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define WHO_AM_I 0x0F
#define CTRL7_G 0x16 // setting hpf for gyro
#define STATUS_REG 0x1E
#define sampleFreq 512.0f // sample frequency in Hz 512
#define betaDef 0.1f // 2 * proportional gain 0.1
#define M_PI 3.14
int8_t readData = 0x80;
int8_t writeData = 0x00;
int8_t k ;
int16_t Ax, Ay, Az; // 16-bit variables to hold raw data from sensor
int16_t Gx, Gy, Gz;
float Aax, Aay, Aaz;
float Ggx, Ggy, Ggz;
const int CS = 53; // Chip Select pin for SPI
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
volatile float beta = betaDef;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float invSqrt(float x);
float roll,pitch;
void setup()
{
Serial.begin(57600);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV4);
SPI.setBitOrder(MSBFIRST);
SPI.setDataMode(SPI_MODE3);
pinMode(CS, OUTPUT);
writeReg(CTRL3_C, 0x40); // BDU UPDATE
delay(100);
writeReg(CTRL9_A, 0x38); // Enable x,y,z axes of ACCEL
writeReg(CTRL10_C, 0x38); //Enable x,y,z of GYRO
writeReg(CTRL1_A, 0x7C); // ODR=833, scale = 8g
writeReg(CTRL2_G, 0x7C); // scale= =2000dps, ODR = 833
writeReg(CTRL4_C, 0x04); // SPI only, I2C disabled
writeReg(CTRL7_G, 0x70); // setting high pass filter for gyro
delay(100);
}
void loop()
{
Gx = (int16_t) readReg(OUT_X_H_G) <<8 | readReg(OUT_X_L_G); // typecast as
Gy = (int16_t) readReg(OUT_Y_H_G) <<8 | readReg(OUT_Y_L_G); // 16-bit
Gz = (int16_t) readReg(OUT_Z_H_G) <<8 | readReg(OUT_Z_L_G);
Ax = (int16_t) readReg(OUT_X_H_A) <<8 | readReg(OUT_X_L_A);
Ay = (int16_t) readReg(OUT_Y_H_A) <<8 | readReg(OUT_Y_L_A);
Az = (int16_t) readReg(OUT_Z_H_A) <<8 | readReg(OUT_Z_L_A);
Aax = (Ax*0.244)/1000;
Aay = (Ay*0.244)/1000;
Aaz = (Az*0.244)/1000;
Ggx = (Gx*0.07)*(M_PI/180);
Ggy = (Gy*0.07)*(M_PI/180);
Ggz = (Gz*0.07)*(M_PI/180);
MadgwickAHRSupdateIMU( Ggx, Ggy, Ggz, Aax,Aay,Aaz);
roll=atan2(2*q2*q3-2*q0*q1,(2*q0*q0)+(2*q3*q3)-1)*180/M_PI;
pitch=-1*asin(2*q1*q3+2*q0*q2)*180/M_PI;
Serial.println(roll);
Serial.println(pitch);
}
int8_t readReg(int8_t address) {
int8_t buffer = 0;
digitalWrite(CS, LOW);
SPI.transfer(readData | address);
buffer = SPI.transfer(writeData);
digitalWrite(CS, HIGH);
return(buffer);
}
void writeReg(int8_t address, int8_t val)
{
digitalWrite(CS, LOW);
SPI.transfer(writeData | address);
SPI.transfer(val);
digitalWrite(CS, HIGH);
}
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, 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;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
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;
}
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;
}