void SensorInitMAG()
{
float Cal[MAG_CAL_DATA_SIZE + QUALITY_FACTOR_SIZE],magCal[3];
bool FlashValid;
int i;
if(!SensorInitState.MAG_Done)
{
#ifdef HMC5883
SensorInitState.MAG_Done = hmc5883lInit();
hmc5883lSelfTest();
hmc5883lGetRatioFactor(&magCal[0],&magCal[1],&magCal[2]);
#endif
#ifdef AK8975
SensorInitState.MAG_Done = AK8975_initialize();
#endif
}
if(SensorInitState.MAG_Done)
{
if (report_format == REPORT_FORMAT_TEXT)
printf("MAG connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_MAG, Cal);
if(FlashValid)
{
CalFlashState.MAG_FLASH = true;
for(i=0; i<MAG_CAL_DATA_SIZE; i++)
MagCalMatrix[i] = Cal[i];
CalFlashState.MAG_QFACTOR = Cal[i];
if (report_format == REPORT_FORMAT_TEXT)
printf("MAG calibration from - [FLASH], Q:%d\n",CalFlashState.MAG_QFACTOR);
}
else
{
for(i=0; i<MAG_CAL_DATA_SIZE; i++)
MagCalMatrix[i] = 0;
MagCalMatrix[3] = magCal[0];//MAG_GAUSS_PER_LSB;
MagCalMatrix[4] = magCal[1];//MAG_GAUSS_PER_LSB;
MagCalMatrix[5] = magCal[2];//MAG_GAUSS_PER_LSB;
if (report_format == REPORT_FORMAT_TEXT)
printf("MAG calibration from - [DEFAULT], Q:%d\n",CalFlashState.MAG_QFACTOR);
}
/*printf("M[0][1][2]: %f %f %f\n", MagCalMatrix[0], MagCalMatrix[1], MagCalMatrix[2]);
printf("M[3][4][5]: %f %f %f\n", MagCalMatrix[3], MagCalMatrix[4], MagCalMatrix[5]);
printf("M[6][7][8]: %f %f %f\n", MagCalMatrix[6], MagCalMatrix[7], MagCalMatrix[8]);
printf("M[9]: %f\n", MagCalMatrix[9]);*/
nvtSetMagCalMatrix(MagCalMatrix);
nvtSetMagGaussPLSB(MAG_GAUSS_PER_LSB);
}
else
printf("MAG connect - [FAIL]\n");
}
void SensorInitGYRO()
{
float Cal[GYRO_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.GYRO_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.GYRO_Done = MPU6050_initialize();
SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#else
#endif
}
if(SensorInitState.GYRO_Done) {
printf("GYRO connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_GYRO, Cal);
if(FlashValid) {
CalFlashState.GYRO_FLASH = true;
GyroOffset[0] = Cal[0];
GyroOffset[1] = Cal[1];
GyroOffset[2] = Cal[2];
GyroScale[0] = Cal[3];
GyroScale[1] = Cal[4];
GyroScale[2] = Cal[5];
printf("GYRO calibration from [FLASH]\n");
}
else {
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
GyroScale[0] = 1.0;
GyroScale[1] = 1.0;
GyroScale[2] = 1.0;
printf("GYRO calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
printf("Scale: %f %f %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
nvtSetGyroScale(GyroScale);
nvtSetGyroOffset(GyroOffset);
#if defined(MPU6050) || defined(MPU6500)
nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
#else
nvtSetGYRODegPLSB(calcGyro(1));
#endif
}
else
printf("GYRO connect - [FAIL]\n");
}
void SensorInitGYRO()
{
float Cal[GYRO_CAL_DATA_SIZE];
bool FlashValid;
if(!SensorInitState.GYRO_Done)
{
#ifdef MPU6050
SensorInitState.GYRO_Done = MPU6050_initialize();
SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#endif
}
if(SensorInitState.GYRO_Done)
{
printf("GYRO connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_GYRO, Cal);
if(FlashValid)
{
CalFlashState.GYRO_FLASH = true;
GyroOffset[0] = Cal[0];
GyroOffset[1] = Cal[1];
GyroOffset[2] = Cal[2];
GyroScale[0] = Cal[3]*IMU_DEG_PER_LSB_CFG;
GyroScale[1] = Cal[4]*IMU_DEG_PER_LSB_CFG;
GyroScale[2] = Cal[5]*IMU_DEG_PER_LSB_CFG;
printf("GYRO calibration - [FLASH]\n");
}
else
{
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
GyroScale[0] = IMU_DEG_PER_LSB_CFG;
GyroScale[1] = IMU_DEG_PER_LSB_CFG;
GyroScale[2] = IMU_DEG_PER_LSB_CFG;
printf("GYRO calibration - [DEF]\n");
}
//printf("Offset: %f %f %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
//printf("Scale: %f %f %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
nvtSetGyroScale(GyroScale);
nvtSetGyroOffset(GyroOffset);
nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
}
else
printf("GYRO connect - [FAIL]\n");
}
/* 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");
}
}
