|
|
/* Includes ------------------------------------------------------------------*/
#include "stm8s.h"
#include "stdio.h"
#include <math.h>
/* Private defines -----------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
void Send(uint8_t dat);
void Delay(uint16_t nCount);
void print(double a);
void delay_us(uint16_t dly1);
unsigned long Read_ADS1232(void);
void ADS1232_GPIO_Init();
double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction);
/* Private functions ---------------------------------------------------------*/
#define LED_GPIO_PORT (GPIOD)
#define LED_GPIO_PINS (GPIO_PIN_1)
#define SCK_Low GPIO_WriteLow(GPIOA,(GPIO_Pin_TypeDef)GPIO_PIN_3)
#define SCK_High GPIO_WriteHigh(GPIOA,(GPIO_Pin_TypeDef)GPIO_PIN_3)
#define DOUT GPIO_ReadInputPin(GPIOD,(GPIO_Pin_TypeDef)GPIO_PIN_4)
void main(void)
{
/* Initialize I/Os in Output Mode */
unsigned char n=0,m=0;
unsigned long i=0,j;
double a=0,b=0,c=0,d=0;
double Q=0.0018;
ADS1232_GPIO_Init();
GPIO_Init(LED_GPIO_PORT, (GPIO_Pin_TypeDef)LED_GPIO_PINS, GPIO_MODE_OUT_PP_LOW_FAST);
UART1_DeInit();
UART1_Init((u32)9600, UART1_WORDLENGTH_8D, UART1_STOPBITS_1, UART1_PARITY_NO, UART1_SYNCMODE_CLOCK_DISABLE, UART1_MODE_TX_ENABLE);
while (1)
{
n++,m++;
/* Toggles LEDs */
// GPIO_WriteReverse(LED_GPIO_PORT, (GPIO_Pin_TypeDef)LED_GPIO_PINS);
//Delay(0xffff);
i=Read_ADS1232();
if(abs(i-j)>1000)
{
Q=0.0018;
n=0;
}
if(n==50)Q=0.000000000000001;
j=i;
a=(double)i;
a/=16777216;
a*=4.99899;
b=KalmanFilter(a,Q,0.000000000002,1);
c+=b;
d+=a;
if(m==10)
{
c*=10000000;
d*=10000000;
/*i=(unsigned long)d;
//print(a);
Send(i%1000000000/100000000+48);
Send('.');
Send(i%100000000/10000000+48);
Send(i%10000000/1000000+48);
Send(i%1000000/100000+48);
Send(i%100000/10000+48);
Send(i%10000/1000+48);
Send(i%1000/100+48);
Send(i%100/10+48);
Send(i%10+48);
Send(',');*/
i=(unsigned long)c;
Send(i%1000000000/100000000+48);
Send('.');
Send(i%100000000/10000000+48);
Send(i%10000000/1000000+48);
Send(i%1000000/100000+48);
Send(i%100000/10000+48);
Send(i%10000/1000+48);
Send(i%1000/100+48);
Send(i%100/10+48);
Send(i%10+48);
Send('\n');
c=0;
d=0;
m=0;
}
}
}
/*-------------------------------------------------------------------------------------------------------------*/
/*
Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
*/
double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double x_last;
double x_mid = x_last;
double x_now;
static double p_last;
double p_mid ;
double p_now;
double kg;
x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
p_now=(1-kg)*p_mid;//最优值对应的covariance
p_last = p_now; //更新covariance值
x_last = x_now; //更新系统状态值
return x_now;
}
/*-------------------------------------------------------------------------------------------------------------*/
void delay_us(uint16_t dly1)
{
uint16_t i;
for(i=dly1;i>0;i--);
}
void ADS1232_GPIO_Init()
{
GPIO_Init(GPIOA, (GPIO_Pin_TypeDef)GPIO_PIN_3, GPIO_MODE_OUT_PP_LOW_FAST);
GPIO_Init(GPIOD, (GPIO_Pin_TypeDef)GPIO_PIN_4, GPIO_MODE_IN_FL_NO_IT);
}
unsigned long Read_ADS1232(void)
{
unsigned long val = 0;
unsigned char i = 0;
SCK_Low; //SCK=0
while(DOUT);//等待DOUT=0
delay_us(1);
for(i=0;i<24;i++)
{
SCK_High;//SCK=1
val=val<<1;
delay_us(1);
SCK_Low; //SCK=0
if(DOUT)//DOUT=1
val++;
delay_us(1);
}
SCK_High;
delay_us(1);
SCK_Low;
delay_us(1);
return val;
}
void Delay(uint16_t nCount)
{
/* Decrement nCount value */
while (nCount != 0)
{
nCount--;
}
}
void Send(uint8_t dat)
{
while(( UART1_GetFlagStatus(UART1_FLAG_TXE)==RESET));
UART1_SendData8(dat);
}
void print(double a)
{
unsigned long temp=0;
temp=(unsigned long)a*1000000;
Send(temp/1000000+48);
Send('.');
Send(temp%1000000/100000+48);
Send(temp%100000/10000+48);
Send(temp%10000/1000+48);
Send(temp%1000/100+48);
Send(temp%100/10+48);
Send(temp%10+48);
Send('\n');
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval : None
*/
void assert_failed(u8* file, u32 line)
{
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* Infinite loop */
while (1)
{
}
}
#endif
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
|
|
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
