|
[pre lang="C" line="1" file="timer.c"]
void TIM6_Init(void)
{
RCC->APB1ENR|=1<<4;
TIM6->PSC=999;
TIM6->ARR=200;
TIM6->EGR=1;
TIM6->DIER|=1<<0;
TIM6->CR1|=1<<0;
NVIC->ISER[1]|=1<<22;
NVIC->IP[54]=1;
}
void TIM6_DAC_IRQHandler(void)
{
mpu.AXtemp=Get(0x3b);
mpu.AYtemp=Get(0x3d);
mpu.AZtemp=Get(0x3f);
mpu.GXtemp=Get(0x43);
mpu.GYtemp=Get(0x45);
mpu.GZtemp=Get(0x47);
mpu.GX+=(((mpu.GXtemp-22)*0.061035f)/500.0f);
mpu.GY+=((mpu.GYtemp-45)*0.061035f)/500.0f;
mpu.GZ+=((mpu.GZtemp+6)*0.061035f)/500.0f;
mpu.AXtemp1=(float)mpu.AXtemp/4096.0f;
mpu.AYtemp1=(float)mpu.AYtemp/4096.0f;
mpu.AZtemp1=(float)mpu.AZtemp/4096.0f;
mpu.AX=atan2f(mpu.AYtemp1,mpu.AZtemp1)*57.2958f;
mpu.AY=atan2f(mpu.AXtemp1,mpu.AZtemp1)*57.2958f;
mpu.AZ=atan2f(mpu.AXtemp1,mpu.AYtemp1)*57.2958f;
TIM6->SR=0;
}[/code]
角速度换算成的角度总是 正常值的一半, MPU角度上的寄存器 我设置的是0x18; |
|