标题: 基于STC15单片机ADC卡尔曼滤波程序 [打印本页]
作者: 哈哈151-    时间: 2020-4-4 16:50
标题: 基于STC15单片机ADC卡尔曼滤波程序
  1. #include "reg51.h"
  2. #include "intrins.h"
  3. #include "stdio.h"

  5. #define FOSC    11059200L
  6. #define BAUD    9600

  8. typedef unsigned char BYTE;
  9. typedef unsigned int WORD;

  11. /*Declare SFR associated with the ADC */
  12. sfr ADC_CONTR   =   0xBC;           //ADC control register
  13. sfr ADC_RES     =   0xBD;           //ADC hight 8-bit result register
  14. sfr ADC_LOW2    =   0xBE;           //ADC low 2-bit result register
  15. sfr P1ASF       =   0x9D;           //P1 secondary function control register

  17. /*Define ADC operation const for ADC_CONTR*/
  18. #define ADC_POWER   0x80            //ADC power control bit
  19. #define ADC_FLAG    0x10            //ADC complete flag
  20. #define ADC_START   0x08            //ADC start control bit
  21. #define ADC_SPEEDLL 0x00            //420 clocks
  22. #define ADC_SPEEDL  0x20            //280 clocks
  23. #define ADC_SPEEDH  0x40            //140 clocks
  24. #define ADC_SPEEDHH 0x60            //70 clocks

  26. void InitUart();
  27. void SendData(BYTE dat);
  28. void Delay(WORD n);
  29. void InitADC();

  31. BYTE ch = 0;                        //ADC channel NO.
  32. WORD adc_buf[3];
  33. float vin_buf[3],kalman_buf[3];

  35. float kalmanFilter_A(float inData)
  36. {
  37.         //其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
  38.         //q,r的值需要我们试出来
  39.         static float prevData=0;
  40.         static float p=10, q=0.0001, r=0.005, kGain=0;
  41.         p = p+q;
  42.         kGain = p/(p+r);

  44.         inData = prevData+(kGain*(inData-prevData));
  45.         p = (1-kGain)*p;

  47.         prevData = inData;

  49.         return inData;
  50. }

  52. void main()
  53. {
  54.     InitUart();                     //Init UART, use to show ADC result
  55.     InitADC();                      //Init ADC sfr
  56.     IE = 0xa0;                      //Enable ADC interrupt and Open master interrupt switch
  57.                                     //Start A/D conversion
  58.     while (1);
  59. }

  61. /*----------------------------
  62. ADC interrupt service routine
  63. ----------------------------*/
  64. void adc_isr() interrupt 5 using 1
  65. {
  66.     ADC_CONTR &= !ADC_FLAG;         //Clear ADC interrupt flag

  68.         adc_buf[ch] = ADC_RES;
  69.         vin_buf[ch] = 5.0*adc_buf[ch]/255;
  70.         kalman_buf[ch] = kalmanFilter_A(vin_buf[ch]);
  71.         printf("%1.3f /r/n ",vin_buf[ch]);
  72.    
  73.     if (++ch > 2) ch = 0;           //switch to next channel
  74.     ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | ch;
  75. }

  77. /*----------------------------
  78. Initial ADC sfr
  79. ----------------------------*/
  80. void InitADC()
  81. {
  82.     P1ASF = 0xff;                   //Set all P1 as analog input port
  83.     ADC_RES = 0;                    //Clear previous result
  84.     ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | ch;
  85.     Delay(2);                       //ADC power-on delay and Start A/D conversion
  86. }

  88. /*----------------------------
  89. Initial UART
  90. ----------------------------*/
  91. void InitUart()
  92. {
  93.     SCON = 0x5a;                    //8 bit data ,no parity bit
  94.     TMOD = 0x20;                    //T1 as 8-bit auto reload
  95.     TH1 = TL1 = -(FOSC/12/32/BAUD); //Set Uart baudrate
  96.     TR1 = 1;                        //T1 start running
  97. }

  99. /*----------------------------
  100. Send one byte data to PC
  101. Input: dat (UART data)
  102. Output:-
  103. ----------------------------*/
  104. void SendData(BYTE dat)
  105. {
  106.     while (!TI);                    //Wait for the previous data is sent
  107.     TI = 0;                         //Clear TI flag
  108.     SBUF = dat;                     //Send current data
  109. }

  111. /*----------------------------
  112. Software delay function
  113. ----------------------------*/
  114. void Delay(WORD n)
  115. {
  116.     WORD x;

  118.     while (n--)
  119.     {
  120.         x = 5000;
  121.         while (x--);
  122.     }
  123. }

复制代码


卡尔曼滤波ADC.rar

39.44 KB, 下载次数: 78, 下载积分: 黑币 -5





欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1