提交实时计步和日常计步最新的代码

detect_step_by_mag.c

@@ -57,9 +57,6 @@ int32_t Get_RealBackLiftPoint(void)
 
 uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 {
-	
-	
-
 	//static int FrontLiftPoint;
 	//static int FrontDropPoint;
 	//static int BackLiftPoint;
@@ -82,7 +79,7 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	float D;
 	//static int16_t Ucounter=0;//离地计时器
 	
-	SEGGER_RTT_printf(0,"Enter RealTimeStep!\n");
+	//SEGGER_RTT_printf(0,"Enter RealTimeStep!\n");
 	Tcounter++;
 	
 	for(int i = 0; i < 3; i ++)
@@ -105,7 +102,6 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 		mag_buf_front[1] = mag_buf_front[2];
 		mag_index_front = 2;
 		calculate_flag = 1;
-		
 	}
 	mag_buf_front[mag_index_front++] = mag_sqrt_front;//前脚掌数据
 	
@@ -130,9 +126,6 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	accZ_buf[acc_index++] = -acc[2];
 	
 	
-	//SEGGER_RTT_printf(0,"mag_index_front==:%d...\n",mag_index_front);
-	//SEGGER_RTT_printf(0,"mag_index_back==:%d...\n",mag_index_back);
-	//SEGGER_RTT_printf(0,"calculate_flag==:%d...\n",calculate_flag);
 	
 	if(calculate_flag==1)
 	{
@@ -144,17 +137,17 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  /*不断更新前后脚掌磁力计的上升起始点或下降起始点*/
 	  if(mag_buf_front[2] - mag_buf_front[1]>=0)
 	  {
-			//if(FKEY==0)
-			//{
+			if(FKEY==0)
+			{
         FrontDropPoint = mag_buf_front[2];
-			//}
-			//else
-			//{
-				//if(mag_buf_front[2]>FrontDropPoint)
-				//{
-					//FrontDropPoint = mag_buf_front[2];
-				//}
-			//}
+			}
+			else
+			{
+				if(mag_buf_front[2]>FrontDropPoint)
+				{
+					FrontDropPoint = mag_buf_front[2];
+				}
+			}
 	  }
     else
     {
@@ -170,17 +163,17 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 
     if(mag_buf_back[2] - mag_buf_back[1]>=0)
 	  {
-			//if(BKEY==0)
-			//{
+			if(BKEY==0)
+			{
         BackDropPoint = mag_buf_back[2];
-			//}
-			//else
-			//{
-				//if(mag_buf_back[2]>BackDropPoint)
-				//{
-					//BackDropPoint = mag_buf_back[2];
-				//}
-			//}
+			}
+			else
+			{
+				if(mag_buf_back[2]>BackDropPoint)
+				{
+					BackDropPoint = mag_buf_back[2];
+				}
+			}
 	  }
     else
     {
@@ -198,38 +191,34 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
     if(mag_buf_front[2] - FrontLiftPoint>2200)
 	  {
 			FKEY = 1;
-			/*
 			if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance)
 			{
 			  FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint;
-			}
-			*/
+			}		
 	  }
 	  if(mag_buf_back[2] - BackLiftPoint>2200)
 	  {
-			BKEY = 1;
-			/*
+			BKEY = 1;		
 			if(mag_buf_back[2] - BackLiftPoint>BackLiftDistance)
 			{
 			  BackLiftDistance = mag_buf_back[2] - BackLiftPoint;
-			}
-			*/
+			}			
 	  }
 		/*判断是否满足FKEY=0和BKEY=0，即是否满足一个升降波形*/
 		
-		//if((mag_buf_front[2] - FrontLiftPoint<=1500)||(FrontDropPoint-mag_buf_front[2]>0.68*FrontLiftDistance)) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
-		if(mag_buf_front[2] - FrontLiftPoint<=1500)
+		if((mag_buf_front[2] - FrontLiftPoint<=1500)||(FrontDropPoint-mag_buf_front[2]>0.45*FrontLiftDistance)) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
+		//if(mag_buf_front[2] - FrontLiftPoint<=1500)
 		{
 			FKEY=0;
-			//FrontLiftDistance = 2200;
+			FrontLiftDistance = 2200;
 		}
 		
 	  
-		//if((mag_buf_back[2] - BackLiftPoint<=1500)||(BackDropPoint-mag_buf_back[2]>0.68*BackLiftDistance))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
-		if(mag_buf_back[2] - BackLiftPoint<=1500)
+		if((mag_buf_back[2] - BackLiftPoint<=1500)||(BackDropPoint-mag_buf_back[2]>0.45*BackLiftDistance))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
+		//if(mag_buf_back[2] - BackLiftPoint<=1500)
 		{
 			BKEY=0;
-			//BackLiftDistance = 2200;
+			BackLiftDistance = 2200;
 		}
     
 		
@@ -249,12 +238,16 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  
 	  if(ShoeTouchFlag==0)
 	  {
-			Ucounter++;
+			if(Ucounter<300)
+			{
+			 Ucounter++;
+			}
 			Rcounter=0;		
 			if(((FKEY==1)||(BKEY==1))&&(Ucounter>=6))
 		  {
 			  step=1;
 			  ShoeTouchFlag=1;
+				SEGGER_RTT_printf(0,"You       have           one             step!\n");
 		  }
 		  else
 		  {
@@ -265,27 +258,34 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  else
 	  {
 		  step = 0;
-			Rcounter++;
+			if(Rcounter<300)
+			{
+			 Rcounter++;
+			}
 			Ucounter=0;
 			if((Rcounter>=6)&&(FKEY==0)&&((abs(accZ_buf[0]-2000)>250)||(abs(accZ_buf[1]-2000)>250)||(abs(accZ_buf[2]-2000)>250))&&(BKEY==0))
 		  {
 			  ShoeTouchFlag=0;
-				Lock=0;
 		  }
 		  else
 		  {
 			  ShoeTouchFlag=1;
 		  }
-
-      if((Rcounter>3*100)&&(Lock==0))//防止卡死
+      /*
+      if(Rcounter==300)//防止卡死
 			{
-				FrontLiftPoint= FrontLiftPoint + 3000;
-				BackLiftPoint = BackLiftPoint + 3000;
-				Lock=1;
-			}			
-			
+				if(FrontDropPoint-mag_buf_front[2]>0.45*FrontLiftDistance)
+				{
+					FKEY=0;
+				}
+				if(BackDropPoint-mag_buf_back[2]>0.45*BackLiftDistance)
+				{
+					BKEY=0;
+				}				
+			}		
+      */			
 	  }
-		
+		SEGGER_RTT_printf(0,"Rcounter==:%d...\n",Rcounter);
 	  /*计算加速度的方差*/
 		/*
 	  MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;

process_result.c

@@ -0,0 +1,329 @@
+#include "process_result.h"
+#include "hal_mt.h"
+#include "app_flash.h"
+#include "app_switchimu.h"
+
+#include "math.h"
+
+#include "detect_step_by_mag.h"
+
+float gyr[3];
+float acc[3];
+float mag[3];
+
+
+int16_t accel[3];
+int16_t gyro[3];
+int16_t front_magn[3];
+int16_t back_magn[3];
+
+int32_t pos[3];
+int16_t att[3];
+
+int16_t zupt;
+
+int8_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;
+
+uint16_t front_mag_norm;
+
+uint16_t back_mag_norm;
+
+uint32_t step_count;
+
+int16_t ShoeTouchFlag2;
+
+int16_t Rcounter2;
+int16_t Tcounter=0;
+int16_t FKEY2;
+int16_t BKEY2;
+//int32_t RealFrontLiftPoint=30000;
+//int32_t RealBackLiftPoint=30000;
+int RealFrontLiftPoint;
+int RealBackLiftPoint;
+
+float gyr[3];
+float acc[3];
+float mag[3];
+
+int low_power_dectect;
+int low_power_step;
+uint16_t low_power_mag;
+
+int zupt_wait_time;
+
+int nan_flag = 0;
+
+//left
+//float acc_bias[3] = {0.091139f, 0.101830f, -0.048524f};
+//float acc_scale[3] =  {0.980197f,   1.003513f,  0.987654f};
+//right
+float acc_bias[3] = {0.047714f, 0.026054f, -0.008199f};
+float acc_scale[3] =  {1.003853f,   1.007566f, 0.986378f};
+
+
+void foot_data_to_package()
+{
+	static uint16_t package_time = 0;
+	
+	foot_data_len = 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)(RealFrontLiftPoint>>24);
+	//foot_data_buf[foot_data_len++] = (uint8_t)(RealFrontLiftPoint>>16);
+	//foot_data_buf[foot_data_len++] = (uint8_t)(RealFrontLiftPoint>>8);
+	//foot_data_buf[foot_data_len++] = (uint8_t)(RealFrontLiftPoint>>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)(RealBackLiftPoint>>24);
+	//foot_data_buf[foot_data_len++] = (uint8_t)(RealBackLiftPoint>>16);
+	//foot_data_buf[foot_data_len++] = (uint8_t)(RealBackLiftPoint>>8);
+	//foot_data_buf[foot_data_len++] = (uint8_t)(RealBackLiftPoint>>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)(FKEY2>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(FKEY2>>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)(ShoeTouchFlag2>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(ShoeTouchFlag2>>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)(BKEY2>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(BKEY2>>0);
+	
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_mag_norm>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_mag_norm>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_mag_norm>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_mag_norm>>0);
+ 	
+	foot_data_buf[foot_data_len++] = (uint8_t)(zupt>>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)(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)(step_count>>24);
+	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>16);
+	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>0);
+	
+//	SEGGER_RTT_printf(0,"test footPDR \n");
+	
+	package_time ++;
+}
+
+void imu_data_to_package()
+{
+	static uint16_t package_time = 0;
+	
+	foot_data_len = 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)(gyro[0]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[0]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[1]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[1]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[2]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[2]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[0]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[0]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[1]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[1]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[2]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[2]>>0);
+	
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[0]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[0]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[1]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[1]>>0);
+	
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[2]>>8);
+	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[2]>>0);
+	
+//	SEGGER_RTT_printf(0," left_sh , foot_data_len = %d\n", foot_data_len);
+	
+	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(int32_t time_stamp, int16_t _acc[3], int16_t _gry[3],  int16_t front_mag[3], int16_t back_mag[3], uint8_t _rssi)
+{
+	/*
+		解析数据
+	*/
+	//static int16_t 
+	if(isnan(_acc[0]) || isnan(_acc[1]) || isnan(_acc[2]) || isnan(_gry[0]) || isnan(_gry[1]) || isnan(_gry[2]) )
+	{
+		nan_flag = 1;
+	}
+	
+	if(nan_flag == 1)
+	{
+		SEGGER_RTT_printf(0,"read nan from IMU", foot_data_len);
+	}
+	
+
+	memcpy(accel, _acc, 3 * sizeof(int16_t));
+	
+	memcpy(gyro , _gry, 3 * sizeof(int16_t));
+	
+	memcpy(front_magn , front_mag, 3 * sizeof(int16_t));
+	
+	memcpy(back_magn,	 back_mag, 3 * sizeof(int16_t));
+	
+	slave_rssi = _rssi - 8;
+	
+	if(slave_rssi < 0)
+	{
+		slave_rssi = 0;
+	}
+	
+	for(int i = 0; i < 3; i++)
+	{
+		gyr[i] = gyro[i] / GYR_LSB;
+	}
+	
+	for(int i = 0; i < 3; i++)
+	{
+		acc[i] = accel[i] / ACC_LSB;
+		
+//		acc[i] = (acc[i] + acc_bias[i]) * acc_scale[i];
+	}
+
+	
+	
+////	SEGGER_RTT_printf(0,"PDR gyro[0]:%d ;gyro[1] : %d ;gyro[2]: %d \n", gyro[0], gyro[1],gyro[2]);
+
+//	//SEGGER_RTT_printf(0,"PDR acc[0]:%d ;acc[1] : %d ;acc[2]: %d \n", (int)(acc[0]*1000), (int)(acc[1]*1000),(int)(acc[2]*1000));
+	
+	front_mag_norm = (uint16_t)(abs(front_magn[2]));
+	
+	back_mag_norm  = (uint16_t)(abs(back_magn[2]));
+//	
+//	//SEGGER_RTT_printf(0,"front_mag_norm : %d; back_mag_norm : %d; front_magn[0] : %d; front_magn[1] : %d; front_magn[2] : %d \r\n", front_mag_norm, back_mag_norm,front_magn[0],front_magn[1],front_magn[2]);
+//	
+//	//检测零速
+//	detect_zero_vel(front_magn, back_magn, accel, &front_zero, &back_zero, &acc_zero);
+
+//	//轨迹计算
+//	if(front_zero > 0 || back_zero > 0 )
+//	{
+//		zupt = 1;
+//	}
+//	else
+//	{
+//		zupt = 0;
+//	}
+//	
+//	footPDR(time_stamp, gyr, acc, 0 , zupt, acc_zero, pos, att);
+
+
+//	//SEGGER_RTT_printf(0,"pos[0] : %d; pos[1] : %d ; pos[2] : %d \r\n", pos[0] , pos[1],  pos[2]);
+//	
+//	//SEGGER_RTT_printf(0,"gyro[0] : %d; gyro[1] : %d ; gyro[2] : %d \r\n", gyro[0] , gyro[1],  gyro[2]);
+//	
+//		//35号左鞋安装有误差，软件来补锅
+////	float pos_tmp[3];
+////	
+////	pos_tmp[0] = -pos[1] * 0.01f;
+////	pos_tmp[1] = pos[0] * 0.01f;
+
+
+////	
+////	pos[0] = (int32_t)((0.9397 * pos_tmp[0] + 0.3420 *  pos_tmp[1])*100.0f);
+////	pos[1] =  (int32_t)((-0.3420 * pos_tmp[0] + 0.9397 *  pos_tmp[1])*100.0f);
+//	
+//	
+//	
+//	int32_t  pos_tmp[3];
+
+//	memcpy(pos_tmp, pos, 3 * sizeof(int32_t));
+
+//	pos[0] = -pos_tmp[1];
+//	pos[1] = pos_tmp[0];
+
+//	accel[2] = -accel[2]; 
+  int16_t result;
+	if(RealTimeStep(front_mag, back_mag,_acc))
+	//if(detect_step_by_mag(front_mag, _acc[2]))
+	{
+		step_count++;
+	}
+	SEGGER_RTT_printf(0,"step_count: %d \r\n", step_count);
+	
+	ShoeTouchFlag2 = Get_ShoeTouchFlag();
+	Rcounter = Get_Rcounter();
+	Tcounter = Get_Tcounter();
+	RealFrontLiftPoint=Get_RealFrontLiftPoint();
+  RealBackLiftPoint=Get_RealBackLiftPoint();
+  FKEY2=Get_FKEY();
+	BKEY2=Get_BKEY();
+
+	foot_data_to_package();	
+
+	
+}