|
|
---------------這是main 函數(shù)----------------
#include "stm32f10x.h"
#include "bsp_usart.h" // 包含 串口 頭文件
#include "delay.h" // 包含 延時(shí) 頭文件
//#include "bsp_TiMbase.h" // 包含 基本定時(shí)器 頭文件
#include "stm32f10x_spi.h"
#include "ioi2c.h" // 包含 I2C 頭文件
#include "MPU6050.h" // 包含 MPU6050 頭文件
#include "control.h" // 包含 控制程序 頭文件
#include "bsp_adc.h" // 包含 adc 頭文件
#include "motor.h" // 包含 電機(jī) 頭文件
float Pitch,Roll,Yaw,fanguanjiao;
//*************************************************************
//* 函數(shù)名：DelayTime_ms
//* 描述 ：Time 延時(shí)的時(shí)間 MS
//* 輸入 ：無(wú)
//* 輸出 ：無(wú)
//*************************************************************
void delay_1ms(u32 time)
{
u32 i=8000*time;
while(i--);
}
//*************************************************************
//* 函數(shù)名：DelayTime_us
//* 描述 ：1us延時(shí)函數(shù)
//* 輸入 ：Time 延時(shí)的時(shí)間 US
//* 輸出 ：無(wú)
//*************************************************************
void delay_1us(u32 time)
{
u32 i=8*time;
while(i--);
}
/**
* @brief 主函數(shù)
* @param 無(wú)
* @retval 無(wú)
*/
int main(void)
{
// float Pitch,Roll,Yaw;
USART_Config();
ADCx_Init();
// 配置串口
// BASIC_TIM_Init();
delay_init();
IIC_Init();
NVIC_Configuration(); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
DMP_Init();
Motor_12_Config(); //L298電機(jī)驅(qū)動(dòng)初始化
printf("\r\n ----這是一個(gè)ADC單通道DMA讀取實(shí)驗(yàn)----\r\n");
while (1)
{
ADCcaclulate();
printf("Read_DMP Return is %d\n",Read_DMP(&Pitch,&Roll,&Yaw));
fanguanjiao = Pitch;
// printf("Pitch is:%f,Roll is:%f,Yaw is:%f\n",Pitch,Roll,Yaw);
printf("fanguanjiao is:%f",fanguanjiao);
if(fanguanjiao>30)
{
Motor_1_PRun();
delay_1ms(2000);
Motor_1_STOP();
delay_1ms(1000);
Motor_1_NRun();
delay_1ms(2000);
}
// printf("Read_DMP Return is %d\n",Read_DMP(&Pitch,&Roll,&Yaw));
// printf("Pitch is:%f,Roll is:%f,Yaw is:%f\n",Pitch,Roll,Yaw);
}
}
-----------------------這是mpu6050.c-------------
#include "MPU6050.h"
#include "IOI2C.h"
#include "bsp_usart.h"
#include <math.h>
#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu.h"
#include "delay.h"
/**************************************************************************/
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define MOTION (0)
#define NO_MOTION (1)
#define DEFAULT_MPU_HZ (200)
#define FLASH_SIZE (512)
#define FLASH_MEM_START ((void*)0x1800)
#define q30 1073741824.0f
static signed char gyro_orientation[9] = {-1, 0, 0,
0,-1, 0,
0, 0, 1};
static unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
static unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
unsigned short scalar;
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
static void run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x7) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
printf("setting bias succesfully ......\r\n");
}
}
uint8_t buffer[14];
int16_t MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*功 能: 將新的ADC數(shù)據(jù)更新到 FIFO數(shù)組，進(jìn)行濾波處理
*******************************************************************************/
void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
{
unsigned char i ;
int32_t sum=0;
for(i=1;i<10;i++){ //FIFO 操作
MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[0][9]=ax;//將新的數(shù)據(jù)放置到 數(shù)據(jù)的最后面
MPU6050_FIFO[1][9]=ay;
MPU6050_FIFO[2][9]=az;
MPU6050_FIFO[3][9]=gx;
MPU6050_FIFO[4][9]=gy;
MPU6050_FIFO[5][9]=gz;
sum=0;
for(i=0;i<10;i++){ //求當(dāng)前數(shù)組的合，再取平均值
sum+=MPU6050_FIFO[0][i];
}
MPU6050_FIFO[0][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[1][i];
}
MPU6050_FIFO[1][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[2][i];
}
MPU6050_FIFO[2][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[3][i];
}
MPU6050_FIFO[3][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[4][i];
}
MPU6050_FIFO[4][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[5][10]=sum/10;
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setClockSource(uint8_t source)
*功 能: 設(shè)置 MPU6050 的時(shí)鐘源
* CLK_SEL | Clock Source
* --------+--------------------------------------
* 0 | Internal oscillator
* 1 | PLL with X Gyro reference
* 2 | PLL with Y Gyro reference
* 3 | PLL with Z Gyro reference
* 4 | PLL with external 32.768kHz reference
* 5 | PLL with external 19.2MHz reference
* 6 | Reserved
* 7 | Stops the clock and keeps the timing generator in reset
*******************************************************************************/
void MPU6050_setClockSource(uint8_t source){
IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}
/** Set full-scale gyroscope range.
* @param range New full-scale gyroscope range value
* @see getFullScaleRange()
* @see MPU6050_GYRO_FS_250
* @see MPU6050_RA_GYRO_CONFIG
* @see MPU6050_GCONFIG_FS_SEL_BIT
* @see MPU6050_GCONFIG_FS_SEL_LENGTH
*/
void MPU6050_setFullScaleGyroRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setFullScaleAccelRange(uint8_t range)
*功 能: 設(shè)置 MPU6050 加速度計(jì)的最大量程
*******************************************************************************/
void MPU6050_setFullScaleAccelRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setSleepEnabled(uint8_t enabled)
*功 能: 設(shè)置 MPU6050 是否進(jìn)入睡眠模式
enabled =1 睡覺(jué)
enabled =0 工作
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: uint8_t MPU6050_getDeviceID(void)
*功 能: 讀取 MPU6050 WHO_AM_I 標(biāo)識(shí) 將返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {
IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
return buffer[0];
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: uint8_t MPU6050_testConnection(void)
*功 能: 檢測(cè)MPU6050 是否已經(jīng)連接
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {
if(MPU6050_getDeviceID() == 0x68) //0b01101000;
return 1;
else return 0;
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*功 能: 設(shè)置 MPU6050 是否為AUX I2C線的主機(jī)
*******************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*功 能: 設(shè)置 MPU6050 是否為AUX I2C線的主機(jī)
*******************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_initialize(void)
*功 能: 初始化 MPU6050 以進(jìn)入可用狀態(tài)。
*******************************************************************************/
void MPU6050_initialize(void) {
MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO); //設(shè)置時(shí)鐘
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺儀最大量程 +-1000度每秒
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //進(jìn)入工作狀態(tài)
MPU6050_setI2CMasterModeEnabled(0); //不讓MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(0); //主控制器的I2C與 MPU6050的AUXI2C 直通。控制器可以直接訪問(wèn)HMC5883L
}
/**************************************************************************
函數(shù)功能：MPU6050內(nèi)置DMP的初始化
入口參數(shù)：無(wú)
返回 值：無(wú)
作 者：平衡小車(chē)之家
**************************************************************************/
void DMP_Init(void)
{
u8 temp[1]={0};
i2cRead(0x68,0x75,1,temp);
printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())
printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
printf("dmp_set_orientation complete ......\r\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test();
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\r\n");
}
}
/**************************************************************************
函數(shù)功能：讀取MPU6050內(nèi)置DMP的姿態(tài)信息
入口參數(shù)：無(wú)
返回 值：無(wú)
作 者：平衡小車(chē)之家
**************************************************************************/
uint8_t Read_DMP(float* Pitch,float* Roll,float* Yaw)
{
short gyro[3], accel[3], sensors;
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return 1;
if (sensors & INV_WXYZ_QUAT)
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
*Pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
*Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*Yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
// delay_ms(10);
return 0;
}
else return 2;
}
/**************************************************************************
函數(shù)功能：讀取MPU6050內(nèi)置溫度傳感器數(shù)據(jù)
入口參數(shù)：無(wú)
返回 值：攝氏溫度
作 者：平衡小車(chē)之家
**************************************************************************/
int Read_Temperature(void)
{
float Temp;
Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
if(Temp>32768) Temp-=65536;
Temp=(36.53+Temp/340)*10;
return (int)Temp;
}
//------------------End of File----------------------------
|
|
，感激不盡！！！
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
