I'm trying to use the C_Math library to calculate angles from accelerometer readings.
I'm using a PIC16F877A. At present i'm trying to debug the maths calculations so I am not using the readings from the acceleromter
instead I'm directly assigning values to the raw X, Y & Z accelerometer variables.
I can seem to get any math functions working. the equation i'm trying to compute is;
X_ANGLE = atan(Y_RAW / sqrt(X_RAW^2 + Z_RAW^2))
but for now reduced to denominator_X_cal = sqrt(X_RAW^2 + Z_RAW^2))
the code has a known working debug function which converts this result into an ascii format and outputs on UART
the function code is below
Code: Select all
void read_accerometers (void)
{
unsigned int Accel_X_raw = 0;
unsigned int Accel_Y_raw = 0;
unsigned int Accel_Z_raw = 0;
double X_Squared = 0;
double Y_Squared = 0;
double Z_Squared = 0;
double denominator_X_cal = 0;
double a = 0;
//ByteReadI2C(GYRO_DEVICE_ID, ADDR_ACCEL_XOUT_H, &AccXDataH, 1);
//ByteReadI2C(GYRO_DEVICE_ID, ADDR_ACCEL_XOUT_L, &AccXDataL, 1);
//ByteReadI2C(GYRO_DEVICE_ID, ADDR_ACCEL_YOUT_H, &AccYDataH, 1);
//ByteReadI2C(GYRO_DEVICE_ID, ADDR_ACCEL_YOUT_L, &AccYDataL, 1);
//ByteReadI2C(GYRO_DEVICE_ID, ADDR_ACCEL_ZOUT_H, &AccZDataH, 1);
//ByteReadI2C(GYRO_DEVICE_ID, ADDR_ACCEL_ZOUT_L, &AccZDataL, 1);
//Accel_X_raw = ((AccXDataH<<8)|AccXDataL);
//Accel_Y_raw = ((AccYDataH<<8)|AccYDataL);
//Accel_Z_raw = ((AccZDataH<<8)|AccZDataL);
//debug set direct variables so not reading acclerometers, therefore
//math calculations can be proven.
Accel_X_raw = 50;
Accel_Y_raw = 20;
Accel_Z_raw = 80;
X_Squared = ((Accel_X_raw)*(Accel_X_raw));
Y_Squared = ((Accel_Y_raw)*(Accel_Y_raw));
Z_Squared = ((Accel_Z_raw)*(Accel_Z_raw));
a = ((Z_Squared)+(X_Squared));
//below line doesn't execute/ get stuck
//if removed code runs normally
denominator_X_cal = sqrt(a);
RB2_BIT = 1; //debug turns on LED if executed
UART_VALUE_TO_ASCII_AND_OUTPUT (denominator_X_cal); // function converts value into ascii characters and tx on UART
TX_DATA(0x0D); //line feed and carriage return
TX_DATA(0x0A);
}
the code gets stuck at the below code line, if it is removed the code will run continually as expected
Code: Select all
denominator_X_cal = sqrt(a);
what am I missing?
Any help on this will be very appreciated as I don't think I'll be able to write my own Sqrt and atan functions to a good enough accuracy.
Thanks in advance