STM32

加入中断后，主程序延时函数不正常。

2019-07-21 01:31发布

站内问答 / STM32/STM8
1135 7 1409
写小车的程序遇到一个问题，网上没找到解答，求高手帮忙。
先贴上相关的代码

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]
友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。
7条回答
Ryan嘀嗒嘀嗒
2019-07-21 12:03
恩，查出来问题出在9250的驱动代码上，但还是不知道具体在哪？
[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]

一周热门 更多>