/* Sensors Init */
void SensorInitACC()
{
float Cal[ACC_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.ACC_Done)
{
#ifdef MPU6050
SensorInitState.ACC_Done = MPU6050_initialize();
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
}
if(SensorInitState.ACC_Done)
{
printf("ACC connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_ACC, Cal);
if(FlashValid)
{
CalFlashState.ACC_FLASH = true;
AccOffset[0] = Cal[0];
AccOffset[1] = Cal[1];
AccOffset[2] = Cal[2];
AccScale[0] = Cal[3];
AccScale[1] = Cal[4];
AccScale[2] = Cal[5];
printf("ACC calibration - [FLASH]\n");
}
else
{
AccOffset[0] = ACC_OFFSET_X;//0;
AccOffset[1] = ACC_OFFSET_Y;//0;
AccOffset[2] = ACC_OFFSET_Z;//0;
AccScale[0] = ACC_SCALE_X;//IMU_G_PER_LSB_CFG;
AccScale[1] = ACC_SCALE_Y;//IMU_G_PER_LSB_CFG;
AccScale[2] = ACC_SCALE_Z;//IMU_G_PER_LSB_CFG;
printf("ACC calibration - [DEF]\n");
}
//printf("Offset: %f %f %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
//printf("Scale: %f %f %f\n", AccScale[0], AccScale[1], AccScale[2]);
nvtSetAccScale(AccScale);
nvtSetAccOffset(AccOffset);
nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
}
else
printf("ACC connect - [FAIL]\n");
}
/* Sensors Init */
void SensorInitACC()
{
float Cal[ACC_CAL_DATA_SIZE];
bool FlashValid;
#if defined(LSM6DS3)
status_t status;
#endif
if(!SensorInitState.ACC_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.ACC_Done = MPU6050_initialize();
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#else
LSM6DS3_init();
status = begin();
if(status==0)
SensorInitState.ACC_Done = true;
else
SensorInitState.ACC_Done = false;
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
}
if(SensorInitState.ACC_Done) {
printf("ACC connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_ACC, Cal);
if(FlashValid) {
CalFlashState.ACC_FLASH = true;
AccOffset[0] = Cal[0];
AccOffset[1] = Cal[1];
AccOffset[2] = Cal[2];
AccScale[0] = Cal[3];
AccScale[1] = Cal[4];
AccScale[2] = Cal[5];
AccRotate[0] = Cal[6];
AccRotate[1] = Cal[7];
AccRotate[2] = Cal[9];
AccRotate[3] = Cal[9];
AccRotate[4] = Cal[10];
AccRotate[5] = Cal[11];
AccRotate[6] = Cal[12];
AccRotate[7] = Cal[13];
AccRotate[8] = Cal[14];
printf("ACC calibration from - [FLASH]\n");
}
else {
AccOffset[0] = 0;
AccOffset[1] = 0;
AccOffset[2] = 0;
AccScale[0] = IMU_G_PER_LSB_CFG;
AccScale[1] = IMU_G_PER_LSB_CFG;
AccScale[2] = IMU_G_PER_LSB_CFG;
AccRotate[0] = 1;
AccRotate[1] = 0;
AccRotate[2] = 0;
AccRotate[3] = 0;
AccRotate[4] = 1;
AccRotate[5] = 0;
AccRotate[6] = 0;
AccRotate[7] = 0;
AccRotate[8] = 1;
printf("ACC calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
printf("Scale: %f %f %f\n", AccScale[0], AccScale[1], AccScale[2]);
printf("M[0][1][2]: %f %f %f\n", AccRotate[0], AccRotate[1], AccRotate[2]);
printf("M[3][4][5]: %f %f %f\n", AccRotate[3], AccRotate[4], AccRotate[5]);
printf("M[6][7][8]: %f %f %f\n", AccRotate[6], AccRotate[7], AccRotate[8]);
nvtSetAccScale(AccScale);
nvtSetAccOffset(AccOffset);
nvtSetAccRotate(AccRotate);
#if defined(MPU6050) || defined(MPU6500)
nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
#else
nvtSetAccG_PER_LSB(calcAccel(1)/*IMU_G_PER_LSB_CFG*/);
#endif
}
else {
__disable_irq();
SYS_UnlockReg();
SYS_ResetChip();
printf("ACC connect - [FAIL]\n");
}
}