good progress workable 20250417
This commit is contained in:
parent
3e08589d31
commit
031899caed
|
|
@ -780,25 +780,6 @@ float KalmanFilter(float inData);
|
|||
#define PRESET_DENOMINATOR 15
|
||||
void STS_RSS_Filter(uint8_t pre_sts_rss_result);
|
||||
|
||||
#define RINGBUFF_LEN 128
|
||||
#define FALSE 0
|
||||
#define TRUE 1
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t Head;
|
||||
uint8_t Tail;
|
||||
uint8_t Lenght;
|
||||
uint8_t Ring_Buff[RINGBUFF_LEN];
|
||||
}RingBuff_t;
|
||||
|
||||
|
||||
|
||||
void RingBuff_Init(void); //初始化
|
||||
uint8_t Write_RingBuff(uint8_t data); //写入
|
||||
uint8_t Read_RingBuff(uint8_t *rData);//读取
|
||||
uint8_t Read_RingBuff_Length(void);
|
||||
|
||||
/* USER CODE BEGIN Private defines */
|
||||
/*
|
||||
In this example TIM2 input clock (TIM2CLK) is set to APB1 clock (PCLK1),
|
||||
|
|
|
|||
|
|
@ -825,8 +825,8 @@ int sts_presence_rss_fall_rise_detection(void)
|
|||
} else {
|
||||
pre_sts_rss_result = (average_result > 0)? 1: 0;
|
||||
}
|
||||
Write_RingBuff(pre_sts_rss_result);
|
||||
STS_RSS_Filter_ring();
|
||||
//Write_RingBuff(pre_sts_rss_result);
|
||||
//STS_RSS_Filter_ring();
|
||||
STS_RSS_Filter(pre_sts_rss_result);
|
||||
|
||||
// APP_LOG(TS_OFF, VLEVEL_M, "\r\nMotionCount=%4d Overall Motion=%d \r\n", (int)motion_count, (int)sts_rss_result);
|
||||
|
|
|
|||
|
|
@ -1534,249 +1534,54 @@ void Radar_Filtering_clutter(volatile uint8_t *color)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
#define PRESET_DENOMINATOR 5
|
||||
#define PRESET_NUMERATOR (PRESET_DENOMINATOR - 3)
|
||||
static uint8_t Motion_Changed_Flag = 0;
|
||||
static uint8_t numerator = 0;
|
||||
static uint8_t denominator = 0;
|
||||
|
||||
uint8_t STS_RSS_Filter(uint8_t pre_sts_rss_result)
|
||||
{
|
||||
_Bool pNew_Motion_Flag = 0;
|
||||
uint8_t Motion_Flag = sts_rss_result, xReturn=0;
|
||||
#if 1
|
||||
|
||||
pNew_Motion_Flag = pre_sts_rss_result;
|
||||
if(pNew_Motion_Flag != Motion_Flag){
|
||||
Motion_Changed_Flag = 1;
|
||||
}
|
||||
|
||||
denominator++;
|
||||
|
||||
if(Motion_Changed_Flag == 1)
|
||||
{
|
||||
|
||||
if(pNew_Motion_Flag != Motion_Flag){
|
||||
numerator++;
|
||||
}
|
||||
if(denominator >= PRESET_DENOMINATOR)
|
||||
{
|
||||
if(numerator >= PRESET_NUMERATOR) //SURE CHANGED
|
||||
{
|
||||
Motion_Flag = !Motion_Flag;
|
||||
if(Motion_Flag)
|
||||
{
|
||||
//M100C_Send_Data(10,0,ZhanYong);
|
||||
xReturn = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
//M100C_Send_Data(10,0,KeYong);
|
||||
xReturn = 0;
|
||||
}
|
||||
}
|
||||
denominator = 0;
|
||||
numerator = 0;
|
||||
Motion_Changed_Flag = 0;
|
||||
}
|
||||
}
|
||||
return xReturn;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#define FILTER_LEN 20
|
||||
#define SLIDING_WIN_LEN 10
|
||||
#define FILTER_LEN 15
|
||||
#define SLIDING_WIN_LEN 12
|
||||
#define SLIDING_THRESHOLD 8
|
||||
static uint8_t motion_read[FILTER_LEN]={0};
|
||||
static uint8_t idx_filter=0;
|
||||
|
||||
|
||||
|
||||
|
||||
void STS_RSS_Filter_ring(void)
|
||||
{
|
||||
uint8_t sum_motion = 0;
|
||||
|
||||
for (uint8_t i=0; i<SLIDING_WIN_LEN; i++ )
|
||||
{
|
||||
Read_RingBuff(&motion_read[i]);
|
||||
sum_motion += motion_read[i];
|
||||
}
|
||||
if (sum_motion >= (SLIDING_WIN_LEN-2))
|
||||
{
|
||||
sts_rss_result = STS_RESULT_MOTION;
|
||||
} else {
|
||||
sts_rss_result = STS_RESULT_NO_MOTION;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void STS_RSS_Filter(uint8_t pre_sts_rss_result)
|
||||
{
|
||||
_Bool pNew_Motion_Flag = 0;
|
||||
static uint8_t Motion_Changed_Flag = 1;
|
||||
static uint8_t numerator = PRESET_NUMERATOR;
|
||||
static uint8_t denominator = PRESET_DENOMINATOR;
|
||||
|
||||
pNew_Motion_Flag = pre_sts_rss_result;
|
||||
|
||||
if(pNew_Motion_Flag != Motion_Flag)
|
||||
{
|
||||
Motion_Changed_Flag = 1;
|
||||
}
|
||||
|
||||
if(Motion_Changed_Flag == 1)
|
||||
{
|
||||
denominator++;
|
||||
|
||||
if(pNew_Motion_Flag != Motion_Flag){
|
||||
numerator++;
|
||||
}
|
||||
|
||||
if(denominator >= PRESET_DENOMINATOR)
|
||||
{
|
||||
if(numerator >= PRESET_NUMERATOR) //的确改变
|
||||
{
|
||||
Motion_Flag = !Motion_Flag;
|
||||
if(Motion_Flag)
|
||||
{
|
||||
//M100C_Send_Data(10,0,ZhanYong);
|
||||
//*color = 2;
|
||||
sts_rss_result = STS_RESULT_MOTION;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
//M100C_Send_Data(10,0,KeYong);
|
||||
//*color = 1;
|
||||
sts_rss_result = STS_RESULT_NO_MOTION;
|
||||
}
|
||||
}
|
||||
|
||||
denominator = 0;
|
||||
numerator = 0;
|
||||
Motion_Changed_Flag = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#if 0
|
||||
|
||||
#define FILTER_LEN 8
|
||||
#define SLIDING_WIN_LEN 3
|
||||
static uint8_t motion_read[FILTER_LEN]={0};
|
||||
static uint8_t idx_filter=0;
|
||||
|
||||
uint8_t STS_RSS_Filter(uint8_t pre_sts_rss_result)
|
||||
{
|
||||
uint8_t j=0;
|
||||
uint8_t sum_sliding_win=0;
|
||||
|
||||
motion_read[idx_filter] = pre_sts_rss_result;
|
||||
|
||||
|
||||
|
||||
switch(idx_filter)
|
||||
{
|
||||
case 1:
|
||||
sum_sliding_win = motion_read[1] + motion_read[0] + motion_read[FILTER_LEN-1];
|
||||
break;
|
||||
case 2:
|
||||
sum_sliding_win = motion_read[2] + motion_read[1] + motion_read[0];
|
||||
break;
|
||||
case 0:
|
||||
sum_sliding_win = motion_read[0] + motion_read[FILTER_LEN-1] + motion_read[FILTER_LEN-2];
|
||||
break;
|
||||
default:
|
||||
sum_sliding_win = motion_read[idx_filter] + motion_read[idx_filter-1] + motion_read[idx_filter-2];
|
||||
break;
|
||||
}
|
||||
|
||||
idx_filter = (idx_filter + 1) % FILTER_LEN;
|
||||
|
||||
uint8_t sum_filter = 0;
|
||||
for (j=0; j<FILTER_LEN; j++)
|
||||
uint8_t k=0;
|
||||
for (j=0; j<SLIDING_WIN_LEN; j++)
|
||||
{
|
||||
sum_filter += motion_read[j];
|
||||
|
||||
if ((idx_filter - j) >=0)
|
||||
{
|
||||
k = idx_filter - j;
|
||||
}
|
||||
else
|
||||
{
|
||||
k = (idx_filter - j + FILTER_LEN)%FILTER_LEN;
|
||||
}
|
||||
|
||||
sum_filter += motion_read[k];
|
||||
}
|
||||
|
||||
return ((sum_sliding_win>=3))? 1:0;
|
||||
idx_filter = (idx_filter + 1) % FILTER_LEN;
|
||||
if (sts_rss_result)
|
||||
{
|
||||
//sts_rss_result= ((sum_filter >= SLIDING_THRESHOLD))? 1:0;
|
||||
sts_rss_result= ((sum_filter > (SLIDING_WIN_LEN - SLIDING_THRESHOLD)))? 1:0;
|
||||
|
||||
} else {
|
||||
|
||||
sts_rss_result= ((sum_filter > SLIDING_THRESHOLD))? 1:0;
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
RingBuff_t ringBuff;//创建一个ringBuff的缓冲区
|
||||
|
||||
|
||||
/**
|
||||
* @brief RingBuff_Init
|
||||
* @param void
|
||||
* @return void
|
||||
* @note 初始化环形缓冲区
|
||||
*/
|
||||
void RingBuff_Init(void)
|
||||
{
|
||||
//初始化相关信息
|
||||
ringBuff.Head = 0;
|
||||
ringBuff.Tail = 0;
|
||||
ringBuff.Lenght = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Write_RingBuff
|
||||
* @param u8 data
|
||||
* @return FLASE:环形缓冲区已满，写入失败;TRUE:写入成功
|
||||
* @note 往环形缓冲区写入u8类型的数据
|
||||
*/
|
||||
uint8_t Write_RingBuff(uint8_t data)
|
||||
{
|
||||
if(ringBuff.Lenght >= RINGBUFF_LEN) //判断缓冲区是否已满
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
ringBuff.Ring_Buff[ringBuff.Tail]=data;
|
||||
// ringBuff.Tail++;
|
||||
ringBuff.Tail = (ringBuff.Tail+1)%RINGBUFF_LEN;//防止越界非法访问
|
||||
ringBuff.Lenght++;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Read_RingBuff
|
||||
* @param u8 *rData，用于保存读取的数据
|
||||
* @return FLASE:环形缓冲区没有数据，读取失败;TRUE:读取成
|
||||
* @note 从环形缓冲区读取一个u8类型的数据
|
||||
*/
|
||||
uint8_t Read_RingBuff(uint8_t *rData)
|
||||
{
|
||||
if(ringBuff.Lenght == 0)//判断非空
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
*rData = ringBuff.Ring_Buff[ringBuff.Head];//先进先出FIFO，从缓冲区头出
|
||||
// ringBuff.Head++;
|
||||
ringBuff.Head = (ringBuff.Head+1)%RINGBUFF_LEN;//防止越界非法访问
|
||||
//ringBuff.Lenght--;
|
||||
return TRUE;
|
||||
}
|
||||
/**
|
||||
* @brief Read_RingBuff_Length
|
||||
* @param void
|
||||
* @return 环形缓冲区数据长度
|
||||
* @note
|
||||
*/
|
||||
uint8_t Read_RingBuff_Length(void)
|
||||
{
|
||||
return ringBuff.Lenght;
|
||||
}
|
||||
|
||||
/* USER CODE BEGIN EF */
|
||||
|
||||
/* USER CODE END EF */
|
||||
|
|
|
|||
|
|
@ -759,6 +759,7 @@ void STS_Sensor_Init(void)
|
|||
void STS_Sensor_Prepare(void)
|
||||
{
|
||||
|
||||
|
||||
UTIL_TIMER_Start(&YunhornSTSRSSWakeUpTimer);
|
||||
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue