STM32F4 HAL库 MPU6050 软件I2C DMP库读取，可获得稳定偏航角yaw
资源内容介绍
STM32F4 HAL库 MPU6050 软件I2C DMP库读取，可获得稳定偏航角yaw /** * @addtogroup DRIVERS Sensor Driver Layer * @brief Hardware drivers to communicate with sensors via I2C. * * @{ * @file inv_mpu.c * @brief An I2C-based driver for Invensense gyroscopes. * @details This driver currently works for the following devices: * MPU6050 * MPU6500 * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) */#include "main.h"/* The following functions must be defined for this platform: * i2c_write(unsigned char slave_addr, unsigned char reg_addr, * unsigned char length, unsigned char const *data) * i2c_read(unsigned char slave_addr, unsigned char reg_addr, * unsigned char length, unsigned char *data) * delay_ms(unsigned long num_ms) * get_ms(unsigned long *count) * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) * labs(long x) * fabsf(float x) * min(int a, int b) */#define MPU6050#define MOTION_DRIVER_TARGET_MSP430#if defined MOTION_DRIVER_TARGET_MSP430#define i2c_write i2cWrite#define i2c_read i2cRead#define delay_ms delay_ms#define get_ms myget_ms//static int reg_int_cb(struct int_param_s *int_param)//{// //return 0;//} //#define log_i(...) do {} while (0)//#define log_e(...) do {} while (0)#define log_e printf#define log_i printf#define fabs fabsf#define min(a,b) ((a<b)?a:b)#elif defined EMPL_TARGET_MSP430#include "msp430.h"#include "msp430_i2c.h"#include "msp430_clock.h"#include "msp430_interrupt.h"#include "log.h"static inline int reg_int_cb(struct int_param_s *int_param){ return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, int_param->active_low);}#define log_i MPL_LOGI#define log_e MPL_LOGE#define fabs fabsf#define min(a,b) ((a<b)?a:b)#elif defined EMPL_TARGET_UC3L0/* Instead of using the standard TWI driver from the ASF library, we're using * a TWI driver that follows the slave address + register address convention. */#include "twi.h"#include "delay.h"#include "sysclk.h"#include "log.h"#include "sensors_xplained.h"#include "uc3l0_clock.h"#define i2c_write(a, b, c, d) twi_write(a, b, d, c)#define i2c_read(a, b, c, d) twi_read(a, b, d, c)#define get_ms uc3l0_get_clock_msstatic inline int reg_int_cb(struct int_param_s *int_param){ sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg); return 0;}#define log_i MPL_LOGI#define log_e MPL_LOGE#define labs abs#define fabs(x) (((x)>0)?(x):-(x))#else//#error Gyro driver is missing the system layer implementations.#endif#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250//#error Which gyro are you using? Define MPUxxxx in your compiler options.#endif/* Time for some messy macro work. =] * #define MPU9150 * is equivalent to.. * #define MPU6050 * #define AK8975_SECONDARY * * #define MPU9250 * is equivalent to.. * #define MPU6500 * #define AK8963_SECONDARY */#if defined MPU9150#ifndef MPU6050#define MPU6050#endif #if defined AK8963_SECONDARY#error "MPU9150 and AK8963_SECONDARY cannot both be defined."#elif !defined AK8975_SECONDARY #define AK8975_SECONDARY#endif #elif defined MPU9250 #ifndef MPU6500#define MPU6500#endif #if defined AK8975_SECONDARY#error "MPU9250 and AK8975_SECONDARY cannot both be defined."#elif !defined AK8963_SECONDARY #define AK8963_SECONDARY#endif #endif #if defined AK8975_SECONDARY || defined AK8963_SECONDARY#define AK89xx_SECONDARY#else#endifstatic int set_int_enable(unsigned char enable);struct gyro_reg_s { unsigned char who_am_i; unsigned char rate_div; unsigned char lpf; unsigned char prod_id; unsigned char user_ctrl; unsigned char fifo_en; unsigned char gyro_cfg; unsigned char accel_cfg; //unsigned char accel_cfg2; //unsigned char lp_accel_odr; unsigned char motion_thr; unsigned char motion_dur; unsigned char fifo_count_h; unsigned char fifo_r_w; unsigned char raw_gyro; unsigned char raw_accel; unsigned char temp; unsigned char int_enable; unsigned char dmp_int_status; unsigned char int_status; //unsigned char accel_intel; unsigned char pwr_mgmt_1; unsigned char pwr_mgmt_2; unsigned char int_pin_cfg; unsigned char mem_r_w; unsigned char accel_offs; unsigned char i2c_mst; unsigned char bank_sel; unsigned char mem_start_addr; unsigned char prgm_start_h;#if defined AK89xx_SECONDARY unsigned char s0_addr; unsigned char s0_reg; unsigned char s0_ctrl; unsigned char s1_addr; unsigned char s1_reg; unsigned char s1_ctrl; unsigned char s4_ctrl; unsigned char s0_do; unsigned char s1_do; unsigned char i2c_delay_ctrl; unsigned char raw_compass; unsigned char yg_offs_tc;#endif};struct hw_s { unsigned char addr; unsigned short max_fifo; unsigned char num_reg; unsigned short temp_sens; short temp_offset; unsigned short bank_size;#if defined AK89xx_SECONDARY unsigned short compass_fsr;#endif};/* When entering motion interrupt mode, the driver keeps track of the * previous state so that it can be restored at a later time. * TODO: This is tacky. Fix it. */struct motion_int_cache_s { unsigned short gyro_fsr; unsigned char accel_fsr; unsigned short lpf; unsigned short sample_rate; unsigned char sensors_on; unsigned char fifo_sensors; unsigned char dmp_on;};/* Cached chip configuration data. * TODO: A lot of these can be handled with a bitmask. */struct chip_cfg_s { unsigned char gyro_fsr; unsigned char accel_fsr; unsigned char sensors; unsigned char lpf; unsigned char clk_src; unsigned short sample_rate; unsigned char fifo_enable; unsigned char int_enable; unsigned char bypass_mode; /* 1 if half-sensitivity. * NOTE: This doesn't belong here, but everything else in hw_s is const, * and this allows us to save some precious RAM. */ unsigned char accel_half; /* 1 if device in low-power acce