mpu6050的DMP移植实例

// I2Cdev library collection - MPU6050 I2C device class header file // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) // 10/3/2011 by Jeff Rowberg <jeff@rowberg.net> // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: // ... - ongoing debug release // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE // DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF // YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. /* ChibiOS I2Cdev MPU6050 device class conversion 2/5/2013 by Jan Schlemminger - C conversion, ChibiOS compliance * First release. I just tested a few functions so this should be considered HIGHLY EXPERIMENTAL!!! * Feel free to test and report bugs. Updates at https://github.com/jevermeister/MPU6050-ChibiOS */ /* ============================================ ChibiOS I2Cdev MPU6050 device class code is placed under the MIT license Copyright (c) 2012 Jan Schlemminger Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. =============================================== */ #include "ch.h" #include "hal.h" #include "MPU6050.h" #include "i2cdev_chibi.h" #include "chprintf.h" // for memcmp #include <string.h> /* Global variables, will see how to get rid of them * */ uint8_t MPUdevAddr; uint8_t MPUbuffer[14]; uint16_t MPUfifoCount; // count of all bytes currently in FIFO uint8_t MPUfifoBuffer[64]; // FIFO storage buffer /** Default constructor, uses default I2C address. * @see MPU6050_DEFAULT_ADDRESS */ /*MPU6050::MPU6050() { devAddr = MPU6050_DEFAULT_ADDRESS; }*/ /** Specific address constructor. * @param address I2C address * @see MPU6050_DEFAULT_ADDRESS * @see MPU6050_ADDRESS_AD0_LOW * @see MPU6050_ADDRESS_AD0_HIGH */ void MPU6050(uint8_t address) { MPUdevAddr = address; } /** Power on and prepare for general usage. * This will activate the device and take it out of sleep mode (which must be done * after start-up). This function also sets both the accelerometer and the gyroscope * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets * the clock source to use the X Gyro for reference, which is slightly better than * the default internal clock source. */ void MPUinitialize() { MPUsetClockSource(MPU6050_CLOCK_PLL_XGYRO); MPUsetFullScaleGyroRange(MPU6050_GYRO_FS_250); MPUsetFullScaleAccelRange(MPU6050_ACCEL_FS_2); MPUsetSleepEnabled(FALSE); // thanks to Jack Elston for pointing this one out! } /** Verify the I2C connection. * Make sure the device is connected and responds as expected. * @return TRUE if connection is valid, FALSE otherwise */ bool_t MPUtestConnection() { return MPUgetDeviceID() == 0x34; } // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC) /** Get the auxiliary I2C supply voltage level. * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to * the MPU-6000, which does not have a VLOGIC pin. * @return I2C supply voltage level (0=VLOGIC, 1=VDD) */ uint8_t MPUgetAuxVDDIOLevel() { I2CdevreadBit(MPUdevAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, MPUbuffer, I2CDEV_DEFAULT_READ_TIMEOUT); return MPUbuffer[0]; } /** Set the auxiliary I2C supply voltage level. * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to * the MPU-6000, which does not have a VLOGIC pin. * @param level I2C supply voltage level (0=VLOGIC, 1=VDD) */ void MPUsetAuxVDDIOLevel(uint8_t level) { I2CdevwriteBit(MPUdevAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level); } // SMPLRT_DIV register /** Get gyroscope output rate divider. * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero * Motion detection, and Free Fall detection are all based on the Sample Rate. * The Sample Rate is generated by dividing the gyroscope output rate by * SMPLRT_DIV: * * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) * * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or * 7), and 1kHz when the DLPF is enabled (see Register 26). * * Note: The accelerometer output rate is 1kHz. This means that for a Sample * Rate greater than 1kHz, the same accelerometer sample may be output to the * FIFO, DMP, and sensor registers more than once. * * For a diagram of the gyroscope and accelerometer signal paths, see Section 8 * of the MPU-6000/MPU-6050 Product Specification document. * * @return Current sample rate * @see MPU6050_RA_SMPLRT_DIV */ uint8_t MPUgetRate() { I2CdevreadByte(MPUdevAddr, MPU6050_RA_SMPLRT_DIV, MPUbuffer, I2CDEV_DEFAULT_READ_TIMEOUT); return MPUbuffer[0]; } /** Set gyroscope sample rate divider. * @param rate New sample rate divider * @see getRate() * @see MPU6050_RA_SMPLRT_DIV */ void MPUsetRate(uint8_t rate) { I2CdevwriteByte(MPUdevAddr, MPU6050_RA_SMPLRT_DIV, rate); } // CONFIG register /** Get external FSYNC configuration. * Configures the external Frame Synchronization (FSYNC) pin sampling. An * external signal connected to the FSYNC pin can be sampled by configuring * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short * strobes may be captured. The latched FSYNC signal will be sampled at the * Sampling Rate, as defined in register 25. After sampling, the latch will * reset to the current FSYNC signal state. * * The sampled value will be reported in place of the least significant bit in * a sensor data register determined by the value of EXT_SYNC_SET according to * the following table. * * <pre> * EXT_SYNC_SET | FSYNC Bit Location * -------------+------------------- * 0 | Input disabled * 1 | TEMP_OUT_L[0] * 2 | GYRO_XOUT_L[0] * 3 | GYRO_YOUT_L[0] * 4 | GYRO_ZOUT_L[0] * 5 | ACCEL_XOUT_L[0] * 6 | ACCEL_YOUT_L[0] * 7 | ACCEL_ZOUT_L[0] * </pre> * * @return FSYNC configuration value */ uint8_t MPUgetExternalFrameSync() { I2CdevreadBits(MPUdevAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPUbuffer, I2CDEV_DEFAULT_READ_TIMEOUT); return MPUbuffer[0]; } /** Set external FSYNC configuration. * @see getExternalFrameSync() * @see MPU6050_RA_CONFIG * @param sync New FSYNC configuration value */ void MPUsetExternalFrameSync(uint8_t sync) { I2CdevwriteBits(MPUdevAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync); } /** Get digital low-pass filter configuration. * T