shoes_project/touch_floor_by_imu/motion/process_result.c

#include "process_result.h"
#include "hal_mt.h"
#include "hal_flash.h"

#include "detect_zero_vel.h"

extern uint8_t isGameMode;

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


uint32_t time_stamp = 0;

int16_t accel[3];
int16_t gyro[3];
int16_t magn[3];

int32_t pos[3];
int16_t att[3];


int zupt;
int zupt_res;

uint16_t laser_distance;

uint8_t slave_rssi;

uint8_t foot_data_buf[64];

uint8_t foot_data_len;


int16_t front_zero;

int16_t back_zero;

int16_t acc_zero;


void init_MOTION(void)
{
	time_stamp = 0;
}

void foot_data_to_package()
{
	static uint16_t package_time = 0;
	
	foot_data_len = 0;
	
	foot_data_buf[foot_data_len++] = (uint8_t)(laser_distance>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(laser_distance>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>0);

 	
	foot_data_buf[foot_data_len++] = (uint8_t)(zupt>>0);
	
	
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t) slave_rssi;
	
	foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(magn[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(magn[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(magn[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(magn[1]>>0);
	
	package_time ++;

}

void get_foot_data(uint8_t *buf, uint8_t *buff_len)
{
	 memcpy(buf, foot_data_buf, foot_data_len);

	* buff_len = foot_data_len;
}

void process_motion(int16_t _acc[3], int16_t _gry[3], int16_t _mag[3], uint16_t _laser_distance, uint8_t _rssi)
{
	/*
		½âÎöÊý¾Ý
	*/
	
	
	memcpy(accel, _acc, 3 * sizeof(int16_t));
	
	accel[2] = -accel[2];
	
	memcpy(gyro , _gry, 3 * sizeof(int16_t));

	memcpy(magn , _mag, 3 * sizeof(int16_t));
	
	
	gyr[0] = (float)gyro[0] / GYR_LSB;
	gyr[1] = (float)gyro[1] / GYR_LSB;
	gyr[2] = (float)gyro[2] / GYR_LSB;

	acc[0] = (float)accel[0] / ACC_LSB;
	acc[1] = (float)accel[1] / ACC_LSB;
	acc[2] = (float)accel[2] / ACC_LSB;

//	acc[0] += 0.03f;
//	acc[1] += 0.1f;

	laser_distance = _laser_distance;
//	
	slave_rssi =  _rssi - 20;
//	

//	
//	if(acc_x_status == 1 || acc_y_status == 1 || acc_z_status == 1)
//	{
//		zupt = 1;
//	}
//	else  
//	{
//		zupt = 0;
//	}
//	
	
	static int acc_x_status;
	static int acc_y_status;
	static int acc_z_status;
	
	step_detect_by_acc_gyr(acc, gyr, &acc_x_status, &acc_y_status, &acc_z_status, &zupt);
	
	if(acc_x_status == 1 || acc_y_status == 1 || acc_z_status == 1)
	{
		zupt = 1;
	}
	else  
	{
		zupt = 0;
	}
	
	//zupt_res = cor_zupt_function(zupt);
	
	//detect_zero_vel(magn[0], magn[1], accel, &front_zero, &back_zero, &acc_zero);
	
//	if(front_zero > 0 || back_zero > 0 )
//	{
//		zupt = 1;
//	}
//	else
//	{
//		zupt = 0;
//	}
	acc[2] = -acc[2];
	
	footPDR(time_stamp, gyr, acc, mag, 0, pos, att, acc_x_status, acc_y_status, zupt_res);
	
//  att[1] = -att[1];
//	att[2] = mFlash.mStep.stepCur[0];

//	att[0] = acc_x_status;
//	att[1] = acc_y_status;
//	att[2] = zupt;

//	detect_step_by_acc(accel, &zupt);
//	att[2] = zupt;
//	att[0] = mFlash.mStep.stepCur[0];
	
  foot_data_to_package();
	
	time_stamp ++;
	
}