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