liweiyan/shoe_mcu_2022_2_22_v3/hal/hal_wearshoes.c

#include "hal_wearshoes.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_ser_imu_mode_manage.h"
#include "tool.h"

#define HAL_WEARSHOES_PROCESS_CYCLE								StandByPower_Interval											//线程周期，单位ms
#define HAL_WEARSHOES_TIMEOUT											(60000/StandByPower_Interval)							//检测周期，当前10S检测一次。

static uint8_t 	isWearShoes = WEARSHOES_YES;


/**
	@brief 计算三轴的变异系数来判断波动大小是否符合抖动
	@param accel - [in] 加速度三轴
	@return 无
*/
static void mode_switch(int32_t mag_x, int32_t mag_y, int32_t mag_z, uint16_t timeout)
{
	static float32_t mag_cur_shake = 0;
	static uint32_t counter = 0;
	uint8_t flag = 0;
	double array[3];

	array[0] = mag_x;
	array[1] = mag_y;
	array[2] = mag_z;
	
	mag_cur_shake = CoefficientVariation(array, 3);

	
//	static char string[200];
//	sprintf(string,"mag_cur_shake:%f \r\n",mag_cur_shake);
//	SEGGER_RTT_printf(0,"%s",string);
	
	if((mag_cur_shake) >= USED_MAG_CHECK_WEARSHOES_VALUE){
		flag = 1;	
	}

	if(isWearShoes && (counter >= timeout))	{
		isWearShoes = WEARSHOES_NO;
		SEGGER_RTT_printf(0,"I am not WearShoes\r\n");
	}
	else if(!isWearShoes && flag){
		isWearShoes = WEARSHOES_YES;
		SEGGER_RTT_printf(0,"I am  WearShoes\r\n");
	}
	
	if(flag)counter = 0;
	else counter++;
	
}

static void hal_wearshoes_determine(uint16_t timeout)
{
	int16_t 					Mag[3]={0,0,0};
	int32_t 					mag_norm;
	ser_imu_data_t		data;
	
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) != -1){
//		//获取最新一组前脚加速度
//		if(IMU_Get_Front_Data_Num() >= 1)IMU_Get_Front_Data(IMU_Get_Front_Data_Num()-1, NULL, Acc, NULL, NULL);
		if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1)hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
		Mag[0] = data.mag[0];Mag[1] = data.mag[1];Mag[2] = data.mag[2];
//		SEGGER_RTT_printf(0,"hal_wearshoes_determine mag[0]:%d mag[1]:%d mag[2]:%d\r\n",Mag[0],Mag[1],Mag[2]);
	}
	else if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1){
		if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT) >= 1){
			hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, 0, &data);
			Mag[0] = data.mag[0];Mag[1] = data.mag[1];Mag[2] = data.mag[2];
		}
	}

	mag_norm  = (int32_t)(sqrt((float) (Mag[0] * Mag[0] + Mag[1] * Mag[1] + Mag[2] * Mag[2])));	
	mode_switch(mag_norm,~mag_norm,0, timeout);

}

void hal_wearshoes_Process(void)
{
	if((hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1) && isWearShoes == WEARSHOES_NO)
	{
		//切换为待机模式
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);											
	}
	else if((hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) != -1) && isWearShoes == WEARSHOES_YES)
	{
		//切换为日常模式
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_OFF);	
	}
	else if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_SELF_CHECK) == -1 && \
					hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) == -1 && \
					hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) == -1
				)
	{
		hal_wearshoes_determine(HAL_WEARSHOES_TIMEOUT);					//检测是否穿鞋
	}
}


uint8_t hal_wearshoes_is_wearshoes(void)
{
	return isWearShoes;
}

void hal_wearshoes_Init(void)
{
	if(INIT_MODE == 1){
		isWearShoes = WEARSHOES_NO;
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_STANDBY, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
	}else if(INIT_MODE == 2){
		isWearShoes = WEARSHOES_YES;
		hal_ser_imu_mode_manage_set_required(HAL_SER_IMU_MODE_MANAGE_NORMAL, HAL_SER_IMU_MODE_MANAGE_IMU_MODE_ON);
	}
	
	
	Process_Start(HAL_WEARSHOES_PROCESS_CYCLE,"hal_wearshoes_Process",hal_wearshoes_Process);
}