Skip to content

Instantly share code, notes, and snippets.

@ksvbka

ksvbka/mpu6050.c

Last active June 19, 2022 01:46
Show Gist options
  • Save ksvbka/8de3412dfcf59513e8a4 to your computer and use it in GitHub Desktop.
Save ksvbka/8de3412dfcf59513e8a4 to your computer and use it in GitHub Desktop.
Download ZIP
Driver MPU6050 for MSP430
Raw
mpu6050.c
/********************************************************************************
Product: MPU6050
Module:
Created: 4/6/2015, by KienLTb
Description: Driver MPU6050 for MSP430
********************************************************************************/
/*-----------------------------------------------------------------------------*/
/* Header inclusions */
/*-----------------------------------------------------------------------------*/
#include "msp430g2553.h"
#include "mpu6050.h"
#include "math.h" // For atan();
/*-----------------------------------------------------------------------------*/
/* Local Constant definitions */
/*-----------------------------------------------------------------------------*/
/* AFS_SEL | Full Scale Range | LSB Sensitivity
* --------+------------------+----------------
* 0 | +/- 2g | 16384 LSB/mg
* 1 | +/- 4g | 8192 LSB/mg
* 2 | +/- 8g | 4096 LSB/mg
* 3 | +/- 16g | 2043 LSB/mg
*/
#define SCALED_ACC_2G 16384
#define SCALED_ACC_4G 8192
#define SCALED_ACC_8G 4096
#define SCALED_ACC_16G 2043
/* FS_SEL | Full Scale Range | LSB Sensitivity
* -------+--------------------+----------------
* 0 | +/- 250 degrees/s | 131 LSB/deg/s
* 1 | +/- 500 degrees/s | 65.5 LSB/deg/s
* 2 | +/- 1000 degrees/s | 32.8 LSB/deg/s
* 3 | +/- 2000 degrees/s | 16.4 LSB/deg/s
*/
#define SCALED_GYRO_250 131.0
#define SCALED_GYRO_500 65.5
#define SCALED_GYRO_1000 32.8
#define SCALED_GYRO_2000 16.4
/*
* Note:
* | ACCELEROMETER | GYROSCOPE
* DLPF_CFG | Bandwidth | Delay | Bandwidth | Delay | Sample Rate
* ---------+-----------+--------+-----------+--------+-------------
* 0 | 260Hz | 0ms | 256Hz | 0.98ms | 8kHz
* 1 | 184Hz | 2.0ms | 188Hz | 1.9ms | 1kHz
* 2 | 94Hz | 3.0ms | 98Hz | 2.8ms | 1kHz
* 3 | 44Hz | 4.9ms | 42Hz | 4.8ms | 1kHz
* 4 | 21Hz | 8.5ms | 20Hz | 8.3ms | 1kHz
* 5 | 10Hz | 13.8ms | 10Hz | 13.4ms | 1kHz
* 6 | 5Hz | 19.0ms | 5Hz | 18.6ms | 1kHz
* 7 | -- Reserved -- | -- Reserved -- | Reserved
*/
/*-----------------------------------------------------------------------------*/
/* Local Macro definitions */
/*-----------------------------------------------------------------------------*/
// Note: This Macro use for Bigendian but MSP430 is little endien => revert
#define CONVERT_TO_16BIT(MSB, LSB) (((WORD)(MSB) << 8) | (WORD)(LSB))
#define ABS(x) (x < 0 ? -x : x)
/*-----------------------------------------------------------------------------*/
/* Local Data type definitions */
/*-----------------------------------------------------------------------------*/
#define dt 0.015f
/*-----------------------------------------------------------------------------*/
/* Global variables */
/*-----------------------------------------------------------------------------*/
/* Offset value to calibrate Gyro */
static int16 Gyro_OffsetValueX = 0;
static int16 Gyro_OffsetValueY = 0;
static int16 Gyro_OffsetValueZ = 0;
/* Off set value to calibrate Acc*/
static int16 Acc_OffsetValueX = 0;
static int16 Acc_OffsetValueY = 0;
static int16 Acc_OffsetValueZ = 0;
/* Scale Value config for ACC - default is 2G*/
static float Acc_scaleValue = SCALED_ACC_2G;
/* Scale Value config for GYRO - default is 2G*/
static float Gyro_scaleValue = SCALED_GYRO_250;
/*-----------------------------------------------------------------------------*/
/* Function prototypes */
/*-----------------------------------------------------------------------------*/
VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG);
BYTE MPU6050_CheckI2C(VOID);
/* MPU6050 test configure of register*/
BYTE MPU6050_TestRegConfig(VOID);
VOID MPU6050_Calibrate_Gyro(VOID);
/* Raw Acc Value*/
VOID MPU6050_GetAccValueRaw(PVOID pValue);
/* Value in degree/s */
VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData);
/* Raw Gyro Value to m/s^2*/
VOID MPU6050_GetGyroValueRaw(PVOID pValue);
/* Convert to m/s^2*/
VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData);
/*-----------------------------------------------------------------------------*/
/* Function implementations */
/*-----------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------------
Function : MPU6050_Init
Purpose : Init and configure for MPU6050
Parameters : None
Return : Note
--------------------------------------------------------------------------------*/
VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG)
{
I2C_WriteByte(0x07, MPU6050_ADDRESS, MPU6050_SIGNAL_PATH_RESET);
// Reset MPU6050;
I2C_WriteByte(DEVICE_RESET, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1);
//Set sample rate
I2C_WriteByte(SET_SAMPLE_RATE_1000HZ, MPU6050_ADDRESS, MPU6050_SMPLRT_DIV);
//Config Low pass filter
I2C_WriteByte(EXT_SYNC_SET_INPUT_DISABLE + DLPF_CFG_BAND_WIDTH_10HZ, MPU6050_ADDRESS, MPU6050_CONFIG);
//Config Accel
BYTE AccConfig;
switch(ACC_SCALE_CONFIG)
{
case ACC_CONFIG_2G:
Acc_scaleValue = SCALED_ACC_2G;
AccConfig = AFS_SEL_SCALE_2G;
break;
case ACC_CONFIG_4G:
Acc_scaleValue = SCALED_ACC_4G;
AccConfig = AFS_SEL_SCALE_4G;
break;
case ACC_CONFIG_8G:
Acc_scaleValue = SCALED_ACC_8G;
AccConfig = AFS_SEL_SCALE_8G;
break;
case ACC_CONFIG_16G:
Acc_scaleValue = SCALED_ACC_16G;
AccConfig = AFS_SEL_SCALE_16G;
break;
}
I2C_WriteByte(AccConfig, MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG);
//Congfig Gyro
BYTE GyroConfig;
switch(GYRO_SCALE_CONFIG)
{
case GYRO_CONFIG_250:
Gyro_scaleValue = SCALED_GYRO_250;
GyroConfig = PS_SEL_SCALE_250;
break;
case GYRO_CONFIG_500:
Gyro_scaleValue = SCALED_GYRO_500;
GyroConfig = PS_SEL_SCALE_500;
break;
case GYRO_CONFIG_1000:
Gyro_scaleValue = SCALED_GYRO_1000;
GyroConfig = PS_SEL_SCALE_1000;
break;
case GYRO_CONFIG_2000:
Gyro_scaleValue = SCALED_GYRO_2000;
GyroConfig = PS_SEL_SCALE_2000;
break;
}
I2C_WriteByte(GyroConfig, MPU6050_ADDRESS, MPU6050_GYRO_CONFIG);
//Enable Interrupt
I2C_WriteByte(0x01, MPU6050_ADDRESS,MPU6050_INT_ENABLE);
// Enable MPU6050;
I2C_WriteByte(CLKSEL_0, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1);
/* Config another Register*/
// I2C_WriteByte(0x01, MPU6050_ADDRESS,MPU6050_INT_ENABLE);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_XOUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_XOUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_YOUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_YOUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_ZOUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_ZOUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_XOUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_XOUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_YOUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_YOUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_ZOUT_H);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_ZOUT_L);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FF_THR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FF_DUR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_MOT_THR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_MOT_DUR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ZRMOT_THR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ZRMOT_DUR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_H);
//
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FIFO_EN);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_ADDR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_REG);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_MST_CTRL);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_ADDR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_REG);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_CTRL);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_ADDR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_REG);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_CTRL);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_ADDR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_REG);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_CTRL);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_ADDR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_REG);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_CTRL);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_ADDR);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_REG);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_DO);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_CTRL);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_DI);
// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_INT_PIN_CFG);
__delay_cycles(100000);
}
/*--------------------------------------------------------------------------------
Function : MPU6050_CheckI2C
Purpose : Check I2C communication
Parameters : None
Return : Value of WHO_AM_I registor (0x68)
-------------------------------------------------------------------------------*/
BYTE MPU6050_CheckI2C(VOID)
{
BYTE byBuff;
I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_WHO_AM_I,1);
return (byBuff);
}
/*--------------------------------------------------------------------------------
Function : MPU6050_TestRegConfig
Purpose : Check the config of some register:
- MPU6050_RA_SMPLRT_DIV == 0x01:
- MPU6050_RA_CONFIG == 0x03;
- MPU6050_RA_GYRO_CONFIG == 0x01;
- MPU6050_RA_ACCEL_CONFIG == 0x00;
Parameters : None
Return : 0 if pass and 1 if failt
-------------------------------------------------------------------------------*/
BYTE MPU6050_TestRegConfig(VOID)
{
BYTE byBuff, ret;
I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_SMPLRT_DIV,1);
ret = (byBuff == 0x01 ? 0: 1);
__delay_cycles(10000);
I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_CONFIG,1);
ret = (byBuff == 0x03 ? 0: 1);
__delay_cycles(10000);
I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_GYRO_CONFIG,1);
ret = (byBuff == 0x01 ? 0: 1);
__delay_cycles(10000);
I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG,1);
ret = (byBuff == 0x00 ? 0: 1);
__delay_cycles(10000);
I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1,1);
ret = (byBuff == 0x00 ? 0: 1);
__delay_cycles(10000);
return ret;
}
/*--------------------------------------------------------------------------------
Function : MPU6050_GetAccValueRaw
Purpose : Get raw value x, y, z of accel
Parameters : PACC_DATA_RAW - pointer to a struct store acc raw data
Return : NULL
--------------------------------------------------------------------------------*/
VOID MPU6050_GetAccValueRaw(PVOID pValue)
{
BYTE pBuff[6];
I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, 6);
((PACC_DATA_RAW)pValue)->x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]) - Acc_OffsetValueX);
((PACC_DATA_RAW)pValue)->y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]) - Acc_OffsetValueY);
((PACC_DATA_RAW)pValue)->z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]) - Acc_OffsetValueZ);
}
/*--------------------------------------------------------------------------------
Function : PMU6050_AccConvertData
Purpose : Scaled data to radian value
Parameters : ACC_DATA_RAW , PVOID scaledData
Return : NULL
--------------------------------------------------------------------------------*/
VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData)
{
((PACC_DATA_SCALED)scaledData)->x = (float)rawValue.x / Acc_scaleValue;
((PACC_DATA_SCALED)scaledData)->y = (float)rawValue.y / Acc_scaleValue;
((PACC_DATA_SCALED)scaledData)->z = (float)rawValue.z / Acc_scaleValue;
}
/*--------------------------------------------------------------------------------
Function : PMU6050_GetAccValueAngle
Purpose : Get the rotation angle of sensor (compare with x, y, z axis) in degre
Parameters : PACC_DATA_ANGLE - pointer to a struct store angle rotation
Return : NULL
--------------------------------------------------------------------------------*/
VOID PMU6050_GetRotationAngle(ACC_DATA_RAW raw, PVOID pRotationAngle)
{
/* Caculate the angle rotation */
/* 180/PI = 57.296 */
/*Fix: use atan2 -> result in -pi -> pi */
float x_angle = 57.296 * atan2((float)raw.y, sqrt((float)raw.z*(float)raw.z + (float)raw.x*(float)raw.x));
if(x_angle < 0) x_angle += 360.0;
float y_angle = 57.296 * atan2((float)raw.x, sqrt((float)raw.z*(float)raw.z + (float)raw.y*(float)raw.y));
if(y_angle < 0) y_angle += 360.0;
float z_angle = 57.296 * atan2((float)raw.z, sqrt(((float)raw.x*(float)raw.x + (float)raw.y*(float)raw.y)));
if(x_angle < 0) x_angle += 360.0;
((PANGLE)pRotationAngle)->x = x_angle;
((PANGLE)pRotationAngle)->y = y_angle;
((PANGLE)pRotationAngle)->z = z_angle;
}
/*--------------------------------------------------------------------------------
Function : MPU6050_GetGyroValueRaw
Purpose : Get raw value x, y, z of Gyro
Parameters : PGYRO_DATA_RAW - pointer to struct store Gyro data
Return : NULL
--------------------------------------------------------------------------------*/
VOID MPU6050_GetGyroValueRaw(PVOID pValue)
{
BYTE pBuff[6];
I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H, 6);
((PGYRO_DATA_RAW)pValue)->x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]) - Gyro_OffsetValueX);
((PGYRO_DATA_RAW)pValue)->y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]) - Gyro_OffsetValueY);
((PGYRO_DATA_RAW)pValue)->z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]) - Gyro_OffsetValueZ);
}
/*--------------------------------------------------------------------------------
Function : PMU6050_GetGyroValueAngle
Purpose : Get value x, y, z of Gyro
Parameters : PGYRO_DATA_ANGLE - pointer to struct store Gyro data
Return : NULL
--------------------------------------------------------------------------------*/
VOID PMU6050_GetGyroValueAngle(PVOID pValue)
{
;
}
/*--------------------------------------------------------------------------------
Function : PMU6050_GyroConvertData
Purpose : Convert data to m/s^2 by divide to scale
Parameters : GYRO_DATA_RAW rawValue, PVOID scaledData
Return : NULL
--------------------------------------------------------------------------------*/
VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData)
{
((PGYRO_DATA_SCALED)scaledData)->x = (float)rawValue.x / Gyro_scaleValue;
((PGYRO_DATA_SCALED)scaledData)->y = (float)rawValue.y / Gyro_scaleValue;
((PGYRO_DATA_SCALED)scaledData)->z = (float)rawValue.z / Gyro_scaleValue;
}
/*--------------------------------------------------------------------------------
Function : MPU6050_Calibrate_Gyro
Purpose : Get the value to calibrate Gyro
Parameters : NULL
Return : NULL
--------------------------------------------------------------------------------*/
VOID MPU6050_Calibrate_Gyro(VOID)
{
int i;
int x = 0;
int y = 0;
int z = 0;
BYTE pBuff[6];
Gyro_OffsetValueX = 0;
Gyro_OffsetValueY = 0;
Gyro_OffsetValueZ = 0;
for(i = 0; i < 2000; i++)
{
I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H, 6);
x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]));
y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]));
z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]));
Gyro_OffsetValueX = (Gyro_OffsetValueX + x) >> 1;
Gyro_OffsetValueY = (Gyro_OffsetValueY + y) >> 1;
Gyro_OffsetValueZ = (Gyro_OffsetValueZ + z) >> 1;
}
// Gyro_OffsetValueX = (int16)(SumGyroX/200);
// Gyro_OffsetValueY = (int16)(SumGyroY/200);
// Gyro_OffsetValueZ = (int16)(SumGyroZ/200);
}
/*--------------------------------------------------------------------------------
Function : ComplementaryFilter
Purpose : Calculate angle use both acc and gyro
Parameters : Raw data (ADC 16bit) of acc and gyro
Return : ANGLE
--------------------------------------------------------------------------------*/
VOID Complementary_Filter(ACC_DATA_RAW accData, GYRO_DATA_RAW gyroData, PVOID pAngle)
{
float pitchAcc, rollAcc;
/* Angle around the X axis */
((PANGLE)pAngle)->x += ((float)gyroData.x / Gyro_scaleValue)*dt; // dt define as 0.015s
/* Angle around the Y axis*/
((PANGLE)pAngle)->y += ((float)gyroData.y / Gyro_scaleValue)*dt;
// Compensate for drift with accelerometer data if !bullshit
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
long forceMagnitudeApprox = ABS(accData.x) + ABS(accData.y) + ABS(accData.z);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = 57.296 * atan2((float)accData.y, sqrt((float)accData.z*(float)accData.z + (float)accData.x*(float)accData.x));
((PANGLE)pAngle)->x = ((PANGLE)pAngle)->x * 0.98 + pitchAcc * 0.02;
// Turning around the Y axis results in a vector on the X-axis
rollAcc = 57.296 * atan2((float)accData.x, sqrt((float)accData.z*(float)accData.z + (float)accData.y*(float)accData.y));
((PANGLE)pAngle)->y = ((PANGLE)pAngle)->y* 0.98 + rollAcc * 0.02;
}
}
Raw
mpu6050.h
/********************************************************************************
Module: MPU6050
Author: 5/4/2015, by KienLTb
Description: MPU6050 Lib for MSP430G2553
********************************************************************************/
#ifndef __MPU6050__H__
#define __MPU6050__H__
/*-----------------------------------------------------------------------------*/
/* Header inclusions */
/*-----------------------------------------------------------------------------*/
#include "msp430g2553.h"
//#include "i2c_sw.h"
#include "i2c_hw.h"
#include "types.h"
/*-----------------------------------------------------------------------------*/
/* Constant definitions */
/*-----------------------------------------------------------------------------*/
#define MPU6050_ADDRESS 0x68 // Address MPU6050 0b01101000 0b10100100
#define MPU6050_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU6050_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU6050_XA_OFFS_L_TC 0x07
#define MPU6050_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU6050_YA_OFFS_L_TC 0x09
#define MPU6050_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU6050_ZA_OFFS_L_TC 0x0B
#define MPU6050_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU6050_XG_OFFS_USRL 0x14
#define MPU6050_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU6050_YG_OFFS_USRL 0x16
#define MPU6050_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_ZG_OFFS_USRL 0x18
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_FF_THR 0x1D
#define MPU6050_FF_DUR 0x1E
#define MPU6050_MOT_THR 0x1F
#define MPU6050_MOT_DUR 0x20
#define MPU6050_ZRMOT_THR 0x21
#define MPU6050_ZRMOT_DUR 0x22
#define MPU6050_FIFO_EN 0x23
#define MPU6050_I2C_MST_CTRL 0x24
#define MPU6050_I2C_SLV0_ADDR 0x25
#define MPU6050_I2C_SLV0_REG 0x26
#define MPU6050_I2C_SLV0_CTRL 0x27
#define MPU6050_I2C_SLV1_ADDR 0x28
#define MPU6050_I2C_SLV1_REG 0x29
#define MPU6050_I2C_SLV1_CTRL 0x2A
#define MPU6050_I2C_SLV2_ADDR 0x2B
#define MPU6050_I2C_SLV2_REG 0x2C
#define MPU6050_I2C_SLV2_CTRL 0x2D
#define MPU6050_I2C_SLV3_ADDR 0x2E
#define MPU6050_I2C_SLV3_REG 0x2F
#define MPU6050_I2C_SLV3_CTRL 0x30
#define MPU6050_I2C_SLV4_ADDR 0x31
#define MPU6050_I2C_SLV4_REG 0x32
#define MPU6050_I2C_SLV4_DO 0x33
#define MPU6050_I2C_SLV4_CTRL 0x34
#define MPU6050_I2C_SLV4_DI 0x35
#define MPU6050_I2C_MST_STATUS 0x36
#define MPU6050_INT_PIN_CFG 0x37
#define MPU6050_INT_ENABLE 0x38
#define MPU6050_DMP_INT_STATUS 0x39
#define MPU6050_INT_STATUS 0x3A
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_EXT_SENS_DATA_00 0x49
#define MPU6050_EXT_SENS_DATA_01 0x4A
#define MPU6050_EXT_SENS_DATA_02 0x4B
#define MPU6050_EXT_SENS_DATA_03 0x4C
#define MPU6050_EXT_SENS_DATA_04 0x4D
#define MPU6050_EXT_SENS_DATA_05 0x4E
#define MPU6050_EXT_SENS_DATA_06 0x4F
#define MPU6050_EXT_SENS_DATA_07 0x50
#define MPU6050_EXT_SENS_DATA_08 0x51
#define MPU6050_EXT_SENS_DATA_09 0x52
#define MPU6050_EXT_SENS_DATA_10 0x53
#define MPU6050_EXT_SENS_DATA_11 0x54
#define MPU6050_EXT_SENS_DATA_12 0x55
#define MPU6050_EXT_SENS_DATA_13 0x56
#define MPU6050_EXT_SENS_DATA_14 0x57
#define MPU6050_EXT_SENS_DATA_15 0x58
#define MPU6050_EXT_SENS_DATA_16 0x59
#define MPU6050_EXT_SENS_DATA_17 0x5A
#define MPU6050_EXT_SENS_DATA_18 0x5B
#define MPU6050_EXT_SENS_DATA_19 0x5C
#define MPU6050_EXT_SENS_DATA_20 0x5D
#define MPU6050_EXT_SENS_DATA_21 0x5E
#define MPU6050_EXT_SENS_DATA_22 0x5F
#define MPU6050_EXT_SENS_DATA_23 0x60
#define MPU6050_MOT_DETECT_STATUS 0x61
#define MPU6050_I2C_SLV0_DO 0x63
#define MPU6050_I2C_SLV1_DO 0x64
#define MPU6050_I2C_SLV2_DO 0x65
#define MPU6050_I2C_SLV3_DO 0x66
#define MPU6050_I2C_MST_DELAY_CTRL 0x67
#define MPU6050_SIGNAL_PATH_RESET 0x68
#define MPU6050_MOT_DETECT_CTRL 0x69
#define MPU6050_USER_CTRL 0x6A
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_BANK_SEL 0x6D
#define MPU6050_MEM_START_ADDR 0x6E
#define MPU6050_MEM_R_W 0x6F
#define MPU6050_DMP_CFG_1 0x70
#define MPU6050_DMP_CFG_2 0x71
#define MPU6050_FIFO_COUNTH 0x72
#define MPU6050_FIFO_COUNTL 0x73
#define MPU6050_FIFO_R_W 0x74
#define MPU6050_WHO_AM_I 0x75
/*---------- CONFIG VALUE----------*/
/* MPU6050_PWR_MGMT_1 REG */
#define CLKSEL_0 0x00 //Internal 8MHz Osilator
#define CLKSEL_1 0x01 //PLL with X axis gyroscope reference
#define CLKSEL_2 0x02 //PLL with Y axis gyroscope reference
#define CLKSEL_3 0x03 //PLL with Z axis gyroscope reference
#define CLKSEL_4 0x04 //PLL with external 32.768kHz reference
#define CLKSEL_5 0x05 //PLL with external 19.2MHz reference
#define CLKSEL_6 0x06 //Reserved
#define CLKSEL_7 0x07 //Stops the clock and keeps the timing generator in reset
#define TEMP_DIS (0x01 << 3) //disables the temperature sensor
#define CYCLE (0x01 << 5) //
#define SLEEP (0x01 << 6) //
#define DEVICE_RESET (0x01 << 7) //
/*----------MPU6050_CONFIG----------*/
#define EXT_SYNC_SET_INPUT_DISABLE (0x00 << 3)// Input disabled
#define EXT_SYNC_SET_TEMP_OUT (0x01 << 3)// TEMP_OUT_L[0]
#define EXT_SYNC_SET_GYRO_XOUT (0x02 << 3)// GYRO_XOUT_L[0]
#define EXT_SYNC_SET_GYRO_YOUT (0x03 << 3)// GYRO_YOUT_L[0]
#define EXT_SYNC_SET_GYRO_ZOUT (0x04 << 3)// GYRO_ZOUT_L[0]
#define EXT_SYNC_SET_ACCEL_XOUT (0x05 << 3)// ACCEL_XOUT_L[0]
#define EXT_SYNC_SET_ACCEL_YOUT (0x06 << 3)// ACCEL_YOUT_L[0]
#define EXT_SYNC_SET_ACCEL_ZOUT (0x07 << 3)// ACCEL_ZOUT_L[0]
#define DLPF_CFG_BAND_WIDTH_260HZ 0x00// BandWidth 260Hz
#define DLPF_CFG_BAND_WIDTH_184HZ 0x01// BandWidth 184Hz
#define DLPF_CFG_BAND_WIDTH_94HZ 0x02// BandWidth 94Hz
#define DLPF_CFG_BAND_WIDTH_44HZ 0x03// BandWidth 44Hz
#define DLPF_CFG_BAND_WIDTH_21HZ 0x04// BandWidth 21Hz
#define DLPF_CFG_BAND_WIDTH_10HZ 0x05// BandWidth 10Hz
#define DLPF_CFG_BAND_WIDTH_5HZ 0x06// BandWidth 5Hz
/*---------MPU6050_GYRO_CONFIG-------*/
#define PS_SEL_SCALE_250 (0x00 << 3)
#define PS_SEL_SCALE_500 (0x01 << 3)
#define PS_SEL_SCALE_1000 (0x02 << 3)
#define PS_SEL_SCALE_2000 (0x03 << 3)
#define ZG_ST (0x01 << 5)
#define YG_ST (0x01 << 6)
#define XG_ST (0x01 << 7)
/*---------MPU6050_ACCEL_CONFIG------*/
#define AFS_SEL_SCALE_2G (0x00 << 3)
#define AFS_SEL_SCALE_4G (0x01 << 3)
#define AFS_SEL_SCALE_8G (0x02 << 3)
#define AFS_SEL_SCALE_16G (0x03 << 3)
#define ZA_ST (0x01 << 5)
#define YA_ST (0x01 << 6)
#define XA_ST (0x01 << 7)
/*---------MPU6050_SMPLRT_DIV_CONFIG---*/
#define SET_SAMPLE_RATE_1000HZ 0x07
/*-----------------------------------------------------------------------------*/
/* Macro definitions */
/*-----------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------*/
/* Global variables */
/*-----------------------------------------------------------------------------*/
#define GYRO_CONFIG_250 0
#define GYRO_CONFIG_500 1
#define GYRO_CONFIG_1000 2
#define GYRO_CONFIG_2000 3
#define ACC_CONFIG_2G 4
#define ACC_CONFIG_4G 5
#define ACC_CONFIG_8G 6
#define ACC_CONFIG_16G 7
/*-----------------------------------------------------------------------------*/
/* Data type definitions */
/*-----------------------------------------------------------------------------*/
//typedef unsigned char BYTE;
//typedef unsigned short WORD;
//typedef unsigned long DWORD;
//typedef unsigned int UINT;
//typedef BYTE BOOL;
//typedef unsigned char CHAR;
//typedef void VOID;
//
//typedef BYTE* PBYTE;
//typedef WORD* PWORD;
//typedef DWORD* PDWORD;
//typedef UINT* PUINT;
//typedef CHAR* PCHAR;
//typedef VOID* PVOID;
//
//typedef BYTE RESULT;
//
//typedef const BYTE* PCBYTE;
//
//#define FALSE 0
//#define TRUE 1
//#define NULL 0
/* Acc data raw value */
typedef struct _ACC_DATA_RAW
{
int16 x;
int16 y;
int16 z;
}ACC_DATA_RAW, *PACC_DATA_RAW;
/* Acc data value in angle */
typedef struct _ACC_DATA_SCALED
{
float x;
float y;
float z;
}ACC_DATA_SCALED, *PACC_DATA_SCALED;
/* Gyro data raw value */
typedef struct _GYRO_DATA_RAW
{
int16 x;
int16 y;
int16 z;
}GYRO_DATA_RAW, *PGYRO_DATA_RAW;
/* Gyro data value in angle */
typedef struct _GYRO_DATA_SCALED
{
float x;
float y;
float z;
}GYRO_DATA_SCALED, *PGYRO_DATA_SCALED;
/* Angle data caculate from raw value */
typedef struct _ANGLE
{
float x;
float y;
float z;
}ANGLE, *PANGLE;
/*-----------------------------------------------------------------------------*/
/* Function prototypes */
/*-----------------------------------------------------------------------------*/
VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG);
/* MPU6050 test i2c connection */
BYTE MPU6050_CheckI2C(VOID);
/* MPU6050 test configure of register*/
BYTE MPU6050_TestRegConfig(VOID);
/* Get the calibrate value and store to offset*/
VOID MPU6050_Calibrate_Gyro(VOID);
/* Raw Acc Value*/
VOID MPU6050_GetAccValueRaw(PVOID pValue);
/* Value in degree/s */
VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData);
/* Get rotation of sensor, value in degree*/
VOID PMU6050_GetRotationAngle(ACC_DATA_RAW raw, PVOID pRotationAngle);
/* Raw Gyro Value to m/s^2*/
VOID MPU6050_GetGyroValueRaw(PVOID pValue);
/* Convert to m/s^2*/
VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData);
/* Complementrary Filter*/
VOID Complementary_Filter(ACC_DATA_RAW accData, GYRO_DATA_RAW gyroData, PVOID pAngle);
#endif // __MPU6050__H__
@Elidrak
Copy link

Elidrak commented May 8, 2019

Hello, I have a MSP430fr2433 and i would like to use your work. How should i proceed to adapt it ?
Thank you for your work, best regards

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment