199 lines
7.4 KiB
C
199 lines
7.4 KiB
C
|
/******************** (C) COPYRIGHT 2020 GEEKIMU *******************************
|
|||
|
|
* File Name : MPU6500.c
|
|||
|
|
* Current Version : V2.0 & ST 3.5.0
|
|||
|
|
* Author : zhanli 719901725@qq.com & JustFeng.
|
|||
|
|
* Date of Issued : 2019.5.26 zhanli : Create
|
|||
|
|
* Comments : MPU6500<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
********************************************************************************/
|
|||
|
|
#include "stm32f10x_spi.h"
|
|||
|
|
#include "mpu6500_register_map.h"
|
|||
|
|
#include "MPU6500.h"
|
|||
|
|
#include "sys.h"
|
|||
|
|
#include "delay.h"
|
|||
|
|
#include "spi.h"
|
|||
|
|
#include "calibrate.h"
|
|||
|
|
|
|||
|
|
/* <20><><EFBFBD><EFBFBD>SPI<50><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>GPIO<49>˿<EFBFBD> ------------------------------------------------------ */
|
|||
|
|
#define mpu6500_CS GPIO_Pin_4
|
|||
|
|
#define mpu6500_CS_G GPIOA
|
|||
|
|
|
|||
|
|
#define mpu6500_DRDY GPIO_Pin_8
|
|||
|
|
#define mpu6500_DRDY_G GPIOB
|
|||
|
|
|
|||
|
|
#define mpu6500_CS_L PAout(4) = 0
|
|||
|
|
#define mpu6500_CS_H PAout(4) = 1
|
|||
|
|
|
|||
|
|
#define GYRO_SCALE 2000.0f / 32768
|
|||
|
|
#define ACCEL_SCALE 4.0f / 32768
|
|||
|
|
#define M_PI 3.14159265358979323846f
|
|||
|
|
|
|||
|
|
MPU_Data mMPU_Data;
|
|||
|
|
|
|||
|
|
extern Calibrate_Info mCali_Info;
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : MPU6500_IO_Init
|
|||
|
|
* Description : <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>MPU6500<EFBFBD><EFBFBD>IO<EFBFBD><EFBFBD>
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2019/5/26 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void MPU6500_IO_Init(void)
|
|||
|
|
{
|
|||
|
|
GPIO_InitTypeDef GPIO_InitStructure;
|
|||
|
|
|
|||
|
|
MEMS_SPI_Init();
|
|||
|
|
|
|||
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); /* ʹ<><CAB9>PA<50>˿<EFBFBD>ʱ<EFBFBD><CAB1> */
|
|||
|
|
|
|||
|
|
GPIO_InitStructure.GPIO_Pin = mpu6500_CS; /* MPU6500 CS<43>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; /* IO<49><4F><EFBFBD>ٶ<EFBFBD>Ϊ50MHz */
|
|||
|
|
GPIO_Init(mpu6500_CS_G, &GPIO_InitStructure); /* <20><><EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>˿<EFBFBD> */
|
|||
|
|
|
|||
|
|
GPIO_SetBits(mpu6500_CS_G, mpu6500_CS); /* <20><>Ĭ<EFBFBD><C4AC><EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD>Ƭѡ */
|
|||
|
|
|
|||
|
|
GPIO_InitStructure.GPIO_Pin = mpu6500_DRDY; /* MPU6500 DRDY <20>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
|||
|
|
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; /* IO<49><4F><EFBFBD>ٶ<EFBFBD>Ϊ50MHz */
|
|||
|
|
GPIO_Init(mpu6500_CS_G, &GPIO_InitStructure); /* <20><><EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>˿<EFBFBD> */
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : MPU6500_Init
|
|||
|
|
* Description : <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>MPU6500<EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD>Լ<EFBFBD>
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2019/5/26 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
uint8_t MPU6500_Init(void)
|
|||
|
|
{
|
|||
|
|
uint8_t res = 0xff;
|
|||
|
|
|
|||
|
|
MPU6500_IO_Init(); /* <20><>ʼ<EFBFBD><CABC>MPU6500<30><30>ӦIO<49><4F> */
|
|||
|
|
Delay_Ms(100); /* <20><>֤100ms<6D>ӳ<EFBFBD><D3B3>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD> */
|
|||
|
|
MPU6500_Write_Byte(PWR_MGMT_1, DEVICE_RESET); /* <20><><EFBFBD><EFBFBD><EFBFBD>豸״̬ */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
res = MPU6500_Read_Byte(WHO_AM_I); /* <20><>ȡ<EFBFBD>豸ID */
|
|||
|
|
|
|||
|
|
Delay_Ms(100); /* <20><>֤100ms<6D>ӳ<EFBFBD><D3B3>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD> */
|
|||
|
|
MPU6500_Write_Byte(SIGNAL_PATH_RESET, TEMP_RESET | /* <20><><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>ͨ<EFBFBD><CDA8> */
|
|||
|
|
GYRO_RESET |
|
|||
|
|
ACCEL_RESET);
|
|||
|
|
Delay_Ms(100); /* <20>ֲpage 42 <20><>ʱ 100ms */
|
|||
|
|
MPU6500_Write_Byte(USER_CTRL, I2C_IF_DIS); /* <20>ر<EFBFBD>MPU6500 IIC */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
MPU6500_Write_Byte(PWR_MGMT_1, CLKSEL_GYRO_Y); /* <20>ο<EFBFBD>DK2 */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
MPU6500_Write_Byte(INV_CONFIG, DLPF_CFG_256); /* <20><><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>250Hz */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
MPU6500_Write_Byte(ACCEL_CONFIG2, ACCEL_FCHOICE_B); /* <20><><EFBFBD>ٶȼƲ<C8BC><C6B2><EFBFBD>Ƶ<EFBFBD><C6B5>4khz */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
MPU6500_Write_Byte(GYRO_CONFIG, FS_SEL_2000); /* <20><><EFBFBD>ж<EFBFBD>2000deg/s */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
MPU6500_Write_Byte(ACCEL_CONFIG, AFS_SEL_4); /* <20><><EFBFBD>ٶȼƼ<C8BC><C6BC>ⷶΧ<E2B7B6><CEA7>4G */
|
|||
|
|
Delay_Ms(10);
|
|||
|
|
MPU6500_Write_Byte(INT_PIN_CFG, INT_RD_CLEAR); /* <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD> */
|
|||
|
|
|
|||
|
|
// Turn on data ready interrupts
|
|||
|
|
// mpu6500_Write_Byte(INT_ENABLE, DATA_RDY_EN);
|
|||
|
|
return res;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : MPU6500_Get_Rawdata
|
|||
|
|
* Description : <EFBFBD><EFBFBD>ȡMPU6500ԭʼ<EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ﰴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽڶ<EFBFBD>ȡ<EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD>Ժ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD>Ϊ
|
|||
|
|
DMAֱ<EFBFBD>Ӷ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2019/5/26 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void MPU6500_Get_Rawdata(s16 *ax,s16 *ay,s16 *az,s16 *gx,s16 *gy,s16 *gz,s16 *temp){
|
|||
|
|
|
|||
|
|
unsigned char Buff[6];
|
|||
|
|
|
|||
|
|
// <20><>ȡ<EFBFBD><C8A1><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Buff[0] = MPU6500_Read_Byte(ACCEL_XOUT_L);
|
|||
|
|
Buff[1] = MPU6500_Read_Byte(ACCEL_XOUT_H);
|
|||
|
|
*ax = (Buff[1] << 8 ) | Buff[0];
|
|||
|
|
|
|||
|
|
Buff[2] = MPU6500_Read_Byte(ACCEL_YOUT_L);
|
|||
|
|
Buff[3] = MPU6500_Read_Byte(ACCEL_YOUT_H);
|
|||
|
|
*ay = (Buff[3] << 8 ) | Buff[2];
|
|||
|
|
|
|||
|
|
Buff[4] = MPU6500_Read_Byte(ACCEL_ZOUT_L);
|
|||
|
|
Buff[5] = MPU6500_Read_Byte(ACCEL_ZOUT_H);
|
|||
|
|
*az = (Buff[5] << 8 ) | Buff[4];
|
|||
|
|
|
|||
|
|
// <20><>ȡ<EFBFBD>¶<EFBFBD><C2B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Buff[0] = MPU6500_Read_Byte(TEMP_OUT_L);
|
|||
|
|
Buff[1] = MPU6500_Read_Byte(TEMP_OUT_H);
|
|||
|
|
*temp = (Buff[1] << 8 ) | Buff[0];
|
|||
|
|
|
|||
|
|
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Buff[0] = MPU6500_Read_Byte(GYRO_XOUT_L);
|
|||
|
|
Buff[1] = MPU6500_Read_Byte(GYRO_XOUT_H);
|
|||
|
|
*gx = (Buff[1] << 8 ) | Buff[0];
|
|||
|
|
|
|||
|
|
Buff[2] = MPU6500_Read_Byte(GYRO_YOUT_L);
|
|||
|
|
Buff[3] = MPU6500_Read_Byte(GYRO_YOUT_H);
|
|||
|
|
*gy = (Buff[3] << 8 ) | Buff[2];
|
|||
|
|
|
|||
|
|
Buff[4] = MPU6500_Read_Byte(GYRO_ZOUT_L);
|
|||
|
|
Buff[5] = MPU6500_Read_Byte(GYRO_ZOUT_H);
|
|||
|
|
*gz = (Buff[5] << 8 ) | Buff[4];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : MPU6500_GetData
|
|||
|
|
* Description : <EFBFBD><EFBFBD>ȡMPU6500<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2019/5/26 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void MPU6500_GetData(MPU_Data *mMPU_Data){
|
|||
|
|
|
|||
|
|
int i = 0;
|
|||
|
|
MPU_RawData mRawData;
|
|||
|
|
|
|||
|
|
MPU6500_Get_Rawdata(&mRawData.Accel[0],&mRawData.Accel[1], &mRawData.Accel[2],
|
|||
|
|
&mRawData.Gyro[0], &mRawData.Gyro[1], &mRawData.Gyro[2],
|
|||
|
|
&mRawData.Temp);
|
|||
|
|
|
|||
|
|
for(i = 0; i < 3; i++)mMPU_Data->Accel[i] = (float)((mRawData.Accel[i]) * ACCEL_SCALE);
|
|||
|
|
|
|||
|
|
for(i = 0; i < 3; i++)mMPU_Data->Gyro[i] = (float)(M_PI * (mRawData.Gyro[i] - mCali_Info.Gyr_Offset[i]) * GYRO_SCALE / 180.0f);
|
|||
|
|
|
|||
|
|
mMPU_Data -> Temp = 10000 * (int32_t)(mRawData.Temp) / 33387 + 2100;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : MPU6500_Write_Byte
|
|||
|
|
* Description : MPU6500дһ<EFBFBD>ֽ<EFBFBD>
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2019/5/26 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
uint8_t MPU6500_Write_Byte(uint8_t reg,uint8_t data)
|
|||
|
|
{
|
|||
|
|
mpu6500_CS_L;
|
|||
|
|
SPI2_ReadWriteByte(reg);
|
|||
|
|
SPI2_ReadWriteByte(data);
|
|||
|
|
mpu6500_CS_H;
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : MPU6500_Read_Byte
|
|||
|
|
* Description : MPU6500<EFBFBD><EFBFBD>һ<EFBFBD>ֽ<EFBFBD>
|
|||
|
|
* Author : zhanli&719901725@qq.com
|
|||
|
|
* Date : 2019/5/26 zhanli
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
uint8_t MPU6500_Read_Byte(uint8_t reg)
|
|||
|
|
{
|
|||
|
|
uint8_t tmp=0;
|
|||
|
|
mpu6500_CS_L;
|
|||
|
|
SPI2_ReadWriteByte(reg|0x80);
|
|||
|
|
tmp = SPI2_ReadWriteByte(0xff);
|
|||
|
|
mpu6500_CS_H;
|
|||
|
|
return tmp;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|