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