logzhan
/
GeekIMU
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeekIMU/2.Firmware/STM32/Firmware/sensor/mpu6500.c

199 lines
7.4 KiB
C
Raw Normal View History

上传GeekIMU资料
2024-11-09 21:39:20 +08:00
/******************** (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);
更新GeekIMU-42688P适配固件，更新基于Python的可视化工具
2025-02-20 23:29:58 +08:00
return res == ;
上传GeekIMU资料
2024-11-09 21:39:20 +08:00
}
/**----------------------------------------------------------------------
* 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;
}