/**
* @brief EXTI line detection callbacks
* @param GPIO_Pin: Specifies the pins connected EXTI line
* @retval None
*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if (GPIO_Pin == GPIO_PIN_13)
{
int32_t d1, d2, d3, d4, d5, d6, d7, d8;
int32_t data[3];
int32_t gdata[6];
if(BSP_HUM_TEMP_isInitialized()) {
BSP_HUM_TEMP_GetHumidity((float *)&HUMIDITY_Value);
BSP_HUM_TEMP_GetTemperature((float *)&TEMPERATURE_Value);
floatToInt(HUMIDITY_Value, &d1, &d2, 2);
float tempF = celsius2fahrenheit(TEMPERATURE_Value);
floatToInt(tempF, &d3, &d4, 2);
sprintf(dataOut, "HUM: %d.%02d rH TEMP: %d.%02d (f)\n\r", (int)d1, (int)d2, (int)d3, (int)d4);
printf(dataOut);
}
if(BSP_PRESSURE_isInitialized())
{
BSP_PRESSURE_GetPressure((float *)&PRESSURE_Value);
BSP_PRESSURE_GetTemperature((float *)&TEMPERATURE2_Value);
floatToInt(PRESSURE_Value, &d5, &d6, 2);
float tempF2 = celsius2fahrenheit(TEMPERATURE2_Value);
floatToInt(tempF2, &d7, &d8, 2);
sprintf(dataOut, "PRESS: %d.%02d hPa TEMP: %d.%02d (f)\n\r", (int)d5, (int)d6, (int)d7, (int)d8);
printf(dataOut);
}
if(BSP_MAGNETO_isInitialized())
{
BSP_MAGNETO_M_GetAxesRaw((AxesRaw_TypeDef *)&MAG_Value);
data[0] = MAG_Value.AXIS_X;
data[1] = MAG_Value.AXIS_Y;
data[2] = MAG_Value.AXIS_Z;
sprintf(dataOut, "MAG_X: %d, MAG_Y: %d, MAG_Z: %d\n\r", (int)data[0], (int)data[1], (int)data[2]);
printf(dataOut);
}
if(BSP_IMU_6AXES_isInitialized()) {
BSP_IMU_6AXES_X_GetAxesRaw((AxesRaw_TypeDef *)&ACC_Value);
BSP_IMU_6AXES_G_GetAxesRaw((AxesRaw_TypeDef *)&GYR_Value);
gdata[0] = ACC_Value.AXIS_X;
gdata[1] = ACC_Value.AXIS_Y;
gdata[2] = ACC_Value.AXIS_Z;
gdata[3] = GYR_Value.AXIS_X;
gdata[4] = GYR_Value.AXIS_Y;
gdata[5] = GYR_Value.AXIS_Z;
sprintf(dataOut, "ACC_X: %d, ACC_Y: %d, ACC_Z: %d\n\r", (int)gdata[0], (int)gdata[1], (int)gdata[2]);
printf(dataOut);
sprintf(dataOut, "GYR_X: %d, GYR_Y: %d, GYR_Z: %d\n\r", (int)gdata[3], (int)gdata[4], (int)gdata[5]);
printf(dataOut);
}
}
}
void imuRead(Axis3f* gyroOut, Axis3f* accOut, Axis3f* magOut)
{
float G_sensitivity = 0.0f;
float X_sensitivity = 0.0f;
BSP_IMU_6AXES_X_GetAxesRaw((AxesRaw_TypeDef *)&accelMpu);
BSP_IMU_6AXES_G_GetAxesRaw((AxesRaw_TypeDef *)&gyroMpu);
BSP_MAGNETO_M_GetAxes((Axes_TypeDef *)&mag);
BSP_IMU_6AXES_G_GetSensitivity(&G_sensitivity);
BSP_IMU_6AXES_X_GetSensitivity(&X_sensitivity);
imuAddBiasValue(&gyroBias, &gyroMpu);
#ifdef IMU_TAKE_ACCEL_BIAS
if (!accelBias.isBiasValueFound)
{
imuAddBiasValue(&accelBias, &accelMpu);
}
#endif
if (!gyroBias.isBiasValueFound)
{
imuFindBiasValue(&gyroBias);
if (gyroBias.isBiasValueFound)
{
//ledseqRun(SYS_LED, seq_calibrated);
#ifndef IMU_TAKE_ACCEL_BIAS
BSP_LED_On(LED2);
#endif
}
}
#ifdef IMU_TAKE_ACCEL_BIAS
if (gyroBias.isBiasValueFound &&
!accelBias.isBiasValueFound)
{
Axis3i32 mean;
imuCalculateBiasMean(&accelBias, &mean);
accelBias.bias.x = mean.x;
accelBias.bias.y = mean.y;
accelBias.bias.z = mean.z - (1 / X_sensitivity * 1000.0f);
accelBias.isBiasValueFound = true;
BSP_LED_On(LED2);
}
#endif
imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues, (int32_t)imuAccLpfAttFactor);
//imuAccAlignToGravity(&accelLPF, &accelLPFAligned);
/*
accOut->x = ACC_Value.AXIS_X / 1000.0;
accOut->y = ACC_Value.AXIS_Y / 1000.0;
accOut->z = ACC_Value.AXIS_Z / 1000.0;
gyroOut->x = GYR_Value.AXIS_X / 1000.0;
gyroOut->y = GYR_Value.AXIS_Y / 1000.0;
gyroOut->z = GYR_Value.AXIS_Z / 1000.0;
magOut->x = MAG_Value.AXIS_X / 1000.0f;
magOut->y = MAG_Value.AXIS_Y / 1000.0f;
magOut->z = MAG_Value.AXIS_Z / 1000.0f;
gyroOut->x = gyroOut->x * M_PI / 180.0;
gyroOut->y = gyroOut->y * M_PI / 180.0;
gyroOut->z = gyroOut->z * M_PI / 180.0;
*/
// Re-map outputs
gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * G_sensitivity / 1000.0f;
gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * G_sensitivity / 1000.0f;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * G_sensitivity / 1000.0f;
/*
gyroOut->x = (gyroMpu.x) * G_sensitivity / 1000.0f;
gyroOut->y = (gyroMpu.y) * G_sensitivity / 1000.0f;
gyroOut->z = (gyroMpu.z) * G_sensitivity / 1000.0f;
*/
gyroOut->x = - gyroOut->x * M_PI / 180.0f;
gyroOut->y = gyroOut->y * M_PI / 180.0f;
gyroOut->z = gyroOut->z * M_PI / 180.0f;
#ifdef IMU_TAKE_ACCEL_BIAS
/*
accOut->x = (accelLPFAligned.x - accelBias.bias.x) * X_sensitivity / 1000.0f;
accOut->y = (accelLPFAligned.y - accelBias.bias.y) * X_sensitivity / 1000.0f;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * X_sensitivity / 1000.0f;
*/
accOut->x = - (accelLPF.x - accelBias.bias.x) * X_sensitivity / 1000.0f;
accOut->y = (accelLPF.y - accelBias.bias.y) * X_sensitivity / 1000.0f;
accOut->z = (accelLPF.z - accelBias.bias.z) * X_sensitivity / 1000.0f;
#else
//accOut->x = (accelLPFAligned.x) * X_sensitivity / 1000.0f;
//accOut->y = (accelLPFAligned.y) * X_sensitivity / 1000.0f;
//accOut->z = (accelLPFAligned.z) * X_sensitivity / 1000.0f;
accOut->x = - (accelLPF.x) * X_sensitivity;
accOut->y = (accelLPF.y) * X_sensitivity;
accOut->z = (accelLPF.z) * X_sensitivity;
#endif
magOut->x = (float)mag.x / 10000.0f;
magOut->y = (float)mag.y / 10000.0f;
magOut->z = (float)mag.z / 10000.0f;
}