mpu6050 DMP算法基于stm32

ID:272602 · 發(fā)表于 2019-6-2 11:32

發(fā)一個(gè)基于stm32F103的MPU6050 dma算法。詳情請(qǐng)下載。iar文件。共同學(xué)習(xí)。
#include "include.h"
static signed char gyro_orientation[9] = {-1, 0, 0,
                                       0,-1, 0,
                                       0, 0, 1};
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
float Pitch,Roll,Yaw;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//下面四個(gè)函數(shù)對(duì)接dmp庫(kù)

//addr：器件slave_address
//reg ：從器件將要寫(xiě)入數(shù)據(jù)的首地址
//len ：寫(xiě)入數(shù)據(jù)的長(zhǎng)度
//data：將要寫(xiě)入的一串?dāng)?shù)據(jù)
u8 I2C_Write_Buffer(u8 addr, u8 reg, u8 len, u8 * data)
{
int i;
I2C_Start();
I2C_Send_Byte(addr << 1 | 0);//7位器件從地址+讀寫(xiě)位
if (I2C_Wait_Ack())
{
      I2C_Stop();
      return 0;
}
I2C_Send_Byte(reg);
I2C_Wait_Ack();
for (i = 0; i < len; i++)
{
      I2C_Send_Byte(*data);
      if (I2C_Wait_Ack())
{
         I2C_Stop();
         return 0;
      }
data++;
}
I2C_Stop();
return 1;
}

//addr：器件slave_address
//reg ：從器件將要讀的數(shù)據(jù)的首地址
//len ：讀出數(shù)據(jù)的長(zhǎng)度
//buf ：將要讀出的數(shù)據(jù)存儲(chǔ)位置
u8 I2C_Read_Buffer(u8 addr, u8 reg, u8 len, u8* buf)
{
I2C_Start();
I2C_Send_Byte(addr << 1 | 0);
if (I2C_Wait_Ack())
{
      I2C_Stop();
      return 0;
}
I2C_Send_Byte(reg);
I2C_Wait_Ack();

I2C_Start();
I2C_Send_Byte(addr << 1 | 1);
I2C_Wait_Ack();
while (len)
{
      *buf = I2C_Read_Byte();
      if (len == 1)
         I2C_NAck();
      else
         I2C_Ack();
      buf++;
      len--;
}
I2C_Stop();
return 1;
}

//返回值 0：讀成功
// -1：讀失敗
int I2C_Read(u8 addr, u8 reg, u8 len, u8 *buf)
{
if(I2C_Read_Buffer(addr,reg,len,buf))
return 0;
else
return -1;
}
//返回值 0：寫(xiě)成功
// -1：寫(xiě)失敗
int I2C_Write(u8 addr, u8 reg, u8 len, u8* data)
{
if(I2C_Write_Buffer(addr,reg,len,data))
return 0;
else
return -1;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void MPU6050_Init(void)
{
int result=0;
I2C_Init_IO();
result=mpu_init();
if(!result)
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))          //mpu_set_sample_rate
if(!dmp_load_motion_driver_firmware())          //dmp_load_motion_driver_firmvare
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))    //dmp_set_orientation
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))            //dmp_enable_feature
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))            //dmp_set_fifo_rate
run_self_test(); //??
if(!mpu_set_dmp_state(1));
}
}

void MPU6050_Pose(void)
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
if(sensors & INV_WXYZ_QUAT )
{
q0 = quat[0] / q30;
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;

Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}
}
//通用定時(shí)器中斷初始化
//這里時(shí)鐘選擇為APB1的2倍，而APB1為36M
//arr：自動(dòng)重裝值。
//psc：時(shí)鐘預(yù)分頻數(shù)
//這里使用的是定時(shí)器2!
void Timer2_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //時(shí)鐘使能

TIM_TimeBaseStructure.TIM_Period = arr; //設(shè)置在下一個(gè)更新事件裝入活動(dòng)的自動(dòng)重裝載寄存器周期的值    計(jì)數(shù)到5000為500ms
TIM_TimeBaseStructure.TIM_Prescaler =psc; //設(shè)置用來(lái)作為T(mén)IMx時(shí)鐘頻率除數(shù)的預(yù)分頻值  10Khz的計(jì)數(shù)頻率
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設(shè)置時(shí)鐘分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上計(jì)數(shù)模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時(shí)間基數(shù)單位

TIM_ITConfig(TIM2,TIM_IT_Update ,ENABLE );//使能或者失能指定的TIM2中斷
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;  //TIM3中斷
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;  //先占優(yōu)先級(jí)1級(jí)
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;  //從優(yōu)先級(jí)3級(jí)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
NVIC_Init(&NVIC_InitStructure);  //根據(jù)NVIC_InitStruct中指定的參數(shù)初始化外設(shè)NVIC寄存器

TIM_Cmd(TIM2, ENABLE);  //使能TIMx外設(shè)
}
//定時(shí)器2中斷服務(wù)程序
//void TIM2_IRQHandler(void)
//{
// if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //溢出中斷
// {
// MPU6050_Pose();
// }
// TIM_ClearITPendingBit(TIM2, TIM_IT_Update);  //清除TIMx的中斷待處理位:TIM 中斷源
//}

ID:1 · 發(fā)表于 2019-6-2 17:32

本帖需要重新編輯補(bǔ)全電路原理圖,源碼,詳細(xì)說(shuō)明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)

帳號(hào)		自動(dòng)登錄	找回密碼
密碼			立即注冊(cè)