shoes/liweiyan/esb-fsm-s/motion/process_result.c

#include "process_result.h"


int time_stamp = 0;

float gyr[3];
float acc[3];
float mag[3];

float magBias[3] = {43.6012f, 154.6074f, -263.2141f};
float magScale[3] = {0.0039f, 0.0037f, 0.0038f};

float accBias[3] = {-0.0641278f, 0.01712f, -0.099816f};
float accScale[3] = {1.0010795f, 0.9971222f, 1.004638f};

int16_t h_pos[3];
int16_t h_att[3];
int16_t h_zupt;
int16_t jump_send_count;
int16_t down_send_count;

int16_t is_jump;
int16_t is_down;

int16_t accel[3];
int16_t gyro[3];
uint16_t h_press;

uint16_t s_press;
int16_t s_pos[3];
int16_t s_att[3];
int8_t s_rssi;
int16_t s_zupt;

int left_press_buff[4];

int32_t left_press = 0;
int32_t right_press = 0;

void sen_data_acc(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(accel[0]>>8);
	buf[L++] = (uint8_t)(accel[0]>>0);
	
	buf[L++] = (uint8_t)(accel[1]>>8);
	buf[L++] = (uint8_t)(accel[1]>>0);
	
	buf[L++] = (uint8_t)(accel[2]>>8);
	buf[L++] = (uint8_t)(accel[2]>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void sen_data_pos(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	static short count = 0;
//	buf[L++] = (uint8_t)(h_pos[0]>>8);
//	buf[L++] = (uint8_t)(h_pos[0]>>0);
//	
//	buf[L++] = (uint8_t)(h_pos[1]>>8);
//	buf[L++] = (uint8_t)(h_pos[1]>>0);
	
	buf[L++] = (uint8_t)(h_pos[2]>>8);
	buf[L++] = (uint8_t)(h_pos[2]>>0);
	
	buf[L++] = (uint8_t)(s_pos[2]>>8);
	buf[L++] = (uint8_t)(s_pos[2]>>0);
	
	buf[L++] = (uint8_t)(count>>8);
	buf[L++] = (uint8_t)(count>>0);
	count ++;
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_data_press_gyr(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(gyro[0]>>8);
	buf[L++] = (uint8_t)(gyro[0]>>0);
	
	buf[L++] = (uint8_t)(gyro[1]>>8);
	buf[L++] = (uint8_t)(gyro[1]>>0);
	
	buf[L++] = (uint8_t)(gyro[2]>>8);
	buf[L++] = (uint8_t)(gyro[2]>>0);
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
	
}

//void send_data_pos_angle(void)
//{
//	uint8_t buf[256];
//	uint8_t L=0;
//	
//	buf[L++] = (uint8_t)(h_pos[0]>>8);
//	buf[L++] = (uint8_t)(h_pos[0]>>0);
//	
//	buf[L++] = (uint8_t)(h_pos[1]>>8);
//	buf[L++] = (uint8_t)(h_pos[1]>>0);
//	
//	buf[L++] = (uint8_t)(h_pos[2]>>8);
//	buf[L++] = (uint8_t)(h_pos[2]>>0);
//	
//	buf[L++] = (uint8_t)(angle[0]>>8);
//	buf[L++] = (uint8_t)(angle[0]>>0);
//	
//	buf[L++] = (uint8_t)(angle[1]>>8);
//	buf[L++] = (uint8_t)(angle[1]>>0);
//	
//	buf[L++] = (uint8_t)(angle[2]>>8);
//	buf[L++] = (uint8_t)(angle[2]>>0);
//	
//	send_protocol(DEX_NUM,CMD_HEART,buf,L);
//}

void send_data_acc(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(accel[0]>>8);
	buf[L++] = (uint8_t)(accel[0]>>0);
	
	buf[L++] = (uint8_t)(accel[1]>>8);
	buf[L++] = (uint8_t)(accel[1]>>0);
	
	buf[L++] = (uint8_t)(accel[2]>>8);
	buf[L++] = (uint8_t)(accel[2]>>0);
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}
void sen_data_press(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	buf[L++] = (uint8_t)(s_press>>8);
	buf[L++] = (uint8_t)(s_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}
void sen_data_left_press(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(left_press>>24);
	buf[L++] = (uint8_t)(left_press>>16);
	
	buf[L++] = (uint8_t)(left_press>>8);
	buf[L++] = (uint8_t)(left_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_package_data(uint16_t package_num, int16_t right_pos_data[3], int16_t left_pos_data[3], int16_t right_att_data[3], int16_t left_att_data[3],
			int16_t s_zupt, int16_t h_zupt,int16_t is_down, int16_t is_jump, int8_t s_rssi, uint8_t time_stamp)
{
	
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(right_pos_data[0]>>8);
	buf[L++] = (uint8_t)(right_pos_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_pos_data[1]>>8);
	buf[L++] = (uint8_t)(right_pos_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_pos_data[2]>>8);
	buf[L++] = (uint8_t)(right_pos_data[2]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[0]>>8);
	buf[L++] = (uint8_t)(left_pos_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[1]>>8);
	buf[L++] = (uint8_t)(left_pos_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[2]>>8);
	buf[L++] = (uint8_t)(left_pos_data[2]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[0]>>8);
	buf[L++] = (uint8_t)(right_att_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[1]>>8);
	buf[L++] = (uint8_t)(right_att_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[2]>>8);
	buf[L++] = (uint8_t)(right_att_data[2]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[0]>>8);
	buf[L++] = (uint8_t)(left_att_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[1]>>8);
	buf[L++] = (uint8_t)(left_att_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[2]>>8);
	buf[L++] = (uint8_t)(left_att_data[2]>>0);
	
	
	buf[L++] = (uint8_t)(s_zupt * 8 + h_zupt * 4 + is_down * 2 + is_jump * 1);
	
	buf[L++] = (uint8_t)(s_rssi >> 0);
	
	buf[L++] = (uint8_t)(time_stamp);
	
	send_protocol(DEX_NUM,package_num,buf,L);
	

}

void send_dual_foot_data(void)
{
	static uint8_t time_stamp = 0;
	
	send_package_data( 0x04, s_pos, h_pos, s_att, h_att ,s_zupt, h_zupt,  is_down,  is_jump, s_rssi, time_stamp);
	
	time_stamp ++;
}


void send_data_to_pc(void)
{
	static uint8_t wait_time = 0;
	
	if(wait_time == 4)
	{
		send_dual_foot_data();
		wait_time = 0;
	}
	
	wait_time ++;
	  
}


void process_motion(IMU_DAT_t* p)
{
//	int16_t accel[3];
//	int16_t gyro[3];
//	uint16_t h_press;

//	uint16_t s_press;
//	int16_t s_pos[3];
//	int16_t s_att[3];
//	int8_t s_rssi;
//	int16_t s_zupt;
	memcpy(accel,p->h.acc,sizeof(accel));
	memcpy(accel,p->h.gyr,sizeof(gyro));
	memcpy(accel,p->s.pos,sizeof(s_pos));
	memcpy(accel,p->s.att,sizeof(s_att));
	h_press = p->h.press;
	s_press = p->s.press;
	s_rssi = p->s.rssi;
	s_zupt = p->s.zupt;
	
	time_stamp = time_stamp + 1;
	
	left_press  = (((int32_t)h_press & 0x0000ffff)<<8);
	right_press = (((int32_t)s_press & 0x0000ffff)<<8);
	

	short down_tag = 0;

	down_tag = press_down_detect_new(time_stamp, left_press, right_press);

	if (down_tag)
	{
		gpio_mt_run(20);
		down_send_count = 20;
	}
	
	if(down_send_count > 0)
	{
		is_down = 1;
		down_send_count --;
	}
	else
	{
		is_down = 0;
	}
	
	short jump_tag  = 0;
	
	jump_tag  = press_jump_detect(h_pos , s_pos);
	
	if (jump_tag)
	{
		gpio_mt_run(20);
		jump_send_count = 20;
	}
	
	//连续发20次蹲，终端再处理
	if(jump_send_count > 0)
	{
		is_jump = 1;
		jump_send_count --;
	}
	else
	{
		is_jump = 0;
	}
	


	gyr[0] = (float)mMPU9250.gyro_x / GYR_LSB;
	gyr[1] = (float)mMPU9250.gyro_y / GYR_LSB;
	gyr[2] = (float)mMPU9250.gyro_z / GYR_LSB;

	acc[0] = (float)mMPU9250.acc_x / ACC_LSB;
	acc[1] = (float)mMPU9250.acc_y / ACC_LSB;
	acc[2] = (float)mMPU9250.acc_z / ACC_LSB;

	
	mag[0] = (float)mMPU9250.mag_x;
	mag[1] = (float)mMPU9250.mag_y;
	mag[2] = (float)mMPU9250.mag_z;
	
	mag[0] = (mag[0] - magBias[0]) * magScale[0];
	mag[1] = (mag[1] - magBias[1]) * magScale[1];
	mag[2] = (mag[2] - magBias[2]) * magScale[2];
	
	acc[0] = (acc[0] - accBias[0]) * accScale[0];
	acc[1] = (acc[1] - accBias[1]) * accScale[1];
	acc[2] = (acc[2] - accBias[2]) * accScale[2];
	
//	acc[0] = (acc[0] + 0.004700898f) / 0.99823547f;
//	acc[1] = (acc[1] - 0.0037821f)   / 0.9982976f;
//	acc[2] = (acc[2] + 0.03741163f) / 1.0116903f;
//	
	unsigned char left_tag = footPDR(time_stamp, gyr, acc, mag, left_press, h_pos, h_att, &h_zupt);
//	
//	if(h_pos[1] < -20)
//	{
//		gpio_mt_run(30);
//	}

//	if (h_zupt > 1 || h_zupt < 0)
//	{
////		send_motion_to_phone(MOTION_LEFT, time_stamp);
//		gpio_mt_run(20);
//	}    
	
	
//	memcpy(pos_n_offset, s_gyro, 3*sizeof(short));
	send_data_to_pc();
}