1285 lines
42 KiB
C
1285 lines
42 KiB
C
/**
|
||
******************************************************************************
|
||
* @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 <stdlib.h>
|
||
#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**********************/
|
||
|