void imu_process_values() {
value_process_dt = micros() - value_process_timer; // for benchmarking
value_process_timer = micros();
process_gyro();
process_accel();
combine();
}
int Sensors_Process(uint8_t SensorType, float dt)
{
switch (SensorType)
{
case GYRO:
if (MARG_SENSORS[GYRO]->SensorValueUpdated)
{
/*ITG 3200*/
float x = (float) ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[1]) | (MARG_SENSORS[GYRO]->SensorRawValue[0] << 8))));
float y = (float) ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[3]) | (MARG_SENSORS[GYRO]->SensorRawValue[2] << 8))));
float z = (float) ((int) ((int16_t) ((MARG_SENSORS[GYRO]->SensorRawValue[5]) | (MARG_SENSORS[GYRO]->SensorRawValue[4] << 8))));
//float x = (float)((int)((int16_t)((MARG_SENSORS[GYRO]->SensorRawValue[0]) | (MARG_SENSORS[GYRO]->SensorRawValue[1] <<8))));
//float y = (float)((int)((int16_t)((MARG_SENSORS[GYRO]->SensorRawValue[2]) | (MARG_SENSORS[GYRO]->SensorRawValue[3] <<8))));
//float z = (float)((int)((int16_t)((MARG_SENSORS[GYRO]->SensorRawValue[4]) | (MARG_SENSORS[GYRO]->SensorRawValue[5] <<8))));
MARG_SENSORS[GYRO]->SensorValueUpdated = -1;
//async_printf("GYRO: %f %f %f\r\n",x, y, z);
process_gyro(x, y, z, dt);
GPIOToggle(LED6);
return 1;
}
break;
case ACC:
if (MARG_SENSORS[ACC]->SensorValueUpdated)
{
float x = (float) ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[0]) | (MARG_SENSORS[ACC]->SensorRawValue[1] << 8))));
float y = (float) ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[2]) | (MARG_SENSORS[ACC]->SensorRawValue[3] << 8))));
float z = (float) ((int) ((int16_t) ((MARG_SENSORS[ACC]->SensorRawValue[4]) | (MARG_SENSORS[ACC]->SensorRawValue[5] << 8))));
MARG_SENSORS[ACC]->SensorValueUpdated = -1;
//async_printf("ACC: %f %f %f\r\n", x, y, z);
process_acc(x, y, z);
GPIOToggle(LED6);
return 1;
}
break;
case MAG:
if (MARG_SENSORS[MAG]->SensorValueUpdated)
{
float x = (float) ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[1]) | (MARG_SENSORS[MAG]->SensorRawValue[0] << 8))));
float y = (float) ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[3]) | (MARG_SENSORS[MAG]->SensorRawValue[2] << 8))));
float z = (float) ((int) ((int16_t) ((MARG_SENSORS[MAG]->SensorRawValue[5]) | (MARG_SENSORS[MAG]->SensorRawValue[4] << 8))));
MARG_SENSORS[MAG]->SensorValueUpdated = -1;
//async_printf("MAG: %f %f %f\r\n",x, y, z);
process_mag(x, y, z);
GPIOToggle(LED6);
return 1;
}
break;
default:
return -1;
}
}
BOOL callback_tilt_reading( struct sCAN* mMsg )
{
BOOL retval = FALSE;
switch (mMsg->id.group.id)
{
case ID_ACCEL_XYZ :
count_accel++;
parse_accel_msg (mMsg);
process_accel (FALSE); //ShowAccelerometerData);
retval= TRUE;
break;
case ID_GYRO_XYZ :
count_gyro++;
parse_gyro_msg (mMsg); // puts into RawxyzGyro
process_gyro (FALSE); //ShowGyroData);
if (ShowGyroData)
{
printf("\n");
//print_vector( &RawxyzGyro ); printf("\n");
//print_message(mMsg); printf("\n");
}
retval=TRUE;
break;
/* case ID_MAGNET_XYZ :
count_magnet++;
if (ShowMagnetData && ShowCANData) print_message(mMsg);
parse_magnet_msg(mMsg);
process_magnet(ShowMagnetData);
if (ShowMagnetData) printf("\n");
retval= TRUE;
break; */
default:
retval= FALSE;
break;
}
//if ((count_accel==1) && (count_gyro==1) && (count_magnet==1))
if ((count_accel==1) && (count_gyro==1))
{
printf("<<<=======FUSION===========>>>\n");
compute_accelGyro_fusion(FALSE);
(*tiltsensor_callback)( );
/*if (count_samples++ < 300)
save_timeslice_data();
else
close_log_file();
*/
count_accel=0;
count_gyro=0;
count_magnet=0;
}
return retval;
}
