小学生
最后登录1970-1-1
在线时间 小时
注册时间2021-9-20
|
#include "stm32f10x.h"
#include "stm32f10x_it.h"
#include "led.h"
#include "key.h"
#include "delay.h"
#include "timer.h"
#include "usart.h"
#include "i2c.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "cJSON.h"
#include "rtc.h"
#include "spi_flash.h"
#include "SysTick.h"
#include "exti.h"
//设置是否使用LCD进行显示，不需要的话把这个宏注释掉即可
//#define USE_LCD_DISPLAY
#ifdef USE_LCD_DISPLAY
#include "ili9341_lcd.h"
#endif
#include "mltypes.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "log.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mpu.h"
#include "inv_mpu.h"
#include "log.h"
#include "packet.h"
#define TASK_ENABLE 0
extern unsigned int Task_Delay[NumOfTask];
/* 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};
/* 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.
*/
/* 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
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* ---------------------------------------------------------------------------*/
#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)
{
while(USART_GetFlagStatus(USART_DEBUG,USART_FLAG_TXE)==RESET); //循环发送,直到发送完毕
USART_SendData(USART_DEBUG,c);
}
/*函数功能：根据匿名最新上位机协议写的显示姿态的程序（上位机0512版本）
*具体协议说明请查看上位机软件的帮助说明。
*/
void Data_Send_Status(float Pitch,float Roll,float Yaw)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
unsigned int _temp;
u8 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;
data_to_send[_cnt++]=sum;
//串口发送数据
for(i=0;i<_cnt;i++)
usart_send_char(data_to_send);
}
/*函数功能：根据匿名最新上位机协议写的显示传感器数据（上位机0512版本）
*具体协议说明请查看上位机软件的帮助说明。
*/
void Send_Data(int16_t *Gyro,int16_t *Accel)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
// unsigned int _temp;
u8 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;
data_to_send[_cnt++]=sum;
//串口发送数据
for(i=0;i<_cnt;i++)
usart_send_char(data_to_send);
}
extern struct inv_sensor_cal_t sensors;
/* 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};
MPU_DEBUG_FUNC();
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)
{
#ifdef USE_LCD_DISPLAY
char cStr [ 70 ];
#endif
unsigned long timestamp,step_count,walk_time;
/*获取欧拉角*/
if (inv_get_sensor_type_euler(data, &accuracy,(inv_time_t*)×tamp))
{
float Pitch,Roll,Yaw;
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);
#ifdef USE_LCD_DISPLAY
sprintf ( cStr, "Pitch : %.4f ",Pitch ); //inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
ILI9341_DispString_EN(30,90,cStr);
sprintf ( cStr, "Roll : %.4f ", Roll ); //inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
ILI9341_DispString_EN(30,110,cStr);
sprintf ( cStr, "Yaw : %.4f ", Yaw ); //inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
ILI9341_DispString_EN(30,130,cStr);
/*温度*/
mpu_get_temperature(data,(inv_time_t*)×tamp);
sprintf ( cStr, "Temperature : %.2f ", data[0]*1.0/(1<<16) ); //inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
ILI9341_DispString_EN(30,150,cStr);
#endif
}
/*获取步数*/
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);
#ifdef USE_LCD_DISPLAY
sUsartPrintf(USART_DEBUG,cStr, "Walked steps : %ld steps over %ld milliseconds..",step_count,walk_time);
ILI9341_DispString_EN(0,180,cStr);
#endif
}
}
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;
MPU_DEBUG_FUNC();
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)
{
MPU_DEBUG_FUNC();
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)
{
MPU_DEBUG_FUNC();
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];
MPU_DEBUG_FUNC();
#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 = (long)(gyro * 32.8f); //convert to +-1000dps
accel *= 2048.f; //convert to +-16G
accel = accel >> 16;
gyro = (long)(gyro >> 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 = USART_ReceiveData(USART_DEBUG);
MPU_DEBUG_FUNC();
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;
}
void gyro_data_ready_cb(void)
{
hal.new_gyro = 1;
}
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;
LED_Init(); //LED初始化
KEY_Init(); //按键初始化
delay_init(); //延时函数初始化
RTC_Init(); //RTC时钟初始化
delay_ms(500);
Usart1_Init(115200);//115200波特率 debug串口
Usart2_Init(115200);//115200波特率 与JAVA通讯串口
Usart3_Init(115200);
TIM2_Int_Init(4999,7199);
TIM3_Int_Init(2499,7199);
#ifdef USE_LCD_DISPLAY
ILI9341_Init (); //LCD 初始化
ILI9341_GramScan ( 6 );
LCD_SetFont(&Font8x16);
LCD_SetColors(RED,BLACK);
#endif
//初始化systick
SysTick_Init();
SysTick->CTRL|=SysTick_CTRL_ENABLE_Msk;
//MPU6050中断引脚
EXTI_Pxy_Config();
//I2C初始化
I2C_Bus_Init();
#ifdef USE_LCD_DISPLAY
ILI9341_Clear ( 0, 0, 240, 320);
ILI9341_DispString_EN ( 20, 20, "This is a MPU6050 demo" );
#endif
UsartPrintf(USART_DEBUG,"mpu 6050 test start");
result = mpu_init(&int_param);
if (result) {
LED0 = 0;
MPL_LOGE("Could not initialize gyro.result = %d\n",result);
#ifdef USE_LCD_DISPLAY
LCD_SetColors(BLUE,BLACK);
ILI9341_DispString_EN ( 0, 40, "No MPU6050 detceted!Please check the hardware connection.");
#endif
}
else
{
LED1 = 0;
#ifdef USE_LCD_DISPLAY
ILI9341_DispString_EN(30,40,"MPU6050 decteted!");
#endif
}
/* 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 (USART_GetFlagStatus (USART_DEBUG, USART_FLAG_RXNE)) {
/* A byte has been received via USART. See handle_input for a list of
* valid commands.
*/
USART_ClearFlag(USART_DEBUG, 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();
}
}
}
|
|
发表于 2021-9-22 21:44:01
提升卡
发表于 2021-9-23 08:45:24
楼主