balance-car.rar_balance_两轮小车_加速度计_陀螺 加速度计_陀螺滤波

#include"INIT.h" tADC_Config Master_Adc_Config; //============================================================================ //函数名称：hw_adc_init //函数返回：0 成功 ，1 失败 //参数说明：MoudelNumber：模块号 //功能概要：AD初始化 //============================================================================ void pllinit180M(void) { uint32_t temp_reg; //使能IO端口时钟 SIM_SCGC5 |= (SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTE_MASK ); //这里处在默认的FEI模式 //首先移动到FBE模式 MCG_C2 = 0; //MCG_C2 = MCG_C2_RANGE(2) | MCG_C2_HGO_MASK | MCG_C2_EREFS_MASK; //初始化晶振后释放锁定状态的振荡器和GPIO SIM_SCGC4 |= SIM_SCGC4_LLWU_MASK; LLWU_CS |= LLWU_CS_ACKISO_MASK; //选择外部晶振，参考分频器，清IREFS来启动外部晶振 //011 If RANGE = 0, Divide Factor is 8; for all other RANGE values, Divide Factor is 256. MCG_C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(3); //等待晶振稳定 //while (!(MCG_S & MCG_S_OSCINIT_MASK)){} //等待锁相环初始化结束 while (MCG_S & MCG_S_IREFST_MASK){} //等待时钟切换到外部参考时钟 while (((MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x2){} //进入FBE模式, //0x18==25分频=2M, //0x08==15分频=3.333M //0x09==16分频=3.125M, //0x10==17分频=2.94M //0x11==18分频=2.7778M //0x12==19分频=2.63M, //0x13==20分频=2.5M MCG_C5 = MCG_C5_PRDIV(0x0e); //确保MCG_C6处于复位状态，禁止LOLIE、PLL、和时钟控制器，清PLL VCO分频器 MCG_C6 = 0x0; //保存FMC_PFAPR当前的值 temp_reg = FMC_PFAPR; //通过M&PFD置位M0PFD来禁止预取功能 FMC_PFAPR |= FMC_PFAPR_M7PFD_MASK | FMC_PFAPR_M6PFD_MASK | FMC_PFAPR_M5PFD_MASK | FMC_PFAPR_M4PFD_MASK | FMC_PFAPR_M3PFD_MASK | FMC_PFAPR_M2PFD_MASK | FMC_PFAPR_M1PFD_MASK | FMC_PFAPR_M0PFD_MASK; ///设置系统分频器 //MCG=PLL, core = MCG, bus = MCG/3, FlexBus = MCG/3, Flash clock= MCG/8 SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0) | SIM_CLKDIV1_OUTDIV2(2) | SIM_CLKDIV1_OUTDIV3(2) | SIM_CLKDIV1_OUTDIV4(7); //从新存FMC_PFAPR的原始值 FMC_PFAPR = temp_reg; //设置VCO分频器，使能PLL为100MHz, LOLIE=0, PLLS=1, CME=0, VDIV=26 MCG_C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV(30); //VDIV = 31 (x54) //VDIV = 26 (x50) while (!(MCG_S & MCG_S_PLLST_MASK)){}; // wait for PLL status bit to set while (!(MCG_S & MCG_S_LOCK_MASK)){}; // Wait for LOCK bit to set //进入PBE模式 //通过清零CLKS位来进入PEE模式 // CLKS=0, FRDIV=3, IREFS=0, IRCLKEN=0, IREFSTEN=0 MCG_C1 &= ~MCG_C1_CLKS_MASK; //等待时钟状态位更新 while (((MCG_S & MCG_S_CLKST_MASK) >> MCG_S_CLKST_SHIFT) != 0x3){}; //SIM_CLKDIV2 |= SIM_CLKDIV2_USBDIV(1); //设置跟踪时钟为内核时钟 SIM_SOPT2 |= SIM_SOPT2_TRACECLKSEL_MASK; //在PTA6引脚上使能TRACE_CLKOU功能 // PORTA_PCR6 = ( PORT_PCR_MUX(0x7)); //最敬爱的 //使能FlexBus模块时钟 SIM_SCGC7 |= SIM_SCGC7_FLEXBUS_MASK; //在PTA6引脚上使能FB_CLKOUT功能 // PORTC_PCR3 = ( PORT_PCR_MUX(0x5)); 最近改的 } uint8 hw_adc_init(int MoudelNumber) { if(MoudelNumber == 0)//模块0 { SIM_SCGC6 |= (SIM_SCGC6_ADC0_MASK ); SIM_SOPT7 &= ~(SIM_SOPT7_ADC0ALTTRGEN_MASK | SIM_SOPT7_ADC0PRETRGSEL_MASK); SIM_SOPT7 = SIM_SOPT7_ADC0TRGSEL(0); } else if(MoudelNumber == 1)//模块1 { SIM_SCGC3 |= (SIM_SCGC3_ADC1_MASK ); SIM_SOPT7 &= ~(SIM_SOPT7_ADC1ALTTRGEN_MASK | SIM_SOPT7_ADC1PRETRGSEL_MASK) ; SIM_SOPT7 = SIM_SOPT7_ADC1TRGSEL(0); } else { return 0; } return 1; } void uart_fujun(void) { unsigned short sbr, brfa; //unsigned char tom; SIM_SCGC4 |= SIM_SCGC4_UART0_MASK; /* Turn on all port clocks */ SIM_SCGC5 |= SIM_SCGC5_PORTD_MASK; SIM_SCGC5 |= (SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTE_MASK ); /* Enable the UART0_TXD function on PTD6 */ // PORTD_PCR6 = PORT_PCR_MUX(0x3); // UART is alt3 function for this pin /* Enable the UART0_RXD function on PTD7 */ // PORTD_PCR7 = PORT_PCR_MUX(0x3); // UART is alt3 function for this pin //共两步骤：超频第2步，设置串口波特率，系统主频跟上面的必须一致才行 //UART baud rate = UART module clock / (16 × (SBR[SBR] +BRFD)) //UART0 and UART1 modules operate from the core/system clock sbr = (unsigned short)((180000000)/(115200*16)); UART0_BDH = (unsigned char)((sbr & 0x1F00) >> 8); UART0_BDL = (unsigned char)( sbr & 0x00FF); /* Determine if a fractional divider is needed to get closer to the baud rate */ //brfa = (((uartclk_khz*32000)/(baud * 16)) - (sbr * 32)); brfa =0; UART0_C4 = (unsigned char)(brfa & 0x001F); /* Configure the UART for 8-bit mode, no parity */ UART0_C1 = 0; /* Enable receiver and transmitter */ UART0_C2 |= (UART_C2_TE_MASK | UART_C2_RE_MASK ); } //============================================================================ //函数名称：hw_ad_once //函数返回：无符号结果值(范围:0-4095) //参数说明：MoudelNumber：模块号 // Channel：通道号 // accuracy：精度 //功能概要：采集一次一路模拟量的AD值 //============================================================================ uint16 hw_ad_once(int MoudelNumber,int Channel,uint8 accuracy)//采集某路模拟量的AD值 { uint16 result = 0; //开始ADC转换 hw_adc_convertstart(MoudelNumber, Channel, accuracy); if(MoudelNumber == 0) { while (( ADC0_SC1A & ADC_SC1_COCO_MASK ) != ADC_SC1_COCO_MASK) { } result = ADC0_RA; ADC0_SC1A &= ~ADC_SC1_COCO_MASK; } else { while (( ADC1_SC1A & ADC_SC1_COCO_MASK ) != ADC_SC1_COCO_MASK) { } result = ADC1_RA; ADC1_SC1A &= ~ADC_SC1_COCO_MASK; } return result; } //============================================================================ //函数名称：hw_ad_mid //函数返回：无符号结果值(范围:0-4095) //参数说明：MoudelNumber：模块号 // Channel：通道号 // accuracy：精度 //功能概要：中值滤波后的结果(范围:0-4095) //============================================================================ uint16 hw_ad_mid(int MoudelNumber,int Channel,uint8 accuracy) //中值滤波 { uint16 i,j,k,tmp; //1.取3次A/D转换结果 i = hw_ad_once(MoudelNumber,Channel,accuracy); j = hw_ad_once(MoudelNumber,Channel,accuracy); k = hw_ad_once(MoudelNumber,Channel,accuracy); //2.取中值 if (i > j) { tmp = i; i = j; j = tmp; } if (k > j) tmp = j; else if(k > i) tmp = k; else tmp = i; return tmp; } //============================================================================ //函数名称：hw_ad_ave //函数返回：无符号结果值(范围:0-4095) //参数说明：MoudelNumber：模块号 // Channel：通道号 // accuracy：精度 // N:均值滤波次数(范围:0~255) //功能概要：均值滤波后的结果(范围:0-4095) //============================================================================ uint16 hw_ad_ave(int MoudelNumber,int Channel,uint8 accuracy,uint8 N) //均值滤波 { uint32 tmp = 0; uint8 i; for(i = 0; i < N; i++) tmp += hw_ad_mid(MoudelNumber,Channel,accuracy