Diff: MPU6050.h

Revision:

0:9c2bb0f94c31

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.h	Mon Jan 18 19:44:43 2021 +0000
@@ -0,0 +1,240 @@
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "mbed.h"
+#include "math.h"
+
+#ifndef M_PI
+#define M_PI                3.14159265358979323846
+#endif
+
+// Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device
+
+// Invensense Inc., www.invensense.com
+// See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in
+// above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
+//
+#define XGOFFS_TC           0x00                        // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
+#define YGOFFS_TC           0x01
+#define ZGOFFS_TC           0x02
+#define X_FINE_GAIN         0x03                        // [7:0] fine gain
+#define Y_FINE_GAIN         0x04
+#define Z_FINE_GAIN         0x05
+#define XA_OFFSET_H         0x06                        // User-defined trim values for accelerometer
+#define XA_OFFSET_L_TC      0x07
+#define YA_OFFSET_H         0x08
+#define YA_OFFSET_L_TC      0x09
+#define ZA_OFFSET_H         0x0A
+#define ZA_OFFSET_L_TC      0x0B
+#define SELF_TEST_X         0x0D
+#define SELF_TEST_Y         0x0E
+#define SELF_TEST_Z         0x0F
+#define SELF_TEST_A         0x10
+#define XG_OFFS_USRH        0x13                        // User-defined trim values for gyroscope; supported in MPU-6050?
+#define XG_OFFS_USRL        0x14
+#define YG_OFFS_USRH        0x15
+#define YG_OFFS_USRL        0x16
+#define ZG_OFFS_USRH        0x17
+#define ZG_OFFS_USRL        0x18
+#define SMPLRT_DIV          0x19
+#define CONFIG              0x1A
+#define GYRO_CONFIG         0x1B
+#define ACCEL_CONFIG        0x1C
+#define FF_THR              0x1D                        // Free-fall
+#define FF_DUR              0x1E                        // Free-fall
+#define MOT_THR             0x1F                        // Motion detection threshold bits [7:0]
+#define MOT_DUR             0x20                        // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR            0x21                        // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR           0x22                        // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+#define FIFO_EN             0x23
+#define I2C_MST_CTRL        0x24
+#define I2C_SLV0_ADDR       0x25
+#define I2C_SLV0_REG        0x26
+#define I2C_SLV0_CTRL       0x27
+#define I2C_SLV1_ADDR       0x28
+#define I2C_SLV1_REG        0x29
+#define I2C_SLV1_CTRL       0x2A
+#define I2C_SLV2_ADDR       0x2B
+#define I2C_SLV2_REG        0x2C
+#define I2C_SLV2_CTRL       0x2D
+#define I2C_SLV3_ADDR       0x2E
+#define I2C_SLV3_REG        0x2F
+#define I2C_SLV3_CTRL       0x30
+#define I2C_SLV4_ADDR       0x31
+#define I2C_SLV4_REG        0x32
+#define I2C_SLV4_DO         0x33
+#define I2C_SLV4_CTRL       0x34
+#define I2C_SLV4_DI         0x35
+#define I2C_MST_STATUS      0x36
+#define INT_PIN_CFG         0x37
+#define INT_ENABLE          0x38
+#define DMP_INT_STATUS      0x39                        // Check DMP interrupt
+#define INT_STATUS          0x3A
+#define ACCEL_XOUT_H        0x3B
+#define ACCEL_XOUT_L        0x3C
+#define ACCEL_YOUT_H        0x3D
+#define ACCEL_YOUT_L        0x3E
+#define ACCEL_ZOUT_H        0x3F
+#define ACCEL_ZOUT_L        0x40
+#define TEMP_OUT_H          0x41
+#define TEMP_OUT_L          0x42
+#define GYRO_XOUT_H         0x43
+#define GYRO_XOUT_L         0x44
+#define GYRO_YOUT_H         0x45
+#define GYRO_YOUT_L         0x46
+#define GYRO_ZOUT_H         0x47
+#define GYRO_ZOUT_L         0x48
+#define EXT_SENS_DATA_00    0x49
+#define EXT_SENS_DATA_01    0x4A
+#define EXT_SENS_DATA_02    0x4B
+#define EXT_SENS_DATA_03    0x4C
+#define EXT_SENS_DATA_04    0x4D
+#define EXT_SENS_DATA_05    0x4E
+#define EXT_SENS_DATA_06    0x4F
+#define EXT_SENS_DATA_07    0x50
+#define EXT_SENS_DATA_08    0x51
+#define EXT_SENS_DATA_09    0x52
+#define EXT_SENS_DATA_10    0x53
+#define EXT_SENS_DATA_11    0x54
+#define EXT_SENS_DATA_12    0x55
+#define EXT_SENS_DATA_13    0x56
+#define EXT_SENS_DATA_14    0x57
+#define EXT_SENS_DATA_15    0x58
+#define EXT_SENS_DATA_16    0x59
+#define EXT_SENS_DATA_17    0x5A
+#define EXT_SENS_DATA_18    0x5B
+#define EXT_SENS_DATA_19    0x5C
+#define EXT_SENS_DATA_20    0x5D
+#define EXT_SENS_DATA_21    0x5E
+#define EXT_SENS_DATA_22    0x5F
+#define EXT_SENS_DATA_23    0x60
+#define MOT_DETECT_STATUS   0x61
+#define I2C_SLV0_DO         0x63
+#define I2C_SLV1_DO         0x64
+#define I2C_SLV2_DO         0x65
+#define I2C_SLV3_DO         0x66
+#define I2C_MST_DELAY_CTRL  0x67
+#define SIGNAL_PATH_RESET   0x68
+#define MOT_DETECT_CTRL     0x69
+#define USER_CTRL           0x6A                        // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1          0x6B                        // Device defaults to the SLEEP mode
+#define PWR_MGMT_2          0x6C
+#define DMP_BANK            0x6D                        // Activates a specific bank in the DMP
+#define DMP_RW_PNT          0x6E                        // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG             0x6F                        // Register in DMP from which to read or to which to write
+#define DMP_REG_1           0x70
+#define DMP_REG_2           0x71
+#define FIFO_COUNTH         0x72
+#define FIFO_COUNTL         0x73
+#define FIFO_R_W            0x74
+#define WHO_AM_I_MPU6050    0x75                        // Should return 0x68
+
+// Register bits
+#define DATA_RDY_INT        0
+#define I2C_MST_EN          5
+
+enum AccelScale
+{
+    AFS_2G,
+    AFS_4G,
+    AFS_8G,
+    AFS_16G
+};
+
+enum GyroScale
+{
+    GFS_250DPS,
+    GFS_500DPS,
+    GFS_1000DPS,
+    GFS_2000DPS
+};
+
+class MPU6050
+{
+    uint8_t     _addr;
+    AccelScale  _accelScale;
+    GyroScale   _gyroScale;
+    I2C         _i2c;
+    InterruptIn _interruptIn;
+    float       _accelRes;
+    float       _gyroRes;
+    int16_t     _accelAdc[3];                           // Stores the 16-bit signed accelerometer sensor output
+    int16_t     _gyroAdc[3];                            // Stores the 16-bit signed gyro sensor output
+    float       _gyroBias[3] = { 0 };
+    float       _accelBias[3] = { 0 };                  // Bias corrections for gyro and accelerometer
+    int16_t     _tempInt;                               // Stores the real internal chip temperature in degrees Celsius
+    float       _temp;
+    float       _selfTest[6];
+
+    // parameters for 6 DoF sensor fusion calculations
+    const float _gyroError = M_PI * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+    const float _gyroDrift = M_PI * (2.0f / 180.0f);    // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+    float       _beta;                                  // compute beta
+    float       _zeta;                                  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+    float       _pitch, _yaw, _roll;
+    float       _q[4] = { 1.0f, 0.0f, 0.0f, 0.0f };     // vector to hold quaternion
+    void        writeReg(uint8_t reg, uint8_t byte);
+    uint8_t     readReg(uint8_t reg);
+    void        readRegBytes(uint8_t reg, uint8_t len, uint8_t* dest);
+    int16_t*    accelADC();
+    int16_t*    gyroADC();
+    int16_t     tempADC();
+public:
+    float   accelData[3];                               // Stores the real accelerometer sensor output
+    float&  accelX = accelData[0];                          // Acceleration liases
+    float&  accelY = accelData[1];
+    float&  accelZ = accelData[2];
+    float   gyroData[3];                                // Stores the real gyro sensor output
+    float&  gyroX = gyroData[0];                           // Gyro aliases
+    float&  gyroY = gyroData[1];
+    float&  gyroZ = gyroData[2];
+
+    MPU6050
+    (
+        uint8_t     addr = 0x68,
+        AccelScale  accelScale = AFS_2G,
+        GyroScale   gyroScale = GFS_250DPS,
+        PinName     sdaPin = I2C_SDA,
+        PinName     sclPin = I2C_SCL,
+        PinName     interruptInPin = NC
+    );
+    virtual ~MPU6050()  { }
+    void    rise(Callback<void (void)> func);
+    template<typename T>
+    void rise(T* tptr, void (T:: *mptr) (void));
+    void fall(Callback<void (void)> func);
+    template<typename T>
+    void fall(T* tptr, void (T:: *mptr) (void));
+    bool init();
+    bool dataReady();
+    float* accel();
+    float* gyro();
+    float temp();
+
+    // Configure the motion detection control for low power accelerometer mode
+    void lowPowerAccelOnly();
+    void reset();
+
+    // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
+    // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
+    void calibrate();
+
+    // Accelerometer and gyroscope self test; check calibration wrt factory settings
+    bool selfTestOK();                                  // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
+    void setGain(float beta, float zeta);
+
+    // Implementation of Sebastian Madgwick's "...efficient orientation filter for inertial/magnetic sensor arrays"
+    // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
+    // which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
+    // device orientation -- which can be converted to yaw, pitch, and roll.
+    // Useful for stabilizing quadcopters, etc.
+    // The performance of the orientation filter is almost as good as conventional Kalman-based filtering algorithms
+    // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
+    void madgwickFilter(float deltaT);
+
+    // To compute yaw, pitch and roll after after aplying the madgwickFilter
+    float yaw();
+    float pitch();
+    float roll();
+};
+#endif // MPU6050_H