/*====================================================================================================*/
void IMU_getDataCorr( IMU_DataTypeDef *pIMU )
{
double tmpData[3] = {0};
IMU_getData(pIMU);
if(pIMU->MPU_GyrAcc_Enable == MPU_GyrAcc_ENABLE) {
pIMU->Gyr[0] = (int16_t)(pIMU->Gyr[0] - pIMU->GyrOffset[0]); // Gyr.X
pIMU->Gyr[1] = (int16_t)(pIMU->Gyr[1] - pIMU->GyrOffset[1]); // Gyr.Y
pIMU->Gyr[2] = (int16_t)(pIMU->Gyr[2] - pIMU->GyrOffset[2]); // Gyr.Z
tmpData[0] = pIMU->Acc[0] - pIMU->AccOffset[0]; // Acc.X
tmpData[1] = pIMU->Acc[1] - pIMU->AccOffset[1]; // Acc.Y
tmpData[2] = pIMU->Acc[2] - pIMU->AccOffset[2]; // Acc.Z
pIMU->Acc[0] = pIMU->AccGain[0] * tmpData[0] + pIMU->AccGain[1] * tmpData[1] + pIMU->AccGain[2] * tmpData[2];
pIMU->Acc[1] = pIMU->AccGain[1] * tmpData[0] + pIMU->AccGain[3] * tmpData[1] + pIMU->AccGain[4] * tmpData[2];
pIMU->Acc[2] = pIMU->AccGain[2] * tmpData[0] + pIMU->AccGain[4] * tmpData[1] + pIMU->AccGain[5] * tmpData[2];
}
if(pIMU->MPU_Mag_Enable == MPU_Mag_ENABLE) {
tmpData[0] = pIMU->Mag[0] - pIMU->MagOffset[0]; // Mag.X
tmpData[1] = pIMU->Mag[1] - pIMU->MagOffset[1]; // Mag.Y
tmpData[2] = pIMU->Mag[2] - pIMU->MagOffset[2]; // Mag.Z
pIMU->Acc[0] = pIMU->AccGain[0] * tmpData[0] + pIMU->AccGain[1] * tmpData[1] + pIMU->AccGain[2] * tmpData[2];
pIMU->Acc[1] = pIMU->AccGain[1] * tmpData[0] + pIMU->AccGain[3] * tmpData[1] + pIMU->AccGain[4] * tmpData[2];
pIMU->Acc[2] = pIMU->AccGain[2] * tmpData[0] + pIMU->AccGain[4] * tmpData[1] + pIMU->AccGain[5] * tmpData[2];
}
if(pIMU->LPS_PresTemp_Enable == LPS_PresTemp_ENABLE) {
pIMU->Temp -= pIMU->TempOffset; // Temp
pIMU->Pres -= pIMU->PresOffset; // Pres
}
}
int main( void )
{
int16_t dataIMU[10] = {0};
System_Init();
while(1) {
LED_B_Toggle;
Delay_100ms(1);
IMU_getData(dataIMU);
printf("AccX:%d\tAccY:%d\tAccZ:%d\tGyrX:%d\tGyrY:%d\tGyrZ:%d\tMagX:%d\tMagY:%d\tMagZ:%d\t\r\n", dataIMU[1], dataIMU[2], dataIMU[3], dataIMU[4], dataIMU[5], dataIMU[6], dataIMU[7], dataIMU[8], dataIMU[9]);
}
}
