写小车的程序遇到一个问题,网上没找到解答,求高手帮忙。
先贴上相关的代码
main.c
[mw_shl_code=c,true]#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "mpu9250.h"
#include "iic.h"
#include "kalman.h"
#include "motorpwm.h"
#include "timer.h"
#include "led.h"
int main(void)
{
delay_init(168); //延时函数初始化
uart_init(115200); //串口初始化为115200
IIC_Init(); //IIC configeration
delay_ms(10); //delay
MPU9250_Init(); //init MPU9250
delay_ms(10);
LED_Init();
PWM_Init();
TIM3_Int_Init();
while(1)
{
delay_ms(500);
USART_SHOW_Kalman_Angle();
LED0 = !LED0; //PF9
}
}
[/mw_shl_code]
timer.c
[mw_shl_code=c,true]# include "timer.h"
# include "kalman.h"
# include "led.h"
# include "mpu9250.h"
//通用定时器中断初始化
//这里时钟选择为APB1的2倍,而APB1为36M
//arr:自动重装值。
//psc:时钟预分频数
//这里使用的是定时器3!
void TIM3_Int_Init(void)
{
u16 arr = 50;
u16 psc = 8400;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); //时钟使能
TIM_TimeBaseStructure.TIM_Period = arr-1; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 计数到50为5ms
TIM_TimeBaseStructure.TIM_Prescaler = psc-1; //设置用来作为TIMx时钟频率除数的预分频值 10Khz的计数频率
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
TIM_ITConfig( //使能或者失能指定的TIM中断
TIM3, //TIM2
TIM_IT_Update ,
ENABLE //使能
);
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; //TIM3中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //先占优先级0级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; //从优先级3级
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
TIM_Cmd(TIM3, ENABLE); //使能TIMx外设
}
void TIM3_IRQHandler(void) //TIM3中断
{
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源
{
TIM_ClearITPendingBit(TIM3, TIM_IT_Update ); //清除TIMx的中断待处理位:TIM 中断源
//LED1 = !LED1; //PF10
Kalman_Filter(MPU9250_GYRO_X(),AE_ANGLE_byACCEL());
}
}
[/mw_shl_code]
kalman.c
[mw_shl_code=c,true]#include "kalman.h"
#include "mpu9250.h"
#include "usart.h"
float Angle=0,Gyro_x=0; //Angle and Gyro after Filter
//******参数************
float Q_angle=0.001;
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.005;
char C_0 = 1;
float Q_bias=0, Angle_err=0;
float PCt_0=0, PCt_1=0, E=0;
float K_0=0, K_1=0, t_0=0, t_1=0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
//*********************************************************
// Kalman_Filter
//*********************************************************
//Kalman_Filter,20MHz process time is about 0.77ms;
float Kalman_Filter(float Gyro,float ANGLE_byACCEL)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = ANGLE_byACCEL - Angle;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
Gyro_x = Gyro - Q_bias;
return Angle/3.1415926*180;
}
//*********************************************************
float Kalman_Filter_result(void)
{
return Kalman_Filter(MPU9250_GYRO_X(),AE_ANGLE_byACCEL());
}
//*********************************************************
void USART_SHOW_Kalman_Angle(void)
{
printf("Kalman_Angle%.2f ",Angle); //.2表示小数点后两位 f:float 制表符
}
[/mw_shl_code]
问题是:只要在中断服务函数里加上
[mw_shl_code=c,true]Kalman_Filter(MPU9250_GYRO_X(),AE_ANGLE_byACCEL());[/mw_shl_code]
[mw_shl_code=c,true]就会出现主函数里边延时函数不正常。[/mw_shl_code]
[mw_shl_code=c,true]
[mw_shl_code=c,true]delay_ms(500);[/mw_shl_code]
变成了延时5ms,而
[mw_shl_code=c,true]delay_ms(50000);[/mw_shl_code]
[mw_shl_code=c,true]变成了延时500ms。[/mw_shl_code]
[mw_shl_code=c,true]想了半天实在是没想明白程序到底是怎么运行的,小白,还望高手不吝赐教。[/mw_shl_code]
[/mw_shl_code]
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
这个我在教程就说了。在中断里面尽量不要调用delay。
[mw_shl_code=c,true]#include "mpu9250.h" #include "iic.h" #include "delay.h" #include "usart.h" #include "math.h" //初始化MPU9250**************************************** //OffsetDPS= X_OFFS_USR * 4 / 2^FS_SEL / Gyro_Sensitivity Nominal FS_SEL = 0 // // //***************************************************** void MPU9250_Init(void) { /* Single_Write(GYRO_ADDRESS,PWR_M, 0x80); // Single_Write(GYRO_ADDRESS,SMPL, 0x07); // Single_Write(GYRO_ADDRESS,DLPF, 0x1E); //?000? Single_Write(GYRO_ADDRESS,INT_C, 0x00 ); // Single_Write(GYRO_ADDRESS,PWR_M, 0x00); // */ IIC_Single_Write(GYRO_ADDRESS,PWR_MGMT_1, 0x00); //?????? IIC_Single_Write(GYRO_ADDRESS,SMPLRT_DIV, 0x07); IIC_Single_Write(GYRO_ADDRESS,CONFIG, 0x06); IIC_Single_Write(GYRO_ADDRESS,GYRO_CONFIG, 0x18); IIC_Single_Write(GYRO_ADDRESS,ACCEL_CONFIG, 0x01); //---------------- // Single_Write(GYRO_ADDRESS,0x6A,0x00);//close Master Mode } //***************************************************** short Connect_HL(unsigned char SlaveAddress,unsigned char REG_Address) //L=H+1 { u16 H,L; H=IIC_Single_Read(SlaveAddress,REG_Address); L=IIC_Single_Read(SlaveAddress,REG_Address+1); return H<<8|L; } short Connect_LH_MAG(unsigned char SlaveAddress,unsigned char REG_Address) //地址规律和另外两个不同H=L+1 { u16 H,L; IIC_Single_Write(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode delay_ms(10); IIC_Single_Write(MAG_ADDRESS,0x0A,0x01); delay_ms(10); L=IIC_Single_Read(SlaveAddress,REG_Address); H=IIC_Single_Read(SlaveAddress,REG_Address+1); return H<<8|L; } //***************************************************** float MPU9250_ACCEL_X(void) { return Connect_HL(ACCEL_ADDRESS,ACCEL_XOUT_H)/ACCEL_Sensi-2; } float MPU9250_ACCEL_Y(void) { return Connect_HL(ACCEL_ADDRESS,ACCEL_YOUT_H)/ACCEL_Sensi-2; } float MPU9250_ACCEL_Z(void) { return Connect_HL(ACCEL_ADDRESS,ACCEL_ZOUT_H)/ACCEL_Sensi-2; } //***************************************************** float MPU9250_GYRO_X(void) { return Connect_HL(GYRO_ADDRESS,GYRO_XOUT_H)/GYRO_Sensi+0.670732; } float MPU9250_GYRO_Y(void) { return Connect_HL(GYRO_ADDRESS,GYRO_YOUT_H)/GYRO_Sensi-0.914634; } float MPU9250_GYRO_Z(void) { return Connect_HL(GYRO_ADDRESS,GYRO_ZOUT_H)/GYRO_Sensi; } //***************************************************** float MPU9250_MAG_X(void) //地址规律和另外两个不同H=L+1 { return Connect_LH_MAG(MAG_ADDRESS,MAG_XOUT_L)/MAG_Sensi; } float MPU9250_MAG_Y(void) { return Connect_LH_MAG(MAG_ADDRESS,MAG_YOUT_L)/MAG_Sensi; } float MPU9250_MAG_Z(void) { return Connect_LH_MAG(MAG_ADDRESS,MAG_ZOUT_L)/MAG_Sensi; } //***************************************************** void USART_SHOW_MPU9250(void) { printf("AX%.6f ",MPU9250_ACCEL_X()); //.2表示小数点后两位 f:float 制表符 printf("AY%.6f ",MPU9250_ACCEL_Y()); printf("AZ%.6f ",MPU9250_ACCEL_Z()); printf("GX%.6f ",MPU9250_GYRO_X()); printf("GY%.6f ",MPU9250_GYRO_Y()); printf("GZ%.6f ",MPU9250_GYRO_Z()); printf("MX%.6f ",MPU9250_MAG_X()); printf("MY%.6f ",MPU9250_MAG_Y()); printf("MZ%.6f ",MPU9250_MAG_Z()); } //***************************************************** float AE_ANGLE_byACCEL(void) //AE-->accelerat expect(y) { float temp=0; if(0<MPU9250_ACCEL_Y()&& 0<MPU9250_ACCEL_Z()) //"<"优先级高于"&&" { temp=3.1415926*0.5+atan(MPU9250_ACCEL_Y()/MPU9250_ACCEL_Z()); } else if (0<MPU9250_ACCEL_Y()&& 0>MPU9250_ACCEL_Z()) { temp=3.1415926*1.5+atan(MPU9250_ACCEL_Y()/MPU9250_ACCEL_Z()); } else if (0>MPU9250_ACCEL_Y()&& 0<MPU9250_ACCEL_Z()) { temp=3.1415926*0.5+atan(MPU9250_ACCEL_Y()/MPU9250_ACCEL_Z()); } else { temp=3.1415926*1.5+atan(MPU9250_ACCEL_Y()/MPU9250_ACCEL_Z()); } return temp; } //***************************************************** //float AE_ANGLE_byACCEL(void) //AE-->accelerat expect(y) //{ // float temp=0; // temp=+atan(MPU9250_ACCEL_Y()/MPU9250_ACCEL_Z()); // return temp*180/3.1415926; //} //***************************************************** void USART_SHOW_ANGLE_byACCEL(void) { printf("ANGLE_byACCEL%.2f ",AE_ANGLE_byACCEL()); //.2表示小数点后两位 f:float 制表符 } //***************************************************** [/mw_shl_code]
现在问题很清晰明了了。
[mw_shl_code=c,true]timer.h //通用定时器中断初始化 //这里时钟选择为APB1的2倍,而APB1为36M //arr:自动重装值。 //psc:时钟预分频数 //这里使用的是定时器3! void TIM3_Int_Init(void) { u16 arr = 50; u16 psc = 8400; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); //时钟使能 TIM_TimeBaseStructure.TIM_Period = arr-1; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 计数到50为5ms TIM_TimeBaseStructure.TIM_Prescaler = psc-1; //设置用来作为TIMx时钟频率除数的预分频值 10Khz的计数频率 TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式 TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位 TIM_ITConfig( //使能或者失能指定的TIM中断 TIM3, //TIM2 TIM_IT_Update , ENABLE //使能 ); NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; //TIM3中断 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //先占优先级0级 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; //从优先级3级 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能 NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器 TIM_Cmd(TIM3, ENABLE); //使能TIMx外设 } void TIM3_IRQHandler(void) //TIM3中断 { if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源 { TIM_ClearITPendingBit(TIM3, TIM_IT_Update ); //清除TIMx的中断待处理位:TIM 中断源 LED1 = !LED1; //PF10 delay_us(4); } } [/mw_shl_code]
问题出在delay_us(4);这句代码,一旦加上,主函数里边延时函数delay_ms(500);就会变成delay_ms(5);
一周热门 更多>