基于STC15单片机ADC卡尔曼滤波程序

ID:507486 · 发表于 2020-4-4 16:50

#include "reg51.h"
#include "intrins.h"
#include "stdio.h"
#define FOSC 11059200L
#define BAUD 9600
typedef unsigned char BYTE;
typedef unsigned int WORD;
/*Declare SFR associated with the ADC */
sfr ADC_CONTR = 0xBC; //ADC control register
sfr ADC_RES = 0xBD; //ADC hight 8-bit result register
sfr ADC_LOW2 = 0xBE; //ADC low 2-bit result register
sfr P1ASF = 0x9D; //P1 secondary function control register
/*Define ADC operation const for ADC_CONTR*/
#define ADC_POWER 0x80 //ADC power control bit
#define ADC_FLAG 0x10 //ADC complete flag
#define ADC_START 0x08 //ADC start control bit
#define ADC_SPEEDLL 0x00 //420 clocks
#define ADC_SPEEDL 0x20 //280 clocks
#define ADC_SPEEDH 0x40 //140 clocks
#define ADC_SPEEDHH 0x60 //70 clocks
void InitUart();
void SendData(BYTE dat);
void Delay(WORD n);
void InitADC();
BYTE ch = 0; //ADC channel NO.
WORD adc_buf[3];
float vin_buf[3],kalman_buf[3];
float kalmanFilter_A(float inData)
{
//其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
//q,r的值需要我们试出来
static float prevData=0;
static float p=10, q=0.0001, r=0.005, kGain=0;
p = p+q;
kGain = p/(p+r);
inData = prevData+(kGain*(inData-prevData));
p = (1-kGain)*p;
prevData = inData;
return inData;
}
void main()
{
InitUart(); //Init UART, use to show ADC result
InitADC(); //Init ADC sfr
IE = 0xa0; //Enable ADC interrupt and Open master interrupt switch
//Start A/D conversion
while (1);
}
/*----------------------------
ADC interrupt service routine
----------------------------*/
void adc_isr() interrupt 5 using 1
{
ADC_CONTR &= !ADC_FLAG; //Clear ADC interrupt flag
adc_buf[ch] = ADC_RES;
vin_buf[ch] = 5.0*adc_buf[ch]/255;
kalman_buf[ch] = kalmanFilter_A(vin_buf[ch]);
printf("%1.3f /r/n ",vin_buf[ch]);
if (++ch > 2) ch = 0; //switch to next channel
ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | ch;
}
/*----------------------------
Initial ADC sfr
----------------------------*/
void InitADC()
{
P1ASF = 0xff; //Set all P1 as analog input port
ADC_RES = 0; //Clear previous result
ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | ch;
Delay(2); //ADC power-on delay and Start A/D conversion
}
/*----------------------------
Initial UART
----------------------------*/
void InitUart()
{
SCON = 0x5a; //8 bit data ,no parity bit
TMOD = 0x20; //T1 as 8-bit auto reload
TH1 = TL1 = -(FOSC/12/32/BAUD); //Set Uart baudrate
TR1 = 1; //T1 start running
}
/*----------------------------
Send one byte data to PC
Input: dat (UART data)
Output:-
----------------------------*/
void SendData(BYTE dat)
{
while (!TI); //Wait for the previous data is sent
TI = 0; //Clear TI flag
SBUF = dat; //Send current data
}
/*----------------------------
Software delay function
----------------------------*/
void Delay(WORD n)
{
WORD x;
while (n--)
{
x = 5000;
while (x--);
}
}

复制代码

帐号		自动登录	找回密码
密码			立即注册

基于STC15单片机ADC卡尔曼滤波程序

评分