shoes_project/girl_shoes_test/motion/detect_step_by_mag.c

#include "detect_step_by_mag.h"
#include "math.h"
#include "stdlib.h"

uint16_t mag_sqrt =0;
uint8_t detect_step_by_mag(float *mag)
{
	static uint8_t up_flag = 0;
	int32_t mag_temp[3];
	
	
	static uint16_t mag_buf[2];
	static uint8_t mag_index = 0;
	
	float D = 800;
	int step;
	int calculate_flag = 0;
	
	for(int i = 0; i < 3; i ++)
	{
		mag_temp[i] = (int32_t) (mag[i] *10);
	}
	mag_sqrt  = (uint16_t)(sqrt((float) (mag_temp[0] * mag_temp[0] + mag_temp[1] * mag_temp[1] + mag_temp[2] * mag_temp[2])));
	
	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);
	
	if(mag_index >= 2)
	{
		mag_buf[0] = mag_buf[1];
		mag_index = 1;
		calculate_flag = 1;
	}
	mag_buf[mag_index++] = mag_sqrt;
	
	if (calculate_flag == 1)
	{
	  if (up_flag == 0)
	  {
		   if(mag_buf[1] - mag_buf[0] > 2*D)
			 {
				 step = 1;
				 up_flag = 1;
			 }
			 else
			 {
				 step = 0;
				 up_flag = 0;
			 }
		
	  }
		else
		{
			step = 0;
			if(mag_buf[1] - mag_buf[0] > 2*D)
			{
			 up_flag = 1;
			}
			else
			{
				up_flag = 0;
			}
		}
	}
	else
	{
		step = 0;
	}
	
	

	return step;
}