Exemplo n.º 1
0
/**
  * @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);

	    }
  }
}
Exemplo n.º 2
0
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;


}