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