how to solve for nan output?

[Ahmitttttttt]

Hi i am working with the altIMU board to display attitude and heading information similar to that of an attitude indicator. I merged the 3 sensor program codes which i obtained from a forum to display the data on the arduino serial monitor. The program which i am using to display the pitch and roll (attitude) data starts to display nan after a short while when the program is run and i have no idea why. Any help would be great guys. Ill attach the links to where i code the various codes as well as codes.

https://github.com/pololu/lps-arduino
https://github.com/pololu/lsm303-arduino#serial
https://github.com/pololu/minimu-9-ahrs-arduino

#include <LPS.h>
#include <LSM303.h>
#include <Wire.h>

LSM303 compass;


void setup(){
setupmagnetometer();
setupattitude();
setupaltitude();
}

void loop(){
loopmagnetometer();
loopattitude();
loopaltitude();
}

void setupmagnetometer(){

Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();

compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
}

void loopmagnetometer(){

compass.read();

To use a different vector as a reference, use the version of heading()
that takes a vector argument; for example, use

compass.heading((LSM303::vector<int>){0, 0, 1});

to use the +Z axis as a reference.
*/
float heading = compass.heading();


Serial.print(heading);
Serial.println(" deg");

delay(1000);
}

LPS ps;

void setupaltitude()
{
Serial.begin(9600);
Wire.begin();

if (!ps.init())
{
Serial.println("Failed to autodetect pressure sensor!");
while (1);
}

ps.enableDefault();
}
void loopaltitude()
{
float pressure = ps.readPressureMillibars();
float altitude = ps.pressureToAltitudeMeters(pressure);
float temperature = ps.readTemperatureC();

Serial.print("Pressure: ");
Serial.print(pressure);
Serial.print(" mbar\tAltitude: ");
Serial.print(altitude);
Serial.print(" m\tTemperature: ");
Serial.print(temperature);
Serial.println(" deg C");

delay(1000);
}
// Uncomment the below line to use this axis definition:
// X axis pointing forward
// Y axis pointing to the right
// and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise
int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z -gyro,accelerometer, magnetometer
// Uncomment the below line to use this axis definition:
// X axis pointing forward
// Y axis pointing to the left
// and Z axis pointing up.
// Positive pitch : nose down
// Positive roll : right wing down
// Positive yaw : counterclockwise
//int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer

// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168

// LSM303 accelerometer: 8 g sensitivity
// 3.9 mg/digit; 1 g = 256
#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer

#define ToRad(x) ((x)*0.01745329252) // *pi/180
#define ToDeg(x) ((x)*57.2957795131) // *180/pi

// L3G4200D gyro: 2000 dps full scale
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second

// LSM303 magnetometer calibration constants; use the Calibrate example from
// the Pololu LSM303 library to find the right values for your board
#define M_X_MIN -421
#define M_Y_MIN -639
#define M_Z_MIN -238
#define M_X_MAX 424
#define M_Y_MAX 295
#define M_Z_MAX 472

#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002

/*For debugging purposes*/
//OUTPUTMODE=1 will print the corrected data,
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1

//#define PRINT_DCM 0 //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw

#define STATUS_LED 13

float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible

long timer=0; //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
int AN[6]; //array that stores the gyro and accelerometer data
int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors

int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_magnetom_z;
float MAG_Heading;

float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};

// Euler angles
float roll;
float pitch;
float yaw;

float errorRollPitch[3]= {0,0,0};
float errorYaw[3]= {0,0,0};

unsigned int counter=0;
byte gyro_sat=0;

float DCM_Matrix[3][3]= {
{
1,0,0 }
,{
0,1,0 }
,{
0,0,1 }
};
float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here


float Temporary_Matrix[3][3]={
{
0,0,0 }
,{
0,0,0 }
,{
0,0,0 }
};

void setupattitude()
{
Serial.begin(115200);
pinMode (STATUS_LED,OUTPUT); // Status LED

I2C_Init();


digitalWrite(STATUS_LED,LOW);
delay(1500);

Accel_Init();
Compass_Init();
Gyro_Init();

delay(20);

for(int i=0;i<32;i++) // We take some readings...
{
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}

for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;

AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];

//Serial.println("Offset:");
for(int y=0; y<6; y++)
Serial.println(AN_OFFSET[y]);

delay(2000);
digitalWrite(STATUS_LED,HIGH);

timer=millis();
delay(20);
counter=0;
}

void loopattitude()
{
if((millis()-timer)>=20) // Main loop runs at 50Hz
{
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0;

// *** DCM algorithm
// Data adquisition
Read_Gyro(); // This read gyro data
Read_Accel(); // Read I2C accelerometer

if (counter > 5) // Read compass data at 10Hz... (5 loop runs)
{
counter=0;
Read_Compass(); // Read I2C magnetometer
Compass_Heading(); // Calculate magnetic heading
}

// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***

Serial.println(roll);
Serial.println(pitch);
}

}