shoe_mcu/motion/detect_zero_vel.c

#include "system.h"

#include "detect_zero_vel.h"
#include "hal_mt.h"

#include "hal_mt.h"
#include "app_flash.h"

//#include "hal_imu.h"

#define MAG_THRESHHOLD 500.f
#define SAMPLE_C 4

int16_t front_zero_tmp = 0;
int16_t back_zero_tmp = 0;

static float var_acc_temp;

int32_t get_var_acc(void)
{
	return var_acc_temp * 1000.f;
}

void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3])
{
}

void cal_step(int16_t front_zero, int16_t back_zero, float acc_x, float acc_y, float acc_z)
{
}

float var_acc_f(int16_t* acc, int length, int sample_c)
{
	if (length < 10*sample_c)
	{
		return 0.0f;
	}

	float mean_x = 0;

	float sum_x = 0;

	for (int i = length - 1; i >= length - 10*sample_c; i-=sample_c)
	{
		sum_x += acc[i];
	}

	mean_x = sum_x *0.1f;

	sum_x = 0.0f;

	for (int i = length - 1; i >= length - 10*sample_c; i-=sample_c)
	{
		sum_x += ((acc[i] - mean_x) * (acc[i] - mean_x));
	}


	return sum_x *0.1f / 2048.f /2048.f;
}

int16_t find_val(int16_t* data, int start_index, int end_index, char s)
{
	int16_t val = data[end_index - 1];

	for (int i = end_index - 1; i >= start_index; i-= SAMPLE_C)
	{
		if (s == '<')
		{
			if (val > data[i])
			{
				val = data[i];
			}
		}
		else
		{
			if (val < data[i])
			{
				val = data[i];
			}
		}
	}

	return val;
}




//2020/01/02
//长趋势判断压力上升
int isLongTimeUpTrend(int16_t *mag_window, int16_t length, int16_t up_threshhold, int16_t *min_val)
{
	//上下沿判断
	int16_t max_index = 0; int16_t min_index = 0;
	
	int16_t window_max_val = mag_window[length - 1]; 
	int16_t window_min_val = mag_window[length - 1]; 
	
	for(int i = length - 1; i >= 0; i -= SAMPLE_C)
	{
		if(window_max_val < mag_window[i])
		{
			window_max_val = mag_window[i];
			max_index = i;
		}
		
		if(window_min_val > mag_window[i])
		{
			window_min_val = mag_window[i];
			min_index = i;
		}
	}
	
	*min_val = window_min_val;
	
	if((max_index > min_index && window_max_val - window_min_val > up_threshhold &&  window_max_val - mag_window[length - 1] < 10 )
		|| (mag_window[length - 1] - window_min_val > up_threshhold && window_max_val - mag_window[length - 1] < 10))
	{
		return 1;
	}
	
	
	return 0;
	
}

int isLongTimeDownTrend(int16_t *mag_window, int16_t length, int16_t down_threshhold, int16_t *max_val)
{

		//上下沿判断
	int16_t max_index = 0; int16_t min_index = 0;
	
	int16_t window_max_val = mag_window[length - 1];
	int16_t window_min_val = mag_window[length - 1]; 
	
	for(int i = length - 1; i >=0; i -= SAMPLE_C)
	{
		if(window_max_val < mag_window[i])
		{
			window_max_val = mag_window[i];
			max_index = i;
		}
		
		if(window_min_val > mag_window[i])
		{
			window_min_val = mag_window[i];
			min_index = i;
		}
	}
	
	*max_val = window_max_val;
	
	if((max_index < min_index && window_max_val - window_min_val >  down_threshhold && mag_window[length - 1] - window_min_val < down_threshhold/2)
		|| window_max_val - mag_window[length - 1] > down_threshhold / 2)
	{
		return 1;
	}
	
	
	return 0;
	
}

//寻找加速度的最大值以及最小值
void  find_acc_max_and_min_val(int16_t*src, int16_t start_index, int16_t end_index, int16_t sample, int16_t* max_val, int16_t*min_val)
{
	*max_val = src[end_index - 1];
	*min_val = src[start_index];
	
	for(int i = end_index - 1; i >= start_index; i -= sample)
	{
		if(*max_val < src[i])
		{
			*max_val  = src[i];
		}
		
		if(*min_val > src[i])
		{
			*min_val  = src[i];
		}
		
	}
	
}

//根据压力的上升或下降，设置零速估计状态
void setZeroStatus(int16_t up_trend, int16_t down_trend, int16_t *zero_tmp)
{
	if(up_trend)
	{
		*zero_tmp = 1;
	}
	else if(down_trend)
	{
		*zero_tmp = 0;
	}
	else if(*zero_tmp == 1)
	{
		*zero_tmp = 2;
	}
}

//归纳整理触地信息的结果
void setTouchFloorStatus(int16_t *zero_tmp, int16_t *zero, 
	int16_t *acc_x_window, int16_t *acc_y_window, int16_t *acc_z_window)
{
	if (*zero_tmp == 1)
	{
		*zero = 1;
	}
	else if(*zero_tmp == 2  &&abs(acc_x_window[WINDOW_SIZE - 1-  SAMPLE_C] - acc_x_window[WINDOW_SIZE - 1]) < 103
		&& abs(acc_y_window[WINDOW_SIZE - 1- SAMPLE_C] - acc_y_window[WINDOW_SIZE - 1]) < 103
		&& abs(acc_z_window[WINDOW_SIZE - 1- SAMPLE_C] - acc_z_window[WINDOW_SIZE - 1]) < 103)
	{
		*zero = 1;
	}
	else
	{
		*zero_tmp = 0;
		*zero = 0;
	}
}

void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3], 
    int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero)
{
	//压力滑动窗口
	static  int16_t front_mag_window[WINDOW_SIZE];
	static  int16_t back_mag_window[WINDOW_SIZE];    
	
	//加速度滑动窗口
	static  int16_t acc_x_window[WINDOW_SIZE];
	static  int16_t acc_y_window[WINDOW_SIZE];
	static  int16_t acc_z_window[WINDOW_SIZE];
	
	static int16_t last_front_zupt;
	static int16_t last_back_zupt;
	
	static int16_t front_zupt_wait;
	static int16_t back_zupt_wait;

	static int16_t press_wait;
	static int16_t shake_acc_wait;
	
	static int16_t continue_up_min_val;


	static int acc_zero_count;

	static int16_t front_up_wait;
	static int16_t back_up_wait;
	static int16_t press_up_wait;
	
	static int16_t special_press_up_wait;
	
	
	static int16_t front_min_val;
	static int16_t back_min_val;
	
	static int16_t front_max_val;
	static int16_t back_max_val;
	
  
  int16_t front_val = abs(front[2]);
  int16_t back_val  = abs(back[2]);
		
		
	
	//滑动窗口更新数据
	memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(int16_t));
	memcpy(back_mag_window,  back_mag_window  + 1, (WINDOW_SIZE - 1) * sizeof(int16_t));
	
	memcpy(acc_x_window,  acc_x_window  + 1, (WINDOW_SIZE - 1) * sizeof(int16_t));
	memcpy(acc_y_window,  acc_y_window  + 1, (WINDOW_SIZE - 1) * sizeof(int16_t));
	memcpy(acc_z_window,  acc_z_window  + 1, (WINDOW_SIZE - 1) * sizeof(int16_t));
	
  front_mag_window[WINDOW_SIZE - 1] = front_val;
  back_mag_window[WINDOW_SIZE - 1]  = back_val;
  
  acc_x_window[WINDOW_SIZE - 1]  = acc[0];
	acc_y_window[WINDOW_SIZE - 1]  = acc[1];
	acc_z_window[WINDOW_SIZE - 1]  = acc[2];


	
	/*
	* 计算稳定的状态
	*/
		
	int16_t acc_max_val_x , acc_min_val_x ;
	
	int16_t acc_max_val_y , acc_min_val_y ;
	
	int16_t acc_max_val_z , acc_min_val_z ;
	
	//寻找加速度窗口的最大值、最小值
	find_acc_max_and_min_val(acc_x_window, WINDOW_SIZE - SAMPLE_C * 10, WINDOW_SIZE, SAMPLE_C , &acc_max_val_x, &acc_min_val_x);
	
	find_acc_max_and_min_val(acc_y_window, WINDOW_SIZE - SAMPLE_C * 10, WINDOW_SIZE, SAMPLE_C, &acc_max_val_y, &acc_min_val_y);
	
	find_acc_max_and_min_val(acc_z_window, WINDOW_SIZE - SAMPLE_C * 10, WINDOW_SIZE, SAMPLE_C, &acc_max_val_z, &acc_min_val_z);

	//判断前后脚的压力上升下降状态

	int back_up_trend = isLongTimeUpTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_min_val);

	//由于不能直接用后脚用压力来判断，那么就弄个倒计时好了
	if (back_up_trend == 1)
	{
		back_up_wait = 20 * SAMPLE_C;
	}

	int back_down_trend = isLongTimeDownTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_max_val);

	//当还处于后脚压力上升的余热中，降低前脚的判断阈值
	int front_up_trend;

	//当back_up_wait 后脚跟压力上升的等待时间 大于0时候， 阈值调低为2000，否则为3000
	if (back_up_wait > 0)
	{
		front_up_trend  = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_min_val);
	}
	else
	{
		front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, 1000, &front_min_val);
	}
	
	//缓慢踩地
	if(front_mag_window[WINDOW_SIZE - 1] - front_mag_window[WINDOW_SIZE - 1 - SAMPLE_C] >-50)
	{
		if(continue_up_min_val > front_mag_window[WINDOW_SIZE - 1])
		{
			continue_up_min_val = front_mag_window[WINDOW_SIZE - 1];
		}
	}
	else
	{
		continue_up_min_val = front_mag_window[WINDOW_SIZE - 1];
	}
	
	if(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 1000)
	{
		front_up_trend = 1;
	}

	//front_down_trend 为 1时候， 意味着前脚掌压力下降
	int front_down_trend = isLongTimeDownTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_max_val);

	// 自定义前后脚压力均上升，则重置压力等待时间，press_up_wait用于判断产生剧烈抖动时，则为触地
	// special_press_up_wait 用于判断延续触地逻辑，使其阈值变大(解决触地剧烈抖动的情况)
	if ((back_mag_window[WINDOW_SIZE - 1] - back_min_val > 1000 && back_max_val - back_mag_window[WINDOW_SIZE - 1] < 500) ||
		(front_mag_window[WINDOW_SIZE - 1] - front_min_val > 1000 && front_max_val - front_mag_window[WINDOW_SIZE - 1] < 500) ||
		(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 1000))
	{
		press_up_wait = 20 * SAMPLE_C;
		
		special_press_up_wait = 20 * SAMPLE_C;
	}
	
	//发现前脚压力向下降的时候，置special_press_up_wait为0，避免离地时候，该倒计时还要生效
	if(front_down_trend  == 1)
	{
		special_press_up_wait = 0;
	}
	
	
	//当前脚掌压力上升，后脚压力上升，则意味全脚掌着地，那就意味是真正的触地
	if (back_mag_window[WINDOW_SIZE - 1] - back_min_val > 2000 && front_mag_window[WINDOW_SIZE - 1] - front_min_val > 2000)
	{
		front_up_trend = 1;
	}

	//if (back_down_trend == 1 || front_down_trend == 1)
	//{
	//	front_up_trend = 0;
	//	back_up_trend = 0;
	//}
	
	//根据压力的上升或下降，设置零速估计状态
	setZeroStatus(front_up_trend, front_down_trend, &front_zero_tmp);
	
	//当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态
	if(front_zero_tmp > 0)
	{
		front_zupt_wait  = 20 * SAMPLE_C;
	}

	//后脚作同样的处理
	//setZeroStatus(back_up_trend, back_down_trend, &back_zero_tmp);
	//
	//
	////当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态
	//if(back_zero_tmp > 0)
	//{
	//	back_zupt_wait  = 20 * SAMPLE_C;
	//}
	
	
	/*过滤一下类似于穿拖鞋，后鞋垫与磁力计传感器之间的距离在振荡*/
	//相当于脚尖开始用力了，开始要走的状态,发现后脚的状态还处于触地状态，那么认为是误判
	/*if( back_zero_tmp != 0 && front_down_trend)
	{
		back_zero_tmp = 0;
	}*/
	//同样的，后脚泄力了，但是前脚还处于稳定的触地状态，那么解除这个稳定状态
	/*if(front_zero_tmp == 2 && back_down_trend)
	{
		front_zero_tmp = 0;
	}*/

	
	//下面处理的刚触地时候，导致剧烈抖动，从而使触地状态不连续
  var_acc_temp = var_acc_f(acc_z_window, 10 * SAMPLE_C, 1);

//	//寻找前后脚窗口的最大最小值
//	float front_mag_max_temp = find_val(front_mag_window, 0, 10 * SAMPLE_C , '>');
//	float front_mag_min_temp = find_val(front_mag_window, 0, 10 * SAMPLE_C  , '<');

//	float back_mag_max_temp = find_val(back_mag_window, 0, 10 * SAMPLE_C  , '>');
//	float back_mag_min_temp = find_val(back_mag_window, 0, 10 * SAMPLE_C  , '<');

	//发现抖动得厉害，而且压力窗口的前半部分处于一种平缓的状态，那么认为这个触地抖动导致的
	//if (var_acc_temp > 0.05f &&  back_mag_max_temp - back_mag_min_temp > 1000)
	//{
	//	press_wait = 10 * SAMPLE_C;
	//}

	//
	////处于假定的触地抖动时间press_wait， 发现有压力上升，那么肯定是触地了
	//if (front_mag_window[WINDOW_SIZE - 1] - front_mag_window[0] > 2000 && press_wait > 0)
	//{
	//	front_zero_tmp = 1;
	//	shake_acc_wait = 20 * SAMPLE_C;
	//}

	//后脚同理
//	if (back_mag_window[WINDOW_SIZE - 1] - back_mag_window[0] > 1500 && press_wait > 0)
//	{
//		back_zero_tmp = 1;
//		shake_acc_wait = 20 * SAMPLE_C;
//	}

	
	//归纳上述的处理结果，并且利用acc来延续触地的判断
	setTouchFloorStatus(&front_zero_tmp, front_zero, acc_x_window, acc_y_window, acc_z_window);
	
	//setTouchFloorStatus(&back_zero_tmp, back_zero, acc_x_window, acc_y_window, acc_z_window);
	
	
	//有时候触地状态中断了，那么根据短时间内出现加速度平稳，那么意味着这个平稳状态是触地状态
	if((front_zupt_wait > 0 || press_up_wait > 0 ) &&
		(acc_max_val_x - acc_min_val_x < 200 && acc_max_val_y - acc_min_val_y < 200 && acc_max_val_z - acc_min_val_z < 200))
	{
		*front_zero = 1;

		if (front_zero_tmp == 0)
		{
			front_zero_tmp = 2;
		}
		
	}
		//利用特殊的压力上升等待时间和抖动来判断
	if(special_press_up_wait > 0 && var_acc_temp > 3.0f)
	{
		*front_zero = 1;

		if (front_zero_tmp == 0)
		{
			front_zero_tmp = 2;
		}
	}
	
	//累加平稳加速度的数目，如果超过一秒，则强制归位触地状态
	if(acc_max_val_x - acc_min_val_x < 103 && acc_max_val_y - acc_min_val_y < 103 && acc_max_val_z - acc_min_val_z < 103)
	{
		*acc_zero = 1;
		 acc_zero_count ++;
	}
	else
	{
		*acc_zero = 0;
		acc_zero_count = 0;
	}

	
//	/*
//	* 强制大于1秒的零速均视为触地
//	*/
	if(acc_zero_count > 100 * SAMPLE_C)
	{
		*front_zero = 1;
		
		front_zero_tmp = 1;

	}
	
	//利用加速度延续
	int16_t continue_thresh = 103;
	
	if(special_press_up_wait > 0 )
	{
		continue_thresh = 1030;
	}
//	if (shake_acc_wait > 0)
//	{
//		continue_thresh = 1030;
//	}else if(MT_GetState())
//	{
//		//震动，导致加速度数据产生抖动
//		continue_thresh = 250;
//	}
	

	if(last_front_zupt == 1
		&& abs(acc_x_window[WINDOW_SIZE - 1 - SAMPLE_C]  - acc_x_window[WINDOW_SIZE - 1]) < continue_thresh
		&& abs(acc_y_window[WINDOW_SIZE - 1 - SAMPLE_C]  - acc_y_window[WINDOW_SIZE - 1]) < continue_thresh
		&& abs(acc_z_window[WINDOW_SIZE - 1 - SAMPLE_C]  - acc_z_window[WINDOW_SIZE - 1]) < continue_thresh)
	{
	
		*front_zero = 1;

		if (front_zero_tmp == 0)
		{
			front_zero_tmp = 2;
		}
	
	}



	//各种倒计时
	if(front_zupt_wait > 0)
	{
		front_zupt_wait --;
	}

	if (press_wait > 0)
	{
		press_wait--;
	}

	if (shake_acc_wait > 0)
	{
		shake_acc_wait--;
	}

	if (back_up_wait > 0)
	{
		back_up_wait--;
	}

	if (press_up_wait > 0)
	{
		press_up_wait--;
	}
	
	if (*front_zero)
	{
		front_zupt_wait = 20 * SAMPLE_C;
	}
	
	if(special_press_up_wait > 0)
	{
		special_press_up_wait --;
	}


	
	last_front_zupt = *front_zero;
	
	last_back_zupt  = *back_zero;
  
}