/** ****************************************************************************** * @file main.c * @author fire * @version V1.0 * @date 2016-xx-xx * @brief MPU6050测试程序 * 本程序必须配合匿名上位机使用！！串口向上位机传输的是数据包，所以直接用串口调试助手看是乱码，这是正常的！！ ****************************************************************************** * @attention * * 实验平台:秉火 STM32 H750 开发板 * 论坛 :http://www.firebbs.cn * 淘宝 :http://firestm32.taobao.com * ****************************************************************************** */ #include "stm32h7xx.h" #include "./led/bsp_led.h" #include "./usart/bsp_debug_usart.h" #include #include "main.h" #include "./i2c/i2c.h" #include "./mpu6050/bsp_mpu_exti.h" #include "stdio.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" #include "invensense.h" #include "invensense_adv.h" #include "eMPL_outputs.h" #include "mltypes.h" #include "mpu.h" #include "log.h" #include "packet.h" /* Private typedef -----------------------------------------------------------*/ /* Data read from MPL. */ #define PRINT_ACCEL (0x01) #define PRINT_GYRO (0x02) #define PRINT_QUAT (0x04) #define PRINT_COMPASS (0x08) #define PRINT_EULER (0x10) #define PRINT_ROT_MAT (0x20) #define PRINT_HEADING (0x40) #define PRINT_PEDO (0x80) #define PRINT_LINEAR_ACCEL (0x100) #define PRINT_GRAVITY_VECTOR (0x200) volatile uint32_t hal_timestamp = 0; #define ACCEL_ON (0x01) #define GYRO_ON (0x02) #define COMPASS_ON (0x04) #define MOTION (0) #define NO_MOTION (1) /* Starting sampling rate. */ #define DEFAULT_MPU_HZ (20) //#define FLASH_SIZE (512) #define FLASH_MEM_START ((void*)0x1800) #define PEDO_READ_MS (1000) #define TEMP_READ_MS (500) #define COMPASS_READ_MS (100) struct rx_s { unsigned char header[3]; unsigned char cmd; }; struct hal_s { unsigned char lp_accel_mode; unsigned char sensors; unsigned char dmp_on; unsigned char wait_for_tap; volatile unsigned char new_gyro; unsigned char motion_int_mode; unsigned long no_dmp_hz; unsigned long next_pedo_ms; unsigned long next_temp_ms; unsigned long next_compass_ms; unsigned int report; unsigned short dmp_features; struct rx_s rx; }; static struct hal_s hal = {0}; /* USB RX binary semaphore. Actually, it's just a flag. Not included in struct * because it's declared extern elsewhere. */ volatile unsigned char rx_new; unsigned char *mpl_key = (unsigned char*)"eMPL 5.1"; /* Platform-specific information. Kinda like a boardfile. */ struct platform_data_s { signed char orientation[9]; }; /* The sensors can be mounted onto the board in any orientation. The mounting * matrix seen below tells the MPL how to rotate the raw data from the * driver(s). * TODO: The following matrices refer to the configuration on internal test * boards at Invensense. If needed, please modify the matrices to match the * chip-to-body matrix for your particular set up. */ static struct platform_data_s gyro_pdata = { .orientation = { 1, 0, 0, 0, 1, 0, 0, 0, 1} }; #if defined MPU9150 || defined MPU9250 static struct platform_data_s compass_pdata = { .orientation = { 0, 1, 0, 1, 0, 0, 0, 0, -1} }; #define COMPASS_ENABLED 1 #elif defined AK8975_SECONDARY static struct platform_data_s compass_pdata = { .orientation = {-1, 0, 0, 0, 1, 0, 0, 0,-1} }; #define COMPASS_ENABLED 1 #elif defined AK8963_SECONDARY static struct platform_data_s compass_pdata = { .orientation = {-1, 0, 0, 0,-1, 0, 0, 0, 1} }; #define COMPASS_ENABLED 1 #endif #define BYTE0(dwTemp) (*(char *)(&dwTemp)) #define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1)) #define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2)) #define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3)) /** * @brief 控制串口发送1个字符 * @param c:要发送的字符 * @retval none */ void usart_send_char(uint8_t c) { HAL_UART_Transmit( &UartHandle,(uint8_t *)&c ,1,1000); } /*函数功能：根据匿名最新上位机协议写的显示姿态的程序（上位机0512版本） *具体协议说明请查看上位机软件的帮助说明。 */ void Data_Send_Status(float Pitch,float Roll,float Yaw) { unsigned char i=0; unsigned char _cnt=0,sum = 0; unsigned int _temp; unsigned char data_to_send[50]; data_to_send[_cnt++]=0xAA; data_to_send[_cnt++]=0xAA; data_to_send[_cnt++]=0x01; data_to_send[_cnt++]=0; _temp = (int)(Roll*100); data_to_send[_cnt++]=BYTE1(_temp); data_to_send[_cnt++]=BYTE0(_temp); _temp = 0-(int)(Pitch*100); data_to_send[_cnt++]=BYTE1(_temp); data_to_send[_cnt++]=BYTE0(_temp); _temp = (int)(Yaw*100); data_to_send[_cnt++]=BYTE1(_temp); data_to_send[_cnt++]=BYTE0(_temp); _temp = 0; data_to_send[_cnt++]=BYTE3(_temp); data_to_send[_cnt++]=BYTE2(_temp); data_to_send[_cnt++]=BYTE1(_temp); data_to_send[_cnt++]=BYTE0(_temp); data_to_send[_cnt++]=0xA0; data_to_send[3] = _cnt-4; //和校验 for(i=0;i<_cnt;i++) sum+= data_to_send[i]; data_to_send[_cnt++]=sum; //串口发送数据 for(i=0;i<_cnt;i++) usart_send_char(data_to_send[i]); } /*函数功能：根据匿名最新上位机协议写的显示传感器数据（上位机0512版本） *具体协议说明请查看上位机软件的帮助说明。 */ void Send_Data(int16_t *Gyro,int16_t *Accel) { unsigned char i=0; unsigned char _cnt=0,sum = 0; // unsigned int _temp; unsigned char data_to_send[50]; data_to_send[_cnt++]=0xAA; data_to_send[_cnt++]=0xAA; data_to_send[_cnt++]=0x02; data_to_send[_cnt++]=0; data_to_send[_cnt++]=BYTE1(Accel[0]); data_to_send[_cnt++]=BYTE0(Accel[0]); data_to_send[_cnt++]=BYTE1(Accel[1]); data_to_send[_cnt++]=BYTE0(Accel[1]); data_to_send[_cnt++]=BYTE1(Accel[2]); data_to_send[_cnt++]=BYTE0(Accel[2]); data_to_send[_cnt++]=BYTE1(Gyro[0]); data_to_send[_cnt++]=BYTE0(Gyro[0]); data_to_send[_cnt++]=BYTE1(Gyro[1]); data_to_send[_cnt++]=BYTE0(Gyro[1]); data_to_send[_cnt++]=BYTE1(Gyro[2]); data_to_send[_cnt++]=BYTE0(Gyro[2]); data_to_send[_cnt++]=0; data_to_send[_cnt++]=0; data_to_send[_cnt++]=0; data_to_send[_cnt++]=0; data_to_send[_cnt++]=0; data_to_send[_cnt++]=0; data_to_send[3] = _cnt-4; //和校验 for(i=0;i<_cnt;i++) sum+= data_to_send[i]; data_to_send[_cnt++]=sum; //串口发送数据 for(i=0;i<_cnt;i++) usart_send_char(data_to_send[i]); } extern struct inv_sensor_cal_t sensors; /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ /* ---------------------------------------------------------------------------*/ /* Get data from MPL. * TODO: Add return values to the inv_get_sensor_type_xxx APIs to differentiate * between new and stale data. */ static void read_from_mpl(void) { long msg, data[9]; int8_t accuracy; unsigned long timestamp; float float_data[3] = {0}; if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)×tamp)) { /* Sends a quaternion packet to the PC. Since this is used by the Python * test app to visually represent a 3D quaternion, it's sent each time * the MPL has new data. */ eMPL_send_quat(data); /* Specific data packets can be sent or suppressed using USB commands. */ if (hal.report & PRINT_QUAT) eMPL_send_data(PACKET_DATA_QUAT, data); } if (hal.report & PRINT_ACCEL) { if (inv_get_sensor_type_accel(data, &accuracy, (inv_time_t*)×tamp)) eMPL_send_data(PACKET_DATA_ACCEL, data); } if (hal.report & PRINT_GYRO) { if (inv_get_sensor_type_gyro(data, &accuracy, (inv_time_t*)×tamp)) eMPL_send_data(PACKET_DATA_GYRO, data); } #ifdef COMPASS_ENABLED if (hal.report & PRINT_COMPASS) { if (inv_get_sensor_type_compass(data, &accuracy, (inv_time_t*)×tamp)) eMPL_send_data(PACKET_DATA_COMPASS, data); } #endif if (hal.report & PRINT_EULER) { if (inv_get_sensor_type_euler(data, &accuracy, (inv_time_t*)×tamp)) eMPL_send_data(PACKET_DATA_EULER, data); } /********************使用液晶屏显示数据**************************/ if(1) { char cStr [ 70 ]; unsigned long timestamp,step_count,walk_time; /*获取欧拉角*/ if (inv_get_sensor_type_euler(data, &accuracy,(inv_time_t*)×tamp)) { float Pitch,Roll,Yaw; //inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位. Pitch =data[0]*1.0/(1<<16) ; Roll = data[1]*1.0/(1<<16); Yaw = data[2]*1.0/(1<<16); /*向匿名上位机发送姿态*/ Data_Send_Status(Pitch,Roll,Yaw); /*向匿名上位机发送原始数据*/ Send_Data((int16_t *)&sensors.gyro.raw,(int16_t *)&sensors.accel.raw); } /*获取步数*/ get_tick_count(×tamp); if (timestamp > hal.next_pedo_ms) { hal.next_pedo_ms = timestamp + PEDO_READ_MS; dmp_get_pedometer_step_count(&step_count); dmp_get_pedometer_walk_time(&walk_time); } } if (hal.report & PRINT_ROT_MAT) { if (inv_get_sensor_type_rot_mat(data, &accuracy, (inv_time_t*)×tamp)) eMPL_send_data(PACKET_DATA_ROT, data); } if (hal.report & PRINT_HEADING) { if (inv_get_sensor_type_heading(data, &accuracy, (inv_time_t*)×tamp)) eMPL_send_data(PACKET_DATA_HEADING, data); } if (hal.report & PRINT_LINEAR_ACCEL) { if (inv_get_sensor_type_linear_acceleration(float_data, &accuracy, (inv_time_t*)×tamp)) { MPL_LOGI("Linear Accel: %7.5f %7.5f %7.5f\r\n", float_data[0], float_data[1], float_data[2]); } } if (hal.report & PRINT_GRAVITY_VECTOR) { if (inv_get_sensor_type_gravity(float_data, &accuracy, (inv_time_t*)×tamp)) MPL_LOGI("Gravity Vector: %7.5f %7.5f %7.5f\r\n", float_data[0], float_data[1], float_data[2]); } if (hal.report & PRINT_PEDO) { unsigned long timestamp; get_tick_count(×tamp); if (timestamp > hal.next_pedo_ms) { hal.next_pedo_ms = timestamp + PEDO_READ_MS; unsigned long step_count, walk_time; dmp_get_pedometer_step_count(&step_count); dmp_get_pedometer_walk_time(&walk_time); MPL_LOGI("Walked %ld steps over %ld milliseconds..\n", step_count, walk_time); } } /* Whenever the MPL detects a change in motion state, the application can * be notified. For this example, we use an LED to represent the current * motion state. */ msg = inv_get_message_level_0(INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT); if (msg) { if (msg & INV_MSG_MOTION_EVENT) { MPL_LOGI("Motion!\n"); } else if (msg & INV_MSG_NO_MOTION_EVENT) { MPL_LOGI("No motion!\n"); } } } #ifdef COMPASS_ENABLED void send_status_compass() { long data[3] = { 0 }; int8_t accuracy = { 0 }; unsigned long timestamp; inv_get_compass_set(data, &accuracy, (inv_time_t*) ×tamp); MPL_LOGI("Compass: %7.4f %7.4f %7.4f ", data[0]/65536.f, data[1]/65536.f, data[2]/65536.f); MPL_LOGI("Accuracy= %d\r\n", accuracy); } #endif /* Handle sensor on/off combinations. */ static void setup_gyro(void) { unsigned char mask = 0, lp_accel_was_on = 0; if (hal.sensors & ACCEL_ON) mask |= INV_XYZ_ACCEL; if (hal.sensors & GYRO_ON) { mask |= INV_XYZ_GYRO; lp_accel_was_on |= hal.lp_accel_mode; } #ifdef COMPASS_ENABLED if (hal.sensors & COMPASS_ON) { mask |= INV_XYZ_COMPASS; lp_accel_was_on |= hal.lp_accel_mode; } #endif /* If you need a power transition, this function should be called with a * mask of the sensors still enabled. The driver turns off any sensors * excluded from this mask. */ mpu_set_sensors(mask); mpu_configure_fifo(mask); if (lp_accel_was_on) { unsigned short rate; hal.lp_accel_mode = 0; /* Switching out of LP accel, notify MPL of new accel sampling rate. */ mpu_get_sample_rate(&rate); inv_set_accel_sample_rate(1000000L / rate); } } static void tap_cb(unsigned char direction, unsigned char count) { switch (direction) { case TAP_X_UP: MPL_LOGI("Tap X+ "); break; case TAP_X_DOWN: MPL_LOGI("Tap X- "); break; case TAP_Y_UP: MPL_LOGI("Tap Y+ "); break; case TAP_Y_DOWN: MPL_LOGI("Tap Y- "); break; case TAP_Z_UP: MPL_LOGI("Tap Z+ "); break; case TAP_Z_DOWN: MPL_LOGI("Tap Z- "); break; default: return; } MPL_LOGI("x%d\n", count); return; } static void android_orient_cb(unsigned char orientation) { switch (orientation) { case ANDROID_ORIENT_PORTRAIT: MPL_LOGI("Portrait\n"); break; case ANDROID_ORIENT_LANDSCAPE: MPL_LOGI("Landscape\n"); break; case ANDROID_ORIENT_REVERSE_PORTRAIT: MPL_LOGI("Reverse Portrait\n"); break; case ANDROID_ORIENT_REVERSE_LANDSCAPE: MPL_LOGI("Reverse Landscape\n"); break; default: return; } } static inline void run_self_test(void) { int result; long gyro[3], accel[3]; #if defined (MPU6500) || defined (MPU9250) result = mpu_run_6500_self_test(gyro, accel, 0); #elif defined (MPU6050) || defined (MPU9150) result = mpu_run_self_test(gyro, accel); #endif if (result == 0x7) { MPL_LOGI("Passed!\n"); MPL_LOGI("accel: %7.4f %7.4f %7.4f\n", accel[0]/65536.f, accel[1]/65536.f, accel[2]/65536.f); MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n", gyro[0]/65536.f, gyro[1]/65536.f, gyro[2]/65536.f); /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/ #ifdef USE_CAL_HW_REGISTERS /* * This portion of the code uses the HW offset registers that are in the MPUxxxx devices * instead of pushing the cal data to the MPL software library */ unsigned char i = 0; for(i = 0; i<3; i++) { gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps accel[i] *= 2048.f; //convert to +-16G accel[i] = accel[i] >> 16; gyro[i] = (long)(gyro[i] >> 16); } mpu_set_gyro_bias_reg(gyro); #if defined (MPU6500) || defined (MPU9250) mpu_set_accel_bias_6500_reg(accel); #elif defined (MPU6050) || defined (MPU9150) mpu_set_accel_bias_6050_reg(accel); #endif #else /* Push the calibrated data to the MPL library. * * MPL expects biases in hardware units << 16, but self test returns * biases in g's << 16. */ unsigned short accel_sens; float gyro_sens; mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; inv_set_accel_bias(accel, 3); mpu_get_gyro_sens(&gyro_sens); gyro[0] = (long) (gyro[0] * gyro_sens); gyro[1] = (long) (gyro[1] * gyro_sens); gyro[2] = (long) (gyro[2] * gyro_sens); inv_set_gyro_bias(gyro, 3); #endif } else { if (!(result & 0x1)) MPL_LOGE("Gyro failed.\n"); if (!(result & 0x2)) MPL_LOGE("Accel failed.\n"); if (!(result & 0x4)) MPL_LOGE("Compass failed.\n"); } } static void handle_input(void) { char c; HAL_UART_Receive(&UartHandle, (uint8_t *)&c, 1, 1000); //USART_ReceiveData(DEBUG_USART); switch (c) { /* These commands turn off individual sensors. */ case '8': hal.sensors ^= ACCEL_ON; setup_gyro(); if (!(hal.sensors & ACCEL_ON)) inv_accel_was_turned_off(); break; case '9': hal.sensors ^= GYRO_ON; setup_gyro(); if (!(hal.sensors & GYRO_ON)) inv_gyro_was_turned_off(); break; #ifdef COMPASS_ENABLED case '0': hal.sensors ^= COMPASS_ON; setup_gyro(); if (!(hal.sensors & COMPASS_ON)) inv_compass_was_turned_off(); break; #endif /* The commands send individual sensor data or fused data to the PC. */ case 'a': hal.report ^= PRINT_ACCEL; break; case 'g': hal.report ^= PRINT_GYRO; break; #ifdef COMPASS_ENABLED case 'c': hal.report ^= PRINT_COMPASS; break; #endif case 'e': hal.report ^= PRINT_EULER; break; case 'r': hal.report ^= PRINT_ROT_MAT; break; case 'q': hal.report ^= PRINT_QUAT; break; case 'h': hal.report ^= PRINT_HEADING; break; case 'i': hal.report ^= PRINT_LINEAR_ACCEL; break; case 'o': hal.report ^= PRINT_GRAVITY_VECTOR; break; #ifdef COMPASS_ENABLED case 'w': send_status_compass(); break; #endif /* This command prints out the value of each gyro register for debugging. * If logging is disabled, this function has no effect. */ case 'd': mpu_reg_dump(); break; /* Test out low-power accel mode. */ case 'p': if (hal.dmp_on) /* LP accel is not compatible with the DMP. */ break; mpu_lp_accel_mode(20); /* When LP accel mode is enabled, the driver automatically configures * the hardware for latched interrupts. However, the MCU sometimes * misses the rising/falling edge, and the hal.new_gyro flag is never * set. To avoid getting locked in this state, we're overriding the * driver's configuration and sticking to unlatched interrupt mode. * * TODO: The MCU supports level-triggered interrupts. */ mpu_set_int_latched(0); hal.sensors &= ~(GYRO_ON|COMPASS_ON); hal.sensors |= ACCEL_ON; hal.lp_accel_mode = 1; inv_gyro_was_turned_off(); inv_compass_was_turned_off(); break; /* The hardware self test can be run without any interaction with the * MPL since it's completely localized in the gyro driver. Logging is * assumed to be enabled; otherwise, a couple LEDs could probably be used * here to display the test results. */ case 't': run_self_test(); /* Let MPL know that contiguity was broken. */ inv_accel_was_turned_off(); inv_gyro_was_turned_off(); inv_compass_was_turned_off(); break; /* Depending on your application, sensor data may be needed at a faster or * slower rate. These commands can speed up or slow down the rate at which * the sensor data is pushed to the MPL. * * In this example, the compass rate is never changed. */ case '1': if (hal.dmp_on) { dmp_set_fifo_rate(10); inv_set_quat_sample_rate(100000L); } else mpu_set_sample_rate(10); inv_set_gyro_sample_rate(100000L); inv_set_accel_sample_rate(100000L); break; case '2': if (hal.dmp_on) { dmp_set_fifo_rate(20); inv_set_quat_sample_rate(50000L); } else mpu_set_sample_rate(20); inv_set_gyro_sample_rate(50000L); inv_set_accel_sample_rate(50000L); break; case '3': if (hal.dmp_on) { dmp_set_fifo_rate(40); inv_set_quat_sample_rate(25000L); } else mpu_set_sample_rate(40); inv_set_gyro_sample_rate(25000L); inv_set_accel_sample_rate(25000L); break; case '4': if (hal.dmp_on) { dmp_set_fifo_rate(50); inv_set_quat_sample_rate(20000L); } else mpu_set_sample_rate(50); inv_set_gyro_sample_rate(20000L); inv_set_accel_sample_rate(20000L); break; case '5': if (hal.dmp_on) { dmp_set_fifo_rate(100); inv_set_quat_sample_rate(10000L); } else mpu_set_sample_rate(100); inv_set_gyro_sample_rate(10000L); inv_set_accel_sample_rate(10000L); break; case ',': /* Set hardware to interrupt on gesture event only. This feature is * useful for keeping the MCU asleep until the DMP detects as a tap or * orientation event. */ dmp_set_interrupt_mode(DMP_INT_GESTURE); break; case '.': /* Set hardware to interrupt periodically. */ dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); break; case '6': /* Toggle pedometer display. */ hal.report ^= PRINT_PEDO; break; case '7': /* Reset pedometer. */ dmp_set_pedometer_step_count(0); dmp_set_pedometer_walk_time(0); break; case 'f': if (hal.lp_accel_mode) /* LP accel is not compatible with the DMP. */ return; /* Toggle DMP. */ if (hal.dmp_on) { unsigned short dmp_rate; unsigned char mask = 0; hal.dmp_on = 0; mpu_set_dmp_state(0); /* Restore FIFO settings. */ if (hal.sensors & ACCEL_ON) mask |= INV_XYZ_ACCEL; if (hal.sensors & GYRO_ON) mask |= INV_XYZ_GYRO; if (hal.sensors & COMPASS_ON) mask |= INV_XYZ_COMPASS; mpu_configure_fifo(mask); /* When the DMP is used, the hardware sampling rate is fixed at * 200Hz, and the DMP is configured to downsample the FIFO output * using the function dmp_set_fifo_rate. However, when the DMP is * turned off, the sampling rate remains at 200Hz. This could be * handled in inv_mpu.c, but it would need to know that * inv_mpu_dmp_motion_driver.c exists. To avoid this, we'll just * put the extra logic in the application layer. */ dmp_get_fifo_rate(&dmp_rate); mpu_set_sample_rate(dmp_rate); inv_quaternion_sensor_was_turned_off(); MPL_LOGI("DMP disabled.\n"); } else { unsigned short sample_rate; hal.dmp_on = 1; /* Preserve current FIFO rate. */ mpu_get_sample_rate(&sample_rate); dmp_set_fifo_rate(sample_rate); inv_set_quat_sample_rate(1000000L / sample_rate); mpu_set_dmp_state(1); MPL_LOGI("DMP enabled.\n"); } break; case 'm': /* Test the motion interrupt hardware feature. */ #ifndef MPU6050 // not enabled for 6050 product hal.motion_int_mode = 1; #endif break; case 'v': /* Toggle LP quaternion. * The DMP features can be enabled/disabled at runtime. Use this same * approach for other features. */ hal.dmp_features ^= DMP_FEATURE_6X_LP_QUAT; dmp_enable_feature(hal.dmp_features); if (!(hal.dmp_features & DMP_FEATURE_6X_LP_QUAT)) { inv_quaternion_sensor_was_turned_off(); MPL_LOGI("LP quaternion disabled.\n"); } else MPL_LOGI("LP quaternion enabled.\n"); break; default: break; } hal.rx.cmd = 0; } /* Every time new gyro data is available, this function is called in an * ISR context. In this example, it sets a flag protecting the FIFO read * function. */ void gyro_data_ready_cb(void) { hal.new_gyro = 1; } /*******************************************************************************/ /** * @brief 主函数 * @param 无 * @retval 无 */ int main(void) { 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; #ifdef COMPASS_ENABLED unsigned char new_compass = 0; unsigned short compass_fsr; #endif /* 系统时钟初始化成480 MHz */ SystemClock_Config(); /* 默认不配置 MPU，若需要更高性能，当配置 MPU 后，使用 DMA 时需注意 Cache 与 内存内容一致性的问题， 具体注意事项请参考配套教程的 MPU 配置相关章节 */ // Board_MPU_Config(0, MPU_Normal_WT, 0xD0000000, MPU_32MB); // Board_MPU_Config(1, MPU_Normal_WT, 0x24000000, MPU_512KB); SCB_EnableICache(); // 使能指令 Cache SCB_EnableDCache(); // 使能数据 Cache /* LED 端口初始化 */ LED_GPIO_Config(); /*初始化USART1*/ DEBUG_USART_Config(); EXTI_MPU_Config(); //初始化 I2C I2cMaster_Init(); result = mpu_init(&int_param); if (result) { MPL_LOGE("Could not initialize gyro.\n"); LED1_ON; } else { LED2_ON; } /* If you're not using an MPU9150 AND you're not using DMP features, this * function will place all slaves on the primary bus. * mpu_set_bypass(1); */ result = inv_init_mpl(); if (result) { MPL_LOGE("Could not initialize MPL.\n"); } /* Compute 6-axis and 9-axis quaternions. */ inv_enable_quaternion(); inv_enable_9x_sensor_fusion(); /* The MPL expects compass data at a constant rate (matching the rate * passed to inv_set_compass_sample_rate). If this is an issue for your * application, call this function, and the MPL will depend on the * timestamps passed to inv_build_compass instead. * * inv_9x_fusion_use_timestamps(1); */ /* This function has been deprecated. * inv_enable_no_gyro_fusion(); */ /* Update gyro biases when not in motion. * WARNING: These algorithms are mutually exclusive. */ inv_enable_fast_nomot(); /* inv_enable_motion_no_motion(); */ /* inv_set_no_motion_time(1000); */ /* Update gyro biases when temperature changes. */ inv_enable_gyro_tc(); /* This algorithm updates the accel biases when in motion. A more accurate * bias measurement can be made when running the self-test (see case 't' in * handle_input), but this algorithm can be enabled if the self-test can't * be executed in your application. * * inv_enable_in_use_auto_calibration(); */ #ifdef COMPASS_ENABLED /* Compass calibration algorithms. */ inv_enable_vector_compass_cal(); inv_enable_magnetic_disturbance(); #endif /* If you need to estimate your heading before the compass is calibrated, * enable this algorithm. It becomes useless after a good figure-eight is * detected, so we'll just leave it out to save memory. * inv_enable_heading_from_gyro(); */ /* Allows use of the MPL APIs in read_from_mpl. */ 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"); } /* Get/set hardware configuration. Start gyro. */ /* Wake up all sensors. */ #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 /* Push both gyro and accel data into the FIFO. */ mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(DEFAULT_MPU_HZ); #ifdef COMPASS_ENABLED /* The compass sampling rate can be less than the gyro/accel sampling rate. * Use this function for proper power management. */ mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS); #endif /* Read back configuration in case it was set improperly. */ 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 /* Sync driver configuration with MPL. */ /* Sample rate expected in microseconds. */ inv_set_gyro_sample_rate(1000000L / gyro_rate); inv_set_accel_sample_rate(1000000L / gyro_rate); #ifdef COMPASS_ENABLED /* The compass rate is independent of the gyro and accel rates. As long as * inv_set_compass_sample_rate is called with the correct value, the 9-axis * fusion algorithm's compass correction gain will work properly. */ inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L); #endif /* Set chip-to-body orientation matrix. * Set hardware units to dps/g's/degrees scaling factor. */ 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 /* Initialize HAL state variables. */ #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; /* Compass reads are handled by scheduler. */ get_tick_count(×tamp); /* To initialize the DMP: * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in * inv_mpu_dmp_motion_driver.h into the MPU memory. * 2. Push the gyro and accel orientation matrix to the DMP. * 3. Register gesture callbacks. Don't worry, these callbacks won't be * executed unless the corresponding feature is enabled. * 4. Call dmp_enable_feature(mask) to enable different features. * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate. * 6. Call any feature-specific control functions. * * To enable the DMP, just call mpu_set_dmp_state(1). This function can * be called repeatedly to enable and disable the DMP at runtime. * * The following is a short summary of the features supported in the DMP * image provided in inv_mpu_dmp_motion_driver.c: * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at * 200Hz. Integrating the gyro data at higher rates reduces numerical * errors (compared to integration on the MCU at a lower sampling rate). * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT. * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes. * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers * an event at the four orientations where the screen should rotate. * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of * no motion. * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO. * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO. * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot * be used in combination with DMP_FEATURE_SEND_RAW_GYRO. */ 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); /* * Known Bug - * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz * there will be a 25Hz interrupt from the MPU device. * * There is a known issue in which if you do not enable DMP_FEATURE_TAP * then the interrupts will be at 200Hz even if fifo rate * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP * * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G */ 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_UART_GET_FLAG(&UartHandle, UART_FLAG_RXNE)) { /* A byte has been received via USART. See handle_input for a list of * valid commands. */ //USART_ClearFlag(&UartHandle, USART_FLAG_RXNE); handle_input(); } get_tick_count(×tamp); #ifdef COMPASS_ENABLED /* We're not using a data ready interrupt for the compass, so we'll * make our compass reads timer-based instead. */ 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 /* Temperature data doesn't need to be read with every gyro sample. * Let's make them timer-based like the compass reads. */ if (timestamp > hal.next_temp_ms) { hal.next_temp_ms = timestamp + TEMP_READ_MS; new_temp = 1; } if (hal.motion_int_mode) { /* Enable motion interrupt. */ mpu_lp_motion_interrupt(500, 1, 5); /* Notify the MPL that contiguity was broken. */ inv_accel_was_turned_off(); inv_gyro_was_turned_off(); inv_compass_was_turned_off(); inv_quaternion_sensor_was_turned_off(); /* Wait for the MPU interrupt. */ while (!hal.new_gyro) {} /* Restore the previous sensor configuration. */ 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; /* This function gets new data from the FIFO when the DMP is in * use. The FIFO can contain any combination of gyro, accel, * quaternion, and gesture data. The sensors parameter tells the * caller which data fields were actually populated with new data. * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then * the FIFO isn't being filled with accel data. * The driver parses the gesture data to determine if a gesture * event has occurred; on an event, the application will be notified * via a callback (assuming that a callback function was properly * registered). The more parameter is non-zero if there are * leftover packets in the FIFO. */ dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more); if (!more) hal.new_gyro = 0; if (sensors & INV_XYZ_GYRO) { /* Push the new data to the MPL. */ inv_build_gyro(gyro, sensor_timestamp); new_data = 1; if (new_temp) { new_temp = 0; /* Temperature only used for gyro temp comp. */ 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; /* This function gets new data from the FIFO. The FIFO can contain * gyro, accel, both, or neither. The sensors parameter tells the * caller which data fields were actually populated with new data. * For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't * being filled with accel data. The more parameter is non-zero if * there are leftover packets in the FIFO. The HAL can use this * information to increase the frequency at which this function is * called. */ 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) { /* Push the new data to the MPL. */ inv_build_gyro(gyro, sensor_timestamp); new_data = 1; if (new_temp) { new_temp = 0; /* Temperature only used for gyro temp comp. */ 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; /* For any MPU device with an AKM on the auxiliary I2C bus, the raw * magnetometer registers are copied to special gyro registers. */ 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]; /* NOTE: If using a third-party compass calibration library, * pass in the compass data in uT * 2^16 and set the second * parameter to INV_CALIBRATED | acc, where acc is the * accuracy from 0 to 3. */ inv_build_compass(compass, 0, sensor_timestamp); } new_data = 1; } #endif if (new_data) { inv_execute_on_data(); /* This function reads bias-compensated sensor data and sensor * fusion outputs from the MPL. The outputs are formatted as seen * in eMPL_outputs.c. This function only needs to be called at the * rate requested by the host. */ read_from_mpl(); } } } /** * @brief System Clock 配置 * system Clock 配置如下: * System Clock source = PLL (HSE) * SYSCLK(Hz) = 480000000 (CPU Clock) * HCLK(Hz) = 240000000 (AXI and AHBs Clock) * AHB Prescaler = 2 * D1 APB3 Prescaler = 2 (APB3 Clock 120MHz) * D2 APB1 Prescaler = 2 (APB1 Clock 120MHz) * D2 APB2 Prescaler = 2 (APB2 Clock 120MHz) * D3 APB4 Prescaler = 2 (APB4 Clock 120MHz) * HSE Frequency(Hz) = 25000000 * PLL_M = 5 * PLL_N = 192 * PLL_P = 2 * PLL_Q = 2 * PLL_R = 2 * VDD(V) = 3.3 * Flash Latency(WS) = 4 * @param None * @retval None */ /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Supply configuration update enable */ HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); /** Configure the main internal regulator output voltage */ __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {} /** Initializes the CPU, AHB and APB busses clocks */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLM = 5; RCC_OscInitStruct.PLL.PLLN = 192; RCC_OscInitStruct.PLL.PLLP = 2; RCC_OscInitStruct.PLL.PLLQ = 2; RCC_OscInitStruct.PLL.PLLR = 2; RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2; RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; RCC_OscInitStruct.PLL.PLLFRACN = 0; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { } /** Initializes the CPU, AHB and APB busses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2 |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) { } } /*********************************************END OF FILE**********************/