int magnet_yaw(void *magnet_object,float *mag_yaw){
int a;
float magnet_data[3];
a=magnet_xyz(magnet_object,magnet_data);
if(a<0){
usart_printf(USARTx,"Issue with reading the magnet sensor value\n\r");
return -1;
}
float heading = atan2f(magnet_data[1], magnet_data[0]);
float declinationAngle = -0.4605;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
float headingDegrees = heading * 180/PI;
usart_printf(USARTx,"magnet_data_x=%f magnet_data_y=%f data_z=%f degress=%f\n\r",magnet_data[0],magnet_data[1],magnet_data[2],headingDegrees);
return 0;
}
int I2C_read(I2C_TypeDef* I2Cx,byte buff ){
// enable acknowledge of recieved data
int timeout;
uint8_t flag1=0,flag2=0;
/* Test on I2C1 EV8 and clear it */
timeout = I2C_TIMEOUT_MAX; /* Initialize timeout value */
I2C_AcknowledgeConfig(I2Cx, ENABLE);
//
// wait until one byte has been received
while( !I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED) ){
ms_delay(100);
if((timeout--)==0){
flag1 = I2Cx->SR1;
flag2 = I2Cx->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Failing at read ack stage\n\r");
return -1;
}
}
// read data from I2C data register and return data byte
byte data = I2C_ReceiveData(I2Cx);
return 0;
}
void debugDisplayArray(FILE *stream,
const char_t *prepend, const void *data, uint_t length)
{
uint_t i;
for(i = 0; i < length; i++)
{
//Beginning of a new line?
if((i % 16) == 0)
usart_puts(prepend);
//Display current data byte
usart_printf("%02X ", *((unsigned char *) data + i));
//End of current line?
if((i % 16) == 15 || i == (length - 1))
usart_printf("\r\n");
}
}
int i2c_beginTransmission(I2C_TypeDef* I2Cx,uint8_t device_address,uint8_t operation){
int timeout;
uint32_t flag1=0,flag2=0;
device_address= device_address << 1;
I2C_GenerateSTART(I2Cx, ENABLE);
//flag1 = I2Cx->SR1;
//flag2 = I2Cx->SR2;
//usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
//usart_printf(USARTx,"Sending start bit to address:%02x\n\r",device_address);
/* Test on I2Cx EV5, Start trnsmitted successfully and clear it */
timeout = I2C_TIMEOUT_MAX; /* Initialize timeout value */
while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
{
/* If the timeout delay is exeeded, exit with error code */
ms_delay(100);
if ((timeout--) == 0){
flag1 = I2Cx->SR1;
flag2 = I2Cx->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Timeout reached while checking the EV5 event\n\r");
return -1;
}}
timeout = I2C_TIMEOUT_MAX;
if (!operation){
I2C_Send7bitAddress(I2Cx, device_address, I2C_Direction_Transmitter);
//timeout = I2C_TIMEOUT_MAX;
while (!I2C_CheckEvent(I2Cx,I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
{
/* If the timeout delay is exeeded, exit with error code */
ms_delay(100);
if ((timeout--) == 0){
flag1 = I2Cx->SR1;
flag2 = I2Cx->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Timeout reached while verifying transmitter selection\n\r");
return -2;
}}
//usart_printf(USARTx,"Transmitter mode selected. Send the address\n\r");
}
else{
I2C_Send7bitAddress(I2Cx, device_address, I2C_Direction_Receiver);
while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_RECEIVED))
{
/* If the timeout delay is exeeded, exit with error code*/
ms_delay(100);
if ((timeout--) == 0){
flag1 = I2Cx->SR1;
flag2 = I2Cx->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Timeout reached while recieving acknowledge on revice mode \n\r");
return -2;
}
}}
//usart_printf(USARTx,"Success with stage one. Going to senddata\n\r");
return 0;
}
int i2c_sendData(I2C_TypeDef* I2Cx,int data){
I2C_SendData(I2Cx, data);
//usart_printf(USARTx,"Sending data:%d\n\r",data);
int timeout;
uint8_t flag1=0,flag2=0;
/* Test on I2C1 EV8 and clear it */
timeout = I2C_TIMEOUT_MAX; /* Initialize timeout value */
while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTING))
{
/* If the timeout delay is exeeded, exit with error code */
if ((timeout--) == 0) {
flag1 = I2Cx->SR1;
flag2 = I2Cx->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Failing at sending data");
return -3;
}
}
return 0;
}
int kalman_roll_pitch(float *final_roll,float *final_pitch){
int a=0;
float accel_roll,accel_pitch,gyro_roll_rate,gyro_pitch_rate;
a=accel_rollpitch(accel_object,&accel_roll,&accel_pitch);
if(a<0){
usart_printf(USARTx,"Issue with reading the accel sensor value in kalman\n\r");
return -1;
}
if(accel_roll > 180 || accel_roll < -180 || accel_pitch > 180 || accel_pitch < -180 ){
usart_printf(USARTx,"Accel's roll and pitch is not proper.\n\r");
return -1;
}
//usart_printf(USARTx,"a.roll=%f a.pitch=%f",accel_roll,accel_pitch);
unsigned int time=((*DWT_CYCCNT)-start)/10000000;
//usart_printf(USARTx,"DWT_CYCCNT=%lu\n\r",*DWT_CYCCNT);
ResetTiming();
start=*DWT_CYCCNT;
a=gyro_rollpitch(gyro_object,&gyro_pitch_rate,&gyro_roll_rate);
if(a<0){
usart_printf(USARTx,"Issue with reading the gyro sensor value in kalman\n\r");
return -1;
}
if(gyro_roll_rate > 2000 || gyro_roll_rate < -2000 || gyro_pitch_rate > 2000 || gyro_pitch_rate < -2000 ){
usart_printf(USARTx,"Gyro's roll and pitch is not proper.\n\r");
return -1;
}
time=1;
gyro_roll_rate=-gyro_roll_rate;
gyro_roll+=gyro_roll_rate*0.01*90/14.0;
gyro_pitch+=gyro_pitch_rate*0.01*90/14.0;
//usart_printf(USARTx,"g.roll=%f g.pitch=%f\n\r",gyro_roll,gyro_pitch);
*final_roll=kalman_getAngle(kalman_roll,accel_roll, gyro_roll_rate,time);
*final_pitch=kalman_getAngle(kalman_pitch,accel_pitch,gyro_pitch_rate,time);
return 0;
}
int accel_rollpitch(void *accel_object,float *accel_roll,float *accel_pitch){
int a; float accel_data[3];
float value_x,value_y,value_z;
a=accel_xyz(accel_object,accel_data);
if(a<0){
usart_printf(USARTx,"Issue with reading the accel sensor value\n\r");
return -1;
}
//usart_printf(USARTx,"Values of accel :\n\rRawAccel_x=%f RawAccel_y=%f RawAccel_z=%f\n\r",accel_data[0],accel_data[1],accel_data[2]);
//usart_printf(USARTx,"Values of accel :\n\rAccel_x=%f\n\rAccel_y=%f\n\rAccel_z=%f\n\r",value_x,value_y,value_z);
*accel_pitch=atan2f(accel_data[1],accel_data[2])*180/PI;
*accel_roll=atan2f(accel_data[0],accel_data[2])*180/PI;
return 0;
}
void IMU_Initialisation(){
uint8_t counter=3;
i2c_initialize(I2C2);
usart_printf(USART1,"Hello\n\r");
uint8_t accel_address=0x53;
uint8_t magnet_address=0x1E;
uint8_t gyro_address=ITG3200_ADDR_AD0_LOW;
counter=3;
while(counter!=0){
accel_object=accelerometer_initialization(accel_address);
if(accel_object==null){
ms_delay(1000);
counter--;
}else
break;
}
if(counter==0){
usart_printf(USARTx,"Failed to initialize accel.Exiting...\n\r");
while(1);
}
usart_printf(USARTx,"Initialized accelerometer\n\r");
counter=3;
while(counter!=0){
magnet_object=magnetometer_initialisation(magnet_address);
if(magnet_object==null){
ms_delay(1000);
counter--;
}else
break;
}
if(counter==0){
usart_printf(USARTx,"Failed to initialize magnetometer.Exiting...\n\r");
while(1);
}
usart_printf(USARTx,"Initialized magnetometer\n\r");
counter=3;
while(counter!=0){
gyro_object=gyro_initialisation(gyro_address);
if(gyro_object==null){
gyro_object=gyro_initialisation(gyro_address);
ms_delay(1000);
counter--;
}else
break;
}
if(counter==0){
usart_printf(USARTx,"Failed to initialize gyro.Exiting...\n\r");
while(1);
}
usart_printf(USARTx," initialize gyro\n\r");
}
static void on_line_recv(const char* c, unsigned int length)
{
if (c[0] == 'm') {
charge_start(MANUAL);
usart_print("manual\n");
} else if (c[0] == 't') {
charge_start(TRICKLE);
usart_print("trickle\n");
} else if (c[0] == 'c') {
charge_start(CHARGE);
usart_print("charge\n");
} else if (c[0] == '=') {
const char* end = c+length+1;
long offset = strtol(&c[1], (char** restrict) &end, 10);
set_charge_offset(offset);
usart_printf("set offset %d\n", offset);
} else if (c[0] == 'v') {
int gyro_rollpitch(void *gyro_object,float *gyro_roll_rate,float *gyro_pitch_rate){
int a;
float gyro_data[3];
int gyro_data_raw[3];
//unsigned int time=((*DWT_CYCCNT)-start)/10000000;
//usart_printf(USARTx,"Time =%lu\n\r",time);
//GPIOD->ODR ^= (1 << 13);
//usart_printf(USARTx,"DWT_CYCCNT=%lu\n\r",*DWT_CYCCNT);
float value_x,value_y,value_z;
a=gyro_xyz(gyro_object,gyro_data);
if(a<0){
usart_printf(USARTx,"Issue with reading the gyro sensor value\n\r");
return -1;
}
*gyro_roll_rate=gyro_data[0];
*gyro_pitch_rate=gyro_data[1];
// *gyro_pitch+=gyro_data[1]*time;
// *gyro_roll+=gyro_data[0]*time;
// ResetTiming();
// start=*DWT_CYCCNT;
// usart_printf(USARTx,"Values of gyro :\n\rRawGyro_x=%f\n\rRawgyro_y=%f\n\rRawgyro_z=%f\n\r",gyro_data[0],gyro_data[1],gyro_data[2]);
// usart_printf(USARTx,"RawGyro_x=%d Rawgyro_y=%d Rawgyro_z=%d\n\r",gyro_data_raw[0],gyro_data_raw[1],gyro_data_raw[2]);
return 0;
}
void usart_printfm(USART_TypeDef* USARTx,const int *format,...){
usart_printf(USARTx,(const char*)format);
}
/* Send the final string to the output device. Change this function to
* suit whatever output device you'd like to use.
*/
void logger_output( char *logmsg ) {
usart_printf("%s",logmsg);
}
int readFrom(byte _dev_address,byte address, int num, byte _buff[]) {
int a=0,i=0;
int timeout;
uint8_t flag1=0,flag2=0;
a=i2c_beginTransmission(I2Ci,_dev_address,i2c_write); // start transmission to device
if(a<0) return -1;
// usart_printfm(USART1,(const int *)"Reading from this device address: %2x\n\r",_dev_address);
a=i2c_sendData(I2Ci,address); // sends address to read from
if(a<0) return -1;
//usart_printfm(USART1,(const int *)"sent this data which is the address to read from: %2x\n\r",address);
// if(a<0)
// return (a-1);
//i2c_stopTransmission(I2Cx); // end transmission
// if(a<0)
// return (a-2);
a=i2c_beginTransmission(I2Ci,_dev_address,i2c_read); // start transmission to device
if(a<0) return -1;
//usart_printfm(USART1,(const int *)"Initiated the register read cycle: %2x\n\r",_dev_address);
// if(a<0)
// return a;
// request 6 bytes from device
if(num==1){
I2C_AcknowledgeConfig(I2Ci, DISABLE);
/* Test on I2C1 EV8 and clear it */
timeout = I2C_TIMEOUT_MAX; /* Initialize timeout value */
// wait until one byte has been received
while( !I2C_CheckEvent(I2Ci, I2C_EVENT_MASTER_BYTE_RECEIVED) ){
ms_delay(100);
if((timeout--)==0){
flag1 = I2Ci->SR1;
flag2 = I2Ci->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Failing at read nnack stage\n\r");
return -1;
}
}
// wait until one byte has been received
I2C_GenerateSTOP(I2Ci, ENABLE);
// read data from I2C data register and return data byte
_buff[i] = I2C_ReceiveData(I2Ci);
return 0;
}
if(num==2)
{
timeout = I2C_TIMEOUT_MAX;
// while(I2C_GetFlagStatus(I2Ci,I2C_FLAG_ADDR)!=SET){
// ms_delay(100);
// if((timeout--)==0){
// flag1 = I2Ci->SR1;
// flag2 = I2Ci->SR2;
// usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
// usart_printf(USARTx,"Failing at reading 2 bytes stage\n\r");
// return ;
// }}
I2C_AcknowledgeConfig(I2Ci, DISABLE);
I2C_NACKPositionConfig(I2Ci, I2C_NACKPosition_Next);
timeout = I2C_TIMEOUT_MAX;
while(I2C_GetFlagStatus(I2Ci,I2C_FLAG_BTF)!=SET){
ms_delay(100);
if((timeout--)==0){
flag1 = I2Ci->SR1;
flag2 = I2Ci->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Failing at reading 2 bytes stage2\n\r");
return -1;
}}
i2c_stopTransmission(I2Ci);
_buff[i] = I2C_ReceiveData(I2Ci);
i++;
_buff[i] = I2C_ReceiveData(I2Ci);
num-=2;
// receive a byte
//usart_printfm(USART1,(const int *)"this byte read from accel: %2x\n\r",i);
return 0;
}
while(num>0) // device may send less than requested (abnormal)
if(num==3){
//_buff[i]=I2C_read_ack(I2Cx);
timeout = I2C_TIMEOUT_MAX;
while(I2C_GetFlagStatus(I2Ci,I2C_FLAG_BTF)!=SET){
ms_delay(100);
if((timeout--)==0){
flag1 = I2Ci->SR1;
flag2 = I2Ci->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Failing at reading 2 bytes stage2\n\r");
return -1;
}}
I2C_AcknowledgeConfig(I2Ci, DISABLE);
_buff[i]=I2C_ReceiveData(I2Ci);
i++;
num--;
while(I2C_GetFlagStatus(I2Ci,I2C_FLAG_BTF)!=SET)
{
ms_delay(100);
if((timeout--)==0){
flag1 = I2Ci->SR1;
flag2 = I2Ci->SR2;
usart_printf(USARTx,"Flag1:%04x \n\r Flag2:%04x\n\r",flag1,flag2);
usart_printf(USARTx,"Failing at reading 2 bytes stage2\n\r");
return -1;
}}
i2c_stopTransmission(I2Ci);
_buff[i]=I2C_ReceiveData(I2Ci);
i++;
_buff[i]=I2C_ReceiveData(I2Ci);
i++;
num-=2;
//usart_print(USART1,"This byte read from nack: %2x\n\r",i);
return 0;
}
else{
a=I2C_read(I2Ci,_buff[i]);
if(a<0) return -1;
i++;
num--;
}
// if(i != num){
// status = ADXL345_ERROR;
// error_code = ADXL345_READ_ERROR;
// }
//ms_delay(10);
//i2c_stopTransmission(I2Cx); // end transmission
//end :
// usart_printf(USARTx,"Error occured !! Restarting the interface!!");
// ms_delay(5000);
// return -1;
return 0;
}
