int main(void)
{
uint32_t lcdid;
inv_error_t result;
unsigned char accel_fsr, new_temp = 0;
unsigned short gyro_rate, gyro_fsr;
unsigned long timestamp;
struct int_param_s int_param;
HAL_Init();
SystemClock_Config();
lcdid=BSP_LCD_Init();
MX_DEBUG_USART_Init();
MX_SPIFlash_Init();
MPU_INT_GPIO_Init();
I2C_Bus_Init();
printf("LCD ID=0x%08X\n",lcdid);
printf("mpu 6050 test start");
LCD_Clear(0,0,LCD_DEFAULT_WIDTH,LCD_DEFAULT_HEIGTH,WHITE);
LCD_BK_ON();
LCD_DispString_EN_CH ( 20, 20, (const uint8_t *)"This is a MPU6050 demo", BACKGROUND, BLACK,USB_FONT_24);
result = mpu_init(&int_param);
if (result) {
MPL_LOGE("Could not initialize gyro.result = %d\n",result);
LCD_DispString_EN_CH(0,40,(const uint8_t *)"No MPU6050 detceted!Please check the hardware connection.", BACKGROUND,RED,USB_FONT_24);
}
else
{
LCD_DispString_EN_CH(30,40,(const uint8_t *)"MPU6050 decteted!", BACKGROUND,BLACK,USB_FONT_24);
}
result = inv_init_mpl();
if (result) {
MPL_LOGE("Could not initialize MPL.\n");
}
inv_enable_quaternion();
inv_enable_9x_sensor_fusion();
inv_enable_fast_nomot();
inv_enable_gyro_tc();
#ifdef COMPASS_ENABLED
inv_enable_vector_compass_cal();
inv_enable_magnetic_disturbance();
#endif
inv_enable_eMPL_outputs();
result = inv_start_mpl();
if (result == INV_ERROR_NOT_AUTHORIZED) {
while (1) {
MPL_LOGE("Not authorized.\n");
}
}
if (result) {
MPL_LOGE("Could not start the MPL.\n");
}
#ifdef COMPASS_ENABLED
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
#else
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
#endif
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);
#ifdef COMPASS_ENABLED
mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS);
#endif
mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
#ifdef COMPASS_ENABLED
mpu_get_compass_fsr(&compass_fsr);
#endif
inv_set_gyro_sample_rate(1000000L / gyro_rate);
inv_set_accel_sample_rate(1000000L / gyro_rate);
#ifdef COMPASS_ENABLED
inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);
#endif
inv_set_gyro_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
(long)gyro_fsr<<15);
inv_set_accel_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
(long)accel_fsr<<15);
#ifdef COMPASS_ENABLED
inv_set_compass_orientation_and_scale(
inv_orientation_matrix_to_scalar(compass_pdata.orientation),
(long)compass_fsr<<15);
#endif
#ifdef COMPASS_ENABLED
hal.sensors = ACCEL_ON | GYRO_ON | COMPASS_ON;
#else
hal.sensors = ACCEL_ON | GYRO_ON;
#endif
hal.dmp_on = 0;
hal.report = 0;
hal.rx.cmd = 0;
hal.next_pedo_ms = 0;
hal.next_compass_ms = 0;
hal.next_temp_ms = 0;
get_tick_count(×tamp);
dmp_load_motion_driver_firmware();
dmp_set_orientation(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
dmp_register_tap_cb(tap_cb);
dmp_register_android_orient_cb(android_orient_cb);
hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL;
dmp_enable_feature(hal.dmp_features);
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
mpu_set_dmp_state(1);
hal.dmp_on = 1;
while(1){
unsigned long sensor_timestamp;
int new_data = 0;
if (__HAL_USART_GET_FLAG(&husart_debug, USART_FLAG_RXNE)) {
__HAL_USART_CLEAR_FLAG(&husart_debug, USART_FLAG_RXNE);
handle_input();
}
get_tick_count(×tamp);
#ifdef COMPASS_ENABLED
if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
hal.new_gyro && (hal.sensors & COMPASS_ON)) {
hal.next_compass_ms = timestamp + COMPASS_READ_MS;
new_compass = 1;
}
#endif
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.motion_int_mode) {
mpu_lp_motion_interrupt(500, 1, 5);
inv_accel_was_turned_off();
inv_gyro_was_turned_off();
inv_compass_was_turned_off();
inv_quaternion_sensor_was_turned_off();
while (!hal.new_gyro) {}
mpu_lp_motion_interrupt(0, 0, 0);
hal.motion_int_mode = 0;
}
if (!hal.sensors || !hal.new_gyro) {
continue;
}
if (hal.new_gyro && hal.lp_accel_mode) {
short accel_short[3];
long accel[3];
mpu_get_accel_reg(accel_short, &sensor_timestamp);
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
hal.new_gyro = 0;
} else if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel_short[3], sensors;
unsigned char more;
long accel[3], quat[4], temperature;
dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
inv_build_quat(quat, 0, sensor_timestamp);
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel_short[3];
unsigned char sensors, more;
long accel[3], temperature;
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
&sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
}
#ifdef COMPASS_ENABLED
if (new_compass) {
short compass_short[3];
long compass[3];
new_compass = 0;
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
compass[0] = (long)compass_short[0];
compass[1] = (long)compass_short[1];
compass[2] = (long)compass_short[2];
inv_build_compass(compass, 0, sensor_timestamp);
}
new_data = 1;
}
#endif
if (new_data) {
inv_execute_on_data();
read_from_mpl();
}
}
}
楼主
标题置顶
标题高亮
