电磁直立车程序

/* $License: Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. See included License.txt for License information. $ */ /** * @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 "include.h" //******************************************* //********************************************* #define MPU6050 //?“辰??辰??那1車?米?∩??D?‾?aMPU6050 #define MOTION_DRIVER_TARGET_MSP430 //?“辰??y?‘2?﹞?,2谷車?MSP430米??y?‘(辰??2米?STM32F1) /* 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) */ #if defined MOTION_DRIVER_TARGET_MSP430 //#include "msp430.h" //#include "msp430_i2c.h" //#include "msp430_clock.h" //#include "msp430_interrupt.h" #define i2c_write MPU_Write_Len #define i2c_read MPU_Read_Len #define delay_ms delay_ms #define get_ms mget_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 printf //∩辰車?D??⊿ #define log_e printf //∩辰車?D??⊿ /* 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 register. */ unsigned char lpf; unsigned char clk_src; /* Sample rate, NOT rate di