SparkFunMPU9250-DMP.h

00001 /******************************************************************************
00002 SparkFunMPU9250-DMP.h - MPU-9250 Digital Motion Processor Arduino Library 
00003 Jim Lindblom @ SparkFun Electronics
00004 original creation date: November 23, 2016
00005 https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library
00006 
00007 This library implements motion processing functions of Invensense's MPU-9250.
00008 It is based on their Emedded MotionDriver 6.12 library.
00009     https://www.invensense.com/developers/software-downloads/
00010 
00011 ******************************************************************************/
00012 #ifndef _SPARKFUN_MPU9250_DMP_H_
00013 #define _SPARKFUN_MPU9250_DMP_H_
00014 
00015 #include <mdcompat.h>
00016 #include <mbed.h>
00017 
00018 // Optimally, these defines would be passed as compiler options, but Arduino
00019 // doesn't give us a great way to do that.
00020 #ifndef MPU9250
00021 #define MPU9250
00022 #endif
00023 #define AK8963_SECONDARY
00024 #define COMPASS_ENABLED
00025 
00026 // Include the Invensense MPU9250 driver and DMP keys:
00027 extern "C" {
00028 #include "inv_mpu.h"
00029 #include "inv_mpu_dmp_motion_driver.h"
00030 }
00031 
00032 typedef int inv_error_t;
00033 #define INV_SUCCESS 0
00034 #define INV_ERROR 0x20
00035 
00036 enum t_axisOrder {
00037     X_AXIS, // 0
00038     Y_AXIS, // 1
00039     Z_AXIS  // 2
00040 };
00041 
00042 // Define's passed to update(), to request a specific sensor (or multiple):
00043 #define UPDATE_ACCEL   (1<<1)
00044 #define UPDATE_GYRO    (1<<2)
00045 #define UPDATE_COMPASS (1<<3)
00046 #define UPDATE_TEMP    (1<<4)
00047 
00048 #define INT_ACTIVE_HIGH 0
00049 #define INT_ACTIVE_LOW  1
00050 #define INT_LATCHED     1
00051 #define INT_50US_PULSE  0
00052 
00053 #define MAX_DMP_SAMPLE_RATE 200 // Maximum sample rate for the DMP FIFO (200Hz)
00054 #define FIFO_BUFFER_SIZE 512 // Max FIFO buffer size
00055 
00056 const signed char defaultOrientation[9] = {
00057     1, 0, 0,
00058     0, 1, 0,
00059     0, 0, 1
00060 };
00061 #define ORIENT_PORTRAIT          0
00062 #define ORIENT_LANDSCAPE         1
00063 #define ORIENT_REVERSE_PORTRAIT  2
00064 #define ORIENT_REVERSE_LANDSCAPE 3
00065 
00066 class MPU9250_DMP 
00067 {
00068 public:
00069     int ax, ay, az;
00070     int gx, gy, gz;
00071     int mx, my, mz;
00072     long qw, qx, qy, qz;
00073     long temperature;
00074     unsigned long time;
00075     float pitch, roll, yaw;
00076     float heading;
00077     
00078     MPU9250_DMP();
00079     
00080     // begin(void) -- Verifies communication with the MPU-9250 and the AK8963,
00081     // and initializes them to the default state:
00082     // All sensors enabled
00083     // Gyro FSR: +/- 2000 dps
00084     // Accel FSR: +/- 2g
00085     // LPF: 42 Hz
00086     // FIFO: 50 Hz, disabled
00087     // Output: INV_SUCCESS (0) on success, otherwise error
00088     inv_error_t begin(void);
00089     
00090     // setSensors(unsigned char) -- Turn on or off MPU-9250 sensors. Any of the 
00091     // following defines can be combined: INV_XYZ_GYRO, INV_XYZ_ACCEL, 
00092     // INV_XYZ_COMPASS, INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO
00093     // Input: Combination of enabled sensors. Unless specified a sensor will be
00094     //  disabled.
00095     // Output: INV_SUCCESS (0) on success, otherwise error
00096     inv_error_t setSensors(unsigned char sensors);
00097     
00098     // setGyroFSR(unsigned short) -- Sets the full-scale range of the gyroscope
00099     // Input: Gyro DPS - 250, 500, 1000, or 2000
00100     // Output: INV_SUCCESS (0) on success, otherwise error
00101     inv_error_t setGyroFSR(unsigned short fsr);
00102     // getGyroFSR -- Returns the current gyroscope FSR
00103     // Output: Current Gyro DPS - 250, 500, 1000, or 2000
00104     unsigned short getGyroFSR(void);
00105     // getGyroSens -- Returns current gyroscope sensitivity. The FSR divided by
00106     // the resolution of the sensor (signed 16-bit).
00107     // Output: Currently set gyroscope sensitivity (e.g. 131, 65.5, 32.8, 16.4)
00108     float getGyroSens(void);
00109     
00110     // setAccelFSR(unsigned short) -- Sets the FSR of the accelerometer
00111     // 
00112     // Input: Accel g range - 2, 4, 8, or 16
00113     // Output: INV_SUCCESS (0) on success, otherwise error
00114     inv_error_t setAccelFSR(unsigned char fsr);
00115     // getAccelFSR -- Returns the current accelerometer FSR
00116     // Output: Current Accel g - 2, 4, 8, or 16
00117     unsigned char getAccelFSR(void);
00118     // getAccelSens -- Returns current accelerometer sensitivity. The FSR 
00119     // divided by the resolution of the sensor (signed 16-bit).
00120     // Output: Currently set accel sensitivity (e.g. 16384, 8192, 4096, 2048)
00121     unsigned short getAccelSens(void);
00122     
00123     // getMagFSR -- Returns the current magnetometer FSR
00124     // Output: Current mag uT range - +/-1450 uT
00125     unsigned short getMagFSR(void);
00126     // getMagSens -- Returns current magnetometer sensitivity. The FSR 
00127     // divided by the resolution of the sensor (signed 16-bit).
00128     // Output: Currently set mag sensitivity (e.g. 0.15)
00129     float getMagSens(void);
00130     
00131     // setLPF -- Sets the digital low-pass filter of the accel and gyro.
00132     // Can be any of the following: 188, 98, 42, 20, 10, 5 (value in Hz)
00133     // Input: 188, 98, 42, 20, 10, or 5 (defaults to 5 if incorrectly set)
00134     // Output: INV_SUCCESS (0) on success, otherwise error
00135     inv_error_t setLPF(unsigned short lpf);
00136     // getLPF -- Returns the set value of the LPF.
00137     //
00138     // Output: 5, 10, 20, 42, 98, or 188 if set. 0 if the LPF is disabled.
00139     unsigned short getLPF(void);
00140     
00141     // setSampleRate -- Set the gyroscope and accelerometer sample rate to a 
00142     // value between 4Hz and 1000Hz (1kHz).
00143     // The library will make an attempt to get as close as possible to the
00144     // requested sample rate.
00145     // Input: Value between 4 and 1000, indicating the desired sample rate
00146     // Output: INV_SUCCESS (0) on success, otherwise error
00147     inv_error_t setSampleRate(unsigned short rate);
00148     // getSampleRate -- Get the currently set sample rate.
00149     // May differ slightly from what was set in setSampleRate.
00150     // Output: set sample rate of the accel/gyro. A value between 4-1000.
00151     unsigned short getSampleRate(void);
00152     
00153     // setCompassSampleRate -- Set the magnetometer sample rate to a value
00154     // between 1Hz and 100 Hz.
00155     // The library will make an attempt to get as close as possible to the
00156     // requested sample rate.
00157     // Input: Value between 1 and 100, indicating the desired sample rate
00158     // Output: INV_SUCCESS (0) on success, otherwise error
00159     inv_error_t setCompassSampleRate(unsigned short rate);
00160     // getCompassSampleRate -- Get the currently set magnetometer sample rate.
00161     // May differ slightly from what was set in setCompassSampleRate.
00162     //
00163     // Output: set sample rate of the magnetometer. A value between 1-100
00164     unsigned short getCompassSampleRate(void);
00165     
00166     // dataReady -- checks to see if new accel/gyro data is available.
00167     // (New magnetometer data cannot be checked, as the library runs that sensor 
00168     //  in single-conversion mode.)
00169     // Output: true if new accel/gyro data is available
00170     bool dataReady();
00171     
00172     // update -- Reads latest data from the MPU-9250's data registers.
00173     // Sensors to be updated can be set using the [sensors] parameter.
00174     // [sensors] can be any combination of UPDATE_ACCEL, UPDATE_GYRO,
00175     // UPDATE_COMPASS, and UPDATE_TEMP.
00176     // Output: INV_SUCCESS (0) on success, otherwise error
00177     // Note: after a successful update the public sensor variables 
00178     // (e.g. ax, ay, az, gx, gy, gz) will be updated with new data 
00179     inv_error_t update(unsigned char sensors = 
00180                        UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
00181     
00182     // updateAccel, updateGyro, updateCompass, and updateTemperature are 
00183     // called by the update() public method. They read from their respective
00184     // sensor and update the class variable (e.g. ax, ay, az)
00185     // Output: INV_SUCCESS (0) on success, otherwise error
00186     inv_error_t updateAccel(void);
00187     inv_error_t updateGyro(void);
00188     inv_error_t updateCompass(void);
00189     inv_error_t updateTemperature(void);
00190     
00191     // configureFifo(unsigned char) -- Initialize the FIFO, set it to read from
00192     // a select set of sensors.
00193     // Any of the following defines can be combined for the [sensors] parameter:
00194     // INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO
00195     // Input: Combination of sensors to be read into FIFO
00196     // Output: INV_SUCCESS (0) on success, otherwise error
00197     inv_error_t configureFifo(unsigned char sensors);
00198     // getFifoConfig -- Returns the sensors configured to be read into the FIFO
00199     // Output: combination of INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_Y_GYRO,
00200     //         INV_X_GYRO, or INV_Z_GYRO
00201     unsigned char getFifoConfig(void);
00202     // fifoAvailable -- Returns the number of bytes currently filled in the FIFO
00203     // Outputs: Number of bytes filled in the FIFO (up to 512)
00204     unsigned short fifoAvailable(void);
00205     // updateFifo -- Reads from the top of the FIFO, and stores the new data
00206     // in ax, ay, az, gx, gy, or gz (depending on how the FIFO is configured).
00207     // Output: INV_SUCCESS (0) on success, otherwise error
00208     inv_error_t updateFifo(void);
00209     // resetFifo -- Resets the FIFO's read/write pointers
00210     // Output: INV_SUCCESS (0) on success, otherwise error
00211     inv_error_t resetFifo(void);
00212     
00213     // enableInterrupt -- Configure the MPU-9250's interrupt output to indicate
00214     // when new data is ready.
00215     // Input: 0 to disable, >=1 to enable
00216     // Output: INV_SUCCESS (0) on success, otherwise error
00217     inv_error_t enableInterrupt(unsigned char enable = 1);
00218     // setIntLevel -- Configure the MPU-9250's interrupt to be either active-
00219     // high or active-low.
00220     // Input: 0 for active-high, 1 for active-low
00221     // Output: INV_SUCCESS (0) on success, otherwise error
00222     inv_error_t setIntLevel(unsigned char active_low);
00223     // setIntLatched -- Configure the MPU-9250's interrupt to latch or operate
00224     // as a 50us pulse.
00225     // Input: 0 for 
00226     // Output: INV_SUCCESS (0) on success, otherwise error
00227     inv_error_t setIntLatched(unsigned char enable);
00228     // getIntStatus -- Reads the MPU-9250's INT_STATUS register, which can
00229     // indicate what (if anything) caused an interrupt (e.g. FIFO overflow or
00230     // or data read).
00231     // Output: contents of the INT_STATUS register
00232     short getIntStatus(void);
00233     
00234     // dmpBegin -- Initialize the DMP, enable one or more features, and set the FIFO's sample rate
00235     // features can be any one of 
00236     // DMP_FEATURE_TAP -- Tap detection
00237     // DMP_FEATURE_ANDROID_ORIENT -- Orientation (portrait/landscape) detection
00238     // DMP_FEATURE_LP_QUAT -- Accelerometer, low-power quaternion calculation
00239     // DMP_FEATURE_PEDOMETER -- Pedometer (always enabled)
00240     // DMP_FEATURE_6X_LP_QUAT -- 6-axis (accel/gyro) quaternion calculation
00241     // DMP_FEATURE_GYRO_CAL -- Gyroscope calibration (0's out after 8 seconds of no motion)
00242     // DMP_FEATURE_SEND_RAW_ACCEL -- Send raw accelerometer values to FIFO
00243     // DMP_FEATURE_SEND_RAW_GYRO -- Send raw gyroscope values to FIFO
00244     // DMP_FEATURE_SEND_CAL_GYRO -- Send calibrated gyroscop values to FIFO
00245     // fifoRate can be anywhere between 4 and 200Hz.
00246     // Input: OR'd list of features and requested FIFO sampling rate
00247     // Output: INV_SUCCESS (0) on success, otherwise error
00248     inv_error_t dmpBegin(unsigned short features = 0, unsigned short fifoRate = MAX_DMP_SAMPLE_RATE);
00249     
00250     // dmpLoad -- Loads the DMP with 3062-byte image memory. Must be called to begin DMP.
00251     // This function is called by the dmpBegin function.
00252     // Output: INV_SUCCESS (0) on success, otherwise error
00253     inv_error_t dmpLoad(void);
00254     
00255     // dmpGetFifoRate -- Returns the sample rate of the FIFO
00256     // Output: Set sample rate, in Hz, of the FIFO
00257     unsigned short dmpGetFifoRate(void);
00258     // dmpSetFiFoRate -- Sets the rate of the FIFO.
00259     // Input: Requested sample rate in Hz (range: 4-200)
00260     // Output: INV_SUCCESS (0) on success, otherwise error
00261     inv_error_t dmpSetFifoRate(unsigned short rate);
00262     
00263     // dmpUpdateFifo -- Reads from the top of the FIFO and fills accelerometer, gyroscope,
00264     // quaternion, and time public variables (depending on how the DMP is configured).
00265     // Should be called whenever an MPU interrupt is detected
00266     // Output: INV_SUCCESS (0) on success, otherwise error
00267     inv_error_t dmpUpdateFifo(void); 
00268     
00269     // dmpEnableFeatures -- Enable one, or multiple DMP features.
00270     // Input: An OR'd list of features (see dmpBegin)
00271     // Output: INV_SUCCESS (0) on success, otherwise error
00272     inv_error_t dmpEnableFeatures(unsigned short mask);
00273     // dmpGetEnabledFeatures -- Returns the OR'd list of enabled DMP features
00274     //
00275     // Output: OR'd list of DMP feature's (see dmpBegin for list)
00276     unsigned short dmpGetEnabledFeatures(void);
00277     
00278     // dmpSetTap -- Enable tap detection and configure threshold, tap time, and minimum tap count.
00279     // Inputs: x/y/zThresh - accelerometer threshold on each axis. Range: 0 to 1600. 0 disables tap
00280     //                       detection on that axis. Units are mg/ms.
00281     //         taps - minimum number of taps to create a tap event (Range: 1-4)
00282     //         tapTime - Minimum number of milliseconds between separate taps
00283     //         tapMulti - Maximum number of milliseconds combined taps
00284     // Output: INV_SUCCESS (0) on success, otherwise error
00285     inv_error_t dmpSetTap(unsigned short xThresh = 250, 
00286                           unsigned short yThresh = 250, 
00287                           unsigned short zThresh = 250,
00288                           unsigned char taps = 1, 
00289                           unsigned short tapTime = 100,
00290                           unsigned short tapMulti = 500);
00291     // tapAvailable -- Returns true if a new tap is available
00292     // Output: True if new tap data is available. Cleared on getTapDir or getTapCount.
00293     bool tapAvailable(void);
00294     // getTapDir -- Returns the tap direction.
00295     // Output: One of the following: TAP_X_UP, TAP_X_DOWN, TAP_Y_UP, TAP_Y_DOWN, TAP_Z_UP,
00296     //         or TAP_Z_DOWN
00297     unsigned char getTapDir(void);
00298     // getTapCount -- Returns the number of taps in the sensed direction
00299     // Output: Value between 1-8 indicating successive number of taps sensed.
00300     unsigned char getTapCount(void);
00301 
00302     // dmpSetOrientation -- Set orientation matrix, used for orientation sensing.
00303     // Use defaultOrientation matrix as an example input.
00304     // Input: Gyro and accel orientation in body frame (9-byte array)
00305     // Output: INV_SUCCESS (0) on success, otherwise error
00306     inv_error_t dmpSetOrientation(const signed char * orientationMatrix = defaultOrientation);
00307     // dmpGetOrientation -- Get the orientation, if any.
00308     // Output: If an orientation is detected, one of ORIENT_LANDSCAPE, ORIENT_PORTRAIT,
00309     //         ORIENT_REVERSE_LANDSCAPE, or ORIENT_REVERSE_PORTRAIT.
00310     unsigned char dmpGetOrientation(void);
00311     
00312     // dmpEnable3Quat -- Enable 3-axis quaternion calculation
00313     // Output: INV_SUCCESS (0) on success, otherwise error
00314     inv_error_t dmpEnable3Quat(void);
00315     
00316     // dmpEnable6Quat -- Enable 6-axis quaternion calculation
00317     // Output: INV_SUCCESS (0) on success, otherwise error
00318     inv_error_t dmpEnable6Quat(void);
00319     
00320     // dmpGetPedometerSteps -- Get number of steps in pedometer register
00321     // Output: Number of steps sensed
00322     unsigned long dmpGetPedometerSteps(void);
00323     // dmpSetPedometerSteps -- Set number of steps to a value
00324     // Input: Desired number of steps to begin incrementing from
00325     // Output: INV_SUCCESS (0) on success, otherwise error
00326     inv_error_t dmpSetPedometerSteps(unsigned long steps);
00327     // dmpGetPedometerTime -- Get number of milliseconds ellapsed over stepping
00328     // Output: Number of milliseconds where steps were detected
00329     unsigned long dmpGetPedometerTime(void);
00330     // dmpSetPedometerTime -- Set number time to begin incrementing step time counter from
00331     // Input: Desired number of milliseconds
00332     // Output: INV_SUCCESS (0) on success, otherwise error
00333     inv_error_t dmpSetPedometerTime(unsigned long time);
00334     
00335     // dmpSetInterruptMode --
00336     // Output: INV_SUCCESS (0) on success, otherwise error
00337     inv_error_t dmpSetInterruptMode(unsigned char mode);
00338     // dmpSetGyroBias --
00339     // Output: INV_SUCCESS (0) on success, otherwise error
00340     inv_error_t dmpSetGyroBias(long * bias);
00341     // dmpSetAccelBias -- 
00342     // Output: INV_SUCCESS (0) on success, otherwise error
00343     inv_error_t dmpSetAccelBias(long * bias);
00344     
00345     // lowPowerAccel --
00346     // Output: INV_SUCCESS (0) on success, otherwise error
00347     inv_error_t lowPowerAccel(unsigned short rate);
00348 
00349     // calcAccel -- Convert 16-bit signed acceleration value to g's
00350     float calcAccel(int axis);
00351     // calcGyro -- Convert 16-bit signed gyroscope value to degree's per second
00352     float calcGyro(int axis);
00353     // calcMag -- Convert 16-bit signed magnetometer value to microtesla (uT)
00354     float calcMag(int axis);
00355     // calcQuat -- Convert Q30-format quaternion to a vector between +/- 1
00356     float calcQuat(long axis);
00357     
00358     // computeEulerAngles -- Compute euler angles based on most recently read qw, qx, qy, and qz
00359     // Input: boolean indicating whether angle results are presented in degrees or radians
00360     // Output: class variables roll, pitch, and yaw will be updated on exit.    
00361     void computeEulerAngles(bool degrees = true);
00362     
00363     // computeCompassHeading -- Compute heading based on most recently read mx, my, and mz values
00364     // Output: class variable heading will be updated on exit
00365     float computeCompassHeading(void);
00366     
00367     // selfTest -- Run gyro and accel self-test.
00368     // Output: Returns bit mask, 1 indicates success. A 0x7 is success on all sensors.
00369     //         Bit pos 0: gyro
00370     //         Bit pos 1: accel
00371     //         Bit pos 2: mag
00372     int selfTest(unsigned char debug = 0);
00373     
00374 private:
00375     unsigned short _aSense;
00376     float _gSense, _mSense;
00377     
00378     // Convert a QN-format number to a float
00379     float qToFloat(long number, unsigned char q);
00380     unsigned short orientation_row_2_scale(const signed char *row);
00381 };
00382 
00383 #endif // _SPARKFUN_MPU9250_DMP_H_