Diff: imu.h

Revision:

0:35bba382318b

Child:

1:7cf70724bdb0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/imu.h	Thu Jun 21 07:00:40 2018 +0000
@@ -0,0 +1,115 @@
+#ifndef __IMU_H_ADA
+#define __IMU_H_ADA
+
+#include "mbed.h"
+#include "rtos.h"
+
+#include "math.h"
+
+#define M_PI  (float)3.1415926535
+#define Kp     4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define Ki     1.0f    // integral gain governs rate of convergence of gyroscope biases
+
+// define MPU9250 register address
+//****************************************
+#define SMPLRT_DIV      0x19    //Sample Rate Divider. Typical values:0x07(125Hz) 1KHz internal sample rate
+#define CONFIG          0x1A    //Low Pass Filter.Typical values:0x06(5Hz)
+#define GYRO_CONFIG     0x1B    //Gyro Full Scale Select. Typical values:0x10(1000dps)
+#define ACCEL_CONFIG    0x1C    //Accel Full Scale Select. Typical values:0x01(2g)
+
+#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 MAG_XOUT_L      0x03
+#define MAG_XOUT_H      0x04
+#define MAG_YOUT_L      0x05
+#define MAG_YOUT_H      0x06
+#define MAG_ZOUT_L      0x07
+#define MAG_ZOUT_H      0x08
+
+
+#define PWR_MGMT_1      0x6B    //Power Management. Typical values:0x00(run mode)
+#define WHO_AM_I        0x75  //identity of the device
+
+
+#define GYRO_ADDRESS   0xD0   //Gyro and Accel device address
+#define MAG_ADDRESS    0x18   //compass device address
+#define ACCEL_ADDRESS  0xD0 
+
+#define ADDRESS_AD0_LOW     0xD0 //address pin low (GND), default for InvenSense evaluation board
+#define ADDRESS_AD0_HIGH    0xD1 //address pin high (VCC)
+#define DEFAULT_ADDRESS     GYRO_ADDRESS
+#define WHO_AM_I_VAL        0x73 //identity of MPU9250 is 0x71. identity of MPU9255 is 0x73.
+
+    typedef struct
+    {
+        int16_t X;
+        int16_t Y;
+        int16_t Z;
+    }MPU9250_TypeDef;
+    
+    typedef struct
+    {
+        int16_t X_Off_Err;
+        int16_t Y_Off_Err;
+        int16_t Z_Off_Err;
+    }MPU9250_TypeDef_Off;
+    
+    typedef struct
+    {
+        uint8_t Index;
+        int16_t AvgBuffer[8];
+    }MPU9250_AvgTypeDef;
+
+class IMU 
+{
+    
+    public:
+    void IMU_Init();
+    void IMU_GetYawPitchRoll(float*Angles);
+    //MPU9255
+    void MPU9250_Init();
+    void MPU9250_READ_ACCEL();
+    void MPU9250_READ_GYRO();
+    void MPU9250_READ_MAG();
+    bool MPU9250_Check();
+    void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
+    void MPU9250_InitGyrOffset();
+    
+    int16_t accel[3], gyro[3], magn[3];
+    
+    private:
+    
+    char    BUF[10];
+    //int16_t accel[3], gyro[3], magn[3];
+    float   angles[3];
+    float   q0, q1, q2, q3;
+    MPU9250_TypeDef     MPU9250_Offset;//={0};
+    MPU9250_TypeDef_Off MPU9250_Magn_Offset;//={0};
+    
+    float   invSqrt(float x);
+    void    IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    void    IMU_GetQuater(void);
+    
+    //void    MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
+    
+    void    I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data);
+    uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
+};
+
+#endif
\ No newline at end of file