liweiyan/shoe_mcu2.2.1_new_v2/app/app_overturn - 副本 (3).c

#include "usr_config.h"
#include "hal_battery.h"
#include "nrf_drv_saadc.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_led.h"
#include "app_overturn.h"
#include "hal_wearshoes.h"
#include "bll_imu.h"

#include "ble_comm.h"

#define SLIDE_WINDOW_LEN 5

//全功率
static const fml_imu_param_t game_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_ON,											//前脚 - 时间戳开启
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	.acc_odr 									  = FML_IMU_ACC_ODR_104HZ,										//前脚 - 加速度采样频率 - 104HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_104HZ,										//前脚 - 陀螺仪采样频率 - 104HZ
	.mag_odr 									  = FML_IMU_MAG_ODR_200HZ,										//前脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_104HZ,	
};

static const fml_imu_param_t game_back_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//后脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,											  //后脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//后脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//后脚 - 地磁计量程 - 30GS
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_200HZ,										//后脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,	
};

//低功耗
static const fml_imu_param_t shoes_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//前脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	.acc_odr 									  = FML_IMU_ACC_ODR_104HZ,										//前脚 - 加速度采样频率 - 104HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_OFF,										  //前脚 - 陀螺仪采样频率 关闭
	.mag_odr 									  = FML_IMU_MAG_ODR_100HZ,										//前脚 - 地磁计采样频率 - 100HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_104HZ,
};

static const fml_imu_param_t shoes_back_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//后脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//后脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//后脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//后脚 - 地磁计量程 - 30GS
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_OFF,										  //后脚 - 地磁计采样频率 - 200HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,				
};

//待机模式
static const fml_imu_param_t standby_front_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//前脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//前脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//前脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//前脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//前脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//前脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//前脚 - 地磁计量程 - 30GS
	.acc_odr 									  = FML_IMU_ACC_ODR_12HZ5,										//前脚 - 加速度采样频率 - 12.5HZ
	.gry_odr 									  = FML_IMU_GRY_ODR_OFF,										  //前脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 									  = FML_IMU_MAG_ODR_OFF,										  //前脚 - 地磁计采样频率 - 关闭
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,										  //前脚 - FIFO采样频率 - 104HZ
};

static const fml_imu_param_t standby_param={
	.acc_power_mode 						= FML_IMU_ACC_POWER_MODE_NORMAL,						//后脚 - 加速度正常模式
	.gry_power_mode 						= FML_IMU_GRY_POWER_MODE_NORMAL,						//后脚 - 陀螺仪正常模式
	.timestamp_resolution 			= FML_IMU_TIMESTAMP_25US,										//后脚 - 时间戳25US精度
	.timestamp_switch 					= FML_IMU_TIMESTAMP_OFF,										//后脚 - 时间戳关闭
	.acc_fs 										= FML_IMU_ACC_FS_16G,												//后脚 - 加速度量程 - 16G
	.gry_fs 										= FML_IMU_GRY_FS_2000DPS,										//后脚 - 陀螺仪量程 - 2000DPS
	.mag_fs 										= FML_IMU_MAG_FS_30GS,											//后脚 - 地磁计量程 - 30GS
	.acc_odr 										= FML_IMU_ACC_ODR_OFF,											//后脚 - 加速度采样频率 - 关闭
	.gry_odr 										= FML_IMU_GRY_ODR_OFF,											//后脚 - 陀螺仪采样频率 - 关闭
	.mag_odr 										= FML_IMU_MAG_ODR_10HZ,										  //后脚 - 地磁计采样频率 - 10HZ
	.fifo_odr 									= FML_IMU_FIFO_ODR_OFF,											//后脚 - FIFO采样频率 - 关闭			
};


typedef struct
{
	int16_t max_temp;
	uint16_t time_count;
}ACC_TIME_TYPE;

typedef struct
{
	ACC_TIME_TYPE slide_window[SLIDE_WINDOW_LEN];
	
	int window_length;
	
}SLIDE_WINDOW;

int cal_unsigned_D_value(uint16_t left_value, uint16_t right_value)
{
	if(left_value <= right_value)
	{
		return right_value - left_value;
	}
	else
	{
		return 65535-left_value + 1 + right_value;
	}
}

static uint8_t flag_move =0;

void sort_silde_window(SLIDE_WINDOW *max_slide_window, SLIDE_WINDOW *min_slide_window, int16_t acc_val, uint16_t	time_count)
{
	ACC_TIME_TYPE acc_time_type_temp = {acc_val, time_count};
	
	//检测当前的值是是否是最大值
	if(max_slide_window->window_length == 0)
	{
		max_slide_window->slide_window[0] = acc_time_type_temp;
		
		max_slide_window->window_length = 1;
	}
	else
	{
		//先判断长度是否溢出来了
		while(cal_unsigned_D_value(max_slide_window->slide_window[0].time_count, time_count)>= SLIDE_WINDOW_LEN)
		{
			memcpy(max_slide_window->slide_window, max_slide_window->slide_window + 1, (SLIDE_WINDOW_LEN - 1) *sizeof(ACC_TIME_TYPE));
			max_slide_window->window_length --;
		}
		
		//再检测新的元素需要前面的元素小
		while(max_slide_window->window_length > 0)
		{
			if(max_slide_window->slide_window[max_slide_window->window_length - 1].max_temp < acc_val)
			{
				max_slide_window->window_length --;
			}
			else
			{
				break;
			}
		}
		
		max_slide_window->slide_window[max_slide_window->window_length] = acc_time_type_temp;
		
		max_slide_window->window_length ++;
	}
	
	//检测当前的值是是否是最大值
	if(min_slide_window->window_length == 0)
	{
		min_slide_window->slide_window[0] = acc_time_type_temp;
		min_slide_window->window_length = 1;
	}
	else
	{
		//先判断长度是否溢出来了
		while(cal_unsigned_D_value(min_slide_window->slide_window[0].time_count, time_count)>= SLIDE_WINDOW_LEN)
		{
			memcpy(min_slide_window->slide_window, min_slide_window->slide_window + 1, (SLIDE_WINDOW_LEN - 1) *sizeof(ACC_TIME_TYPE));
			min_slide_window->window_length --;
		}
		
		//再检测新的元素需要前面的元素小
		while(min_slide_window->window_length > 0)
		{
			if(min_slide_window->slide_window[min_slide_window->window_length - 1].max_temp > acc_val)
			{
				min_slide_window->window_length --;
			}
			else
			{
				break;
			}
		}
		
		min_slide_window->slide_window[min_slide_window->window_length] = acc_time_type_temp;
		
		min_slide_window->window_length ++;
	}
}

/************************ 踮脚显示电量 ***********************************/

//没穿鞋的情况下

void app_BatDispaly_Process_N(void)        
{
		ser_imu_data_t data= {0};
	  static uint8_t lED_LIFE_SIGNAL = 0;
    static uint8_t lED_LIFE_TIME = 0;
		
//	DEBUG_LOG("app_BatDispaly_Process_N;\n");
		
		if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY))
		{
			return;//停止状态
		}

		//获取最新一组前脚加速度
		int16_t data_size = 0;
		
		data_size = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		
	  if(data_size > 0){
			
				hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, data_size -1 , &data);
				
				int32_t acc_norm = (data.acc[0] * data.acc[0] +data.acc[1] * data.acc[1] + data.acc[2] * data.acc[2]);
				
				if(abs(acc_norm - 4194304) < 362144 && data.acc[2] > 1536)
				{
					lED_LIFE_SIGNAL = 3;
				}
				else if(lED_LIFE_SIGNAL > 0)lED_LIFE_SIGNAL --;
				
				if(abs(acc_norm - 4194304) < 362144 && data.acc[2] < -1024)
				{ 
					lED_LIFE_SIGNAL = 0;
				}
				
//				DEBUG_LOG("acc_norm:%d,lED_LIFE_SIGNAL:%d,data.acc2:%d,lED_LIFE_TIME:%d\n",abs(acc_norm - 4194304),lED_LIFE_SIGNAL,data.acc[2],lED_LIFE_TIME);
				if(lED_LIFE_SIGNAL > 0)
				{
//					DEBUG_LOG("unwearshoes LED_Start(LED_OVERTURN,COLOR_GREEN);;\n");
					 if(lED_LIFE_TIME == 0){
								lED_LIFE_TIME = (10000/StandByPower_Interval);
								if(GetBatteryPersent()>20){
									LED_Start(LED_OVERTURN,COLOR_GREEN);
								}else{
									LED_Start(LED_OVERTURN,COLOR_ORANGE);
								}
								Process_SetHoldOn(app_BatDispaly_Process_N,1);
						 }
						 else if (2 == lED_LIFE_TIME){
							  LED_Stop(LED_OVERTURN);
						    Process_SetHoldOn(app_BatDispaly_Process_N,0);
						 }
						 if (lED_LIFE_TIME > 1)lED_LIFE_TIME--;
				}
				else
				{
//					DEBUG_LOG("unwearshoes LED_Stop(LED_OVERTURN);\n");
					if(0 != lED_LIFE_TIME ){lED_LIFE_TIME =0;
					   LED_Stop(LED_OVERTURN);
					   Process_SetHoldOn(app_BatDispaly_Process_N,0);
					}
					if(1 == flag_move){flag_move =0;
					 LED_Stop(LED_OVERTURN);
					 Process_SetHoldOn(app_BatDispaly_Process_N,0);
				  }
				}
	 }
}

//穿鞋的情况下
void app_BatDispaly_Process(void)
{
	  ser_imu_data_t data={0};
		
		static uint16_t	time_count = 0;
		
		static uint16_t station_count =0;
		static uint16_t overturn_180_count = 0;
		static uint8_t lED_LIFE_SIGNAL = 0;
    static uint8_t lED_LIFE_TIME = 0;
		
		static SLIDE_WINDOW max_slide_window = { {-3096,0},0 };
		static SLIDE_WINDOW min_slide_window = { {3096,0},0 };
		
		
		
		if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL))
		{
			return;//走路状态
		}			
//		DEBUG_LOG("app_BatDispaly_Process;\n");
		//获取最新一组前脚加速度, 有fifo
		int16_t data_size = 0;
		
		data_size = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		
	  if(data_size > 0){
			
//					DEBUG_LOG("data_size : %d,  hal_wearshoes_is_wearshoes: %d\n", data_size,hal_wearshoes_is_wearshoes());
		
					hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, data_size-1 , &data);
				
					//耗时较少的计算滑动窗口的最大值、最小值
					sort_silde_window(&max_slide_window, &min_slide_window,  data.acc[0], time_count);
					
					if(time_count > SLIDE_WINDOW_LEN)
					{
						if(data.acc[0] - min_slide_window.slide_window[0].max_temp < 256 &&  max_slide_window.slide_window[0].max_temp -  data.acc[0] < 256
							&& max_slide_window.slide_window[0].max_temp  - min_slide_window.slide_window[0].max_temp < 256 &&  data.acc[0] < -1400)
						{
							station_count++;
						}
						else station_count =0;
					}
					
					time_count++;
					
					//在穿鞋状态下，被翻转了
					int32_t acc_norm = (data.acc[0] * data.acc[0] +data.acc[1] * data.acc[1] + data.acc[2] * data.acc[2]);
					if(abs(acc_norm - 4194304) < 362144 && data.acc[2] > 1536)
					{
						//DEBUG_LOG(" wear shoes if(abs(acc_norm - 4194304) < 262144 && data.acc[2] > 1536);\n");
						overturn_180_count ++;
					}
					else
					{
						overturn_180_count = 0;
					}
					
					if(overturn_180_count >= 5 || station_count > 4)
					{
//						 DEBUG_LOG(" overturn_180_count > 5;\n");
						lED_LIFE_SIGNAL = 6;
					}
					
					if(lED_LIFE_SIGNAL > 0)lED_LIFE_SIGNAL --;
					
					if(lED_LIFE_SIGNAL > 0)
					{
//						DEBUG_LOG("wearshoes LED_Start(LED_OVERTURN,COLOR_GREEN);;\n");
						 if(lED_LIFE_TIME == 0){
								lED_LIFE_TIME = (10000/LowPower_Interval);
								if(GetBatteryPersent()>20){
									LED_Start(LED_OVERTURN,COLOR_GREEN);
								}else{
									LED_Start(LED_OVERTURN,COLOR_ORANGE);
								}
								Process_SetHoldOn(app_BatDispaly_Process,1);
						 }
						 else if (2 == lED_LIFE_TIME){
							  LED_Stop(LED_OVERTURN);
						    Process_SetHoldOn(app_BatDispaly_Process,0);
						 }
						 if (lED_LIFE_TIME > 1)lED_LIFE_TIME--;
					}
					else
					{
//						DEBUG_LOG("wearshoes LED_Stop(LED_OVERTURN);\n");
						if(0 != lED_LIFE_TIME ){lED_LIFE_TIME =0;
					    LED_Stop(LED_OVERTURN);
					    Process_SetHoldOn(app_BatDispaly_Process,0);
					  }
						if(0 == flag_move){flag_move =1;
							 LED_Stop(LED_OVERTURN);
							 Process_SetHoldOn(app_BatDispaly_Process_N,0);
				    }
					}
		}
		
}


void app_overturn_Init(void)
{
	Process_Start(LowPower_Interval,"BatDispaly",app_BatDispaly_Process);
	Process_Start(StandByPower_Interval,"BatDispaly_N",app_BatDispaly_Process_N);
	fml_imu_mode_manage_imu_full_power_init(&app1_config_param.config_param[BLL_IMU_DIR_FRONT], &app1_config_param.config_param[BLL_IMU_DIR_BACK]);
	
}