自主巡检平衡车固件源代码

/************************************************************************** 作者：Mini Balance 淘宝店铺：http://shop114407458.taobao.com/ **************************************************************************/ /** * @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 <stdio.h> #include <stdint.h> #include <stdlib.h> #include <string.h> #include <math.h> #include "M451Series.h" #include "IOI2C.h" #include "dmpkey.h" #include "inv_mpu_dmp_motion_driver.h" #include "inv_mpu.h" #include "dmpmap.h" #include "SysTick.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 /*#include "msp430.h" #include "msp430_i2c.h" #include "msp430_clock.h" #include "msp430_interrupt.h" */ #define i2c_write i2cWrite #define i2c_read i2cRead #define delay_ms delay_ms #define get_ms get_ms //static 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);*/ // return 0; //} //#define log_i(...) do {} while (0) //#define log_e(...) do {} while (0) #define log_e printf #define log_i printf /* labs is already defined by TI's toolchain. */ /* fabs is for doubles. fabsf is for floats. */ #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" #define i2c_write msp430_i2c_write #define i2c_read msp430_i2c_read #define delay_ms msp430_delay_ms #define get_ms msp430_get_clock_ms*/ 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 /* labs is already defined by TI's toolchain. */ /* fabs is for doubles. fabsf is for floats. */ #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) /* delay_ms is a function already defined in ASF. */ #define get_ms uc3l0_get_clock_ms static 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 /* UC3 is a 32-bit processor, so abs and labs are equivalent. */ #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 /* #ifndef MPU6050 */ #if defined AK8963_SECONDARY #error "MPU9150 and AK8963_SECONDARY cannot both be defined." #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */ #define AK8975_SECONDARY #endif /* #if defined AK8963_SECONDARY */ #elif defined MPU9250 /* #if defined MPU9150 */ #ifndef MPU6500 #define MPU6500 #endif /* #ifndef MPU6500 */ #if defined AK8975_SECONDARY #error "MPU9250 and AK8975_SECONDARY cannot both be defined." #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */ #define AK8963_SECONDARY #endif /* #if defined AK8975_SECONDARY */ #endif /* #if defined MPU9150 */ #if defined AK8975_SECONDARY || defined AK8963_SECONDARY #define AK89xx_SECONDARY #else /* #warning "No compass = less profit for Invensense. Lame." */ #endif static int set_int_enable(unsigned char enable); /* Hardware registers needed by driver. */ 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; /* The I2C_MST_VDDIO bit is in this register. */ unsigned char yg_offs_tc; #endif }; /* Information specific to a particular device. */ 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 { /* Matches gyro_cfg >> 3 & 0x03 */ unsigned char gyro_fsr; /* Matches accel_cfg >> 3 & 0x03 */ unsigned char accel_fsr; /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ unsigned char sensors; /* Matches config regi