/** * @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; }