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

detect_step_by_mag.c

@@ -19,14 +19,20 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	static int16_t FrontDropPoint;
 	static int16_t BackLiftPoint;
 	static int16_t BackDropPoint;
+	static int16_t FrontMagLevel=0;//前脚电平
+	static int16_t BackMagLevel=0;//后脚电平
 	static int32_t SD;
-	static int16_t accl;
+	static int16_t counter;
+	static int16_t BeginToCount;
+	static int16_t FOB;//Front(1)orBack(2)
 	int32_t mag_temp_front[3];
 	int32_t mag_temp_back[3];
 	static int16_t accZ_buf[3];
 	static uint8_t acc_index = 0;
-	static uint16_t mag_buf_front[3];
-	static uint16_t mag_buf_back[3];
+	//static uint16_t mag_buf_front[3];
+	//static uint16_t mag_buf_back[3];
+	static int mag_buf_front[3];
+	static int mag_buf_back[3];
 	static uint8_t mag_index_front = 0;
 	static uint8_t mag_index_back = 0;
 	int32_t mag_sqrt_front =0;
@@ -43,13 +49,14 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	{
 		mag_temp_front[i] = (int32_t) (front[i]);
 	}
-	mag_sqrt_front  = (uint16_t)(sqrt((float) (mag_temp_front[0] * mag_temp_front[0] + mag_temp_front[1] * mag_temp_front[1] + mag_temp_front[2] * mag_temp_front[2])));
-		
+	//mag_sqrt_front  = (uint16_t)(sqrt((float) (mag_temp_front[0] * mag_temp_front[0] + mag_temp_front[1] * mag_temp_front[1] + mag_temp_front[2] * mag_temp_front[2])));
+	mag_sqrt_front = 	abs(mag_temp_front[2]);
 	for(int i = 0; i < 3; i ++)
 	{
 		mag_temp_back[i] = (int32_t) (back[i]);
 	}
-	mag_sqrt_back  = (uint16_t)(sqrt((float) (mag_temp_back[0] * mag_temp_back[0] + mag_temp_back[1] * mag_temp_back[1] + mag_temp_back[2] * mag_temp_back[2])));
+	//mag_sqrt_back  = (uint16_t)(sqrt((float) (mag_temp_back[0] * mag_temp_back[0] + mag_temp_back[1] * mag_temp_back[1] + mag_temp_back[2] * mag_temp_back[2])));
+	mag_sqrt_back = abs(mag_temp_back[2]);
 	
 //	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);
 
@@ -109,10 +116,12 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
     if(mag_buf_front[1] - FrontLiftPoint>2200)
 	  {
 		  FrontTouchFlag = 2;
+			//FrontMagLevel = 1;//high level
 	  }
 	  else if(mag_buf_front[1] - FrontDropPoint<-1000)
 	  {
 		  FrontTouchFlag = 0;
+			//FrontMagLevel = 0;//low level
 	  }
 	  else if((abs(mag_buf_front[1]-mag_buf_front[0])<500)&&(abs(mag_buf_front[1]-mag_buf_front[2])<500))
 	  {
@@ -122,10 +131,12 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  if(mag_buf_back[1] - BackLiftPoint>2200)
 	  {
 		  BackTouchFlag = 2;
+			//BackMagLevel = 1;//high level
 	  }
 	  else if(mag_buf_back[1] - BackDropPoint<-1000)
 	  {
 		  BackTouchFlag = 0;
+			//BackMagLevel = 0;//low level
 	  }
 	  else if((abs(mag_buf_back[1]-mag_buf_back[0])<500)&&(abs(mag_buf_back[1]-mag_buf_back[2])<500))
 	  {
@@ -136,13 +147,23 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	
 	  if(ShoeTouchFlag==0)
 	  {
-		  if((FrontTouchFlag==2||BackTouchFlag==2)&&(SD>60))
-			//if((FrontTouchFlag==2||BackTouchFlag==2))
+		  //if((FrontTouchFlag==2||BackTouchFlag==2)&&((accZ_buf[0]>2150)||accZ_buf[1]>2150||accZ_buf[2]>2150))
+			//if((((FrontTouchFlag==2)&&(FrontMagLevel==0))||((BackTouchFlag==2)&&(BackMagLevel==0)))&&(SD>60))
+			if(FrontTouchFlag==2||BackTouchFlag==2)
 		  {
 			  step=1;
 			  ShoeTouchFlag=1;
-			  accl = 0;
-				
+			  counter = 0;
+				BeginToCount = 0;
+				if(FrontTouchFlag==2)
+				{
+					FOB = 1;
+				}
+				if(BackTouchFlag==2)
+				{
+					FOB = 2;
+				}
+				/*
 				SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf_front[2]);
 		    SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf_front[1]);
 		    SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf_front[0]);
@@ -152,6 +173,7 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 		    SEGGER_RTT_printf(0,"mag_back1==:%d...\n",mag_buf_back[0]);
 		
 	      SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
+				
 		    SEGGER_RTT_printf(0,"acc2==:%d...\n",accZ_buf[1]);
 		    SEGGER_RTT_printf(0,"acc1==:%d...\n",accZ_buf[0]);
 				
@@ -161,8 +183,7 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 		    SEGGER_RTT_printf(0,"BackDropPoint==:%d...\n",BackDropPoint);
 				
 				SEGGER_RTT_printf(0,"SD==:%d...\n",SD);
-				
-				
+        */				
 		  }
 		  else
 		  {
@@ -173,14 +194,20 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  else
 	  {
 		  step=0;
-		  //if(abs(accZ_buf[0]-2000)<600||abs(accZ_buf[1]-2000)<600||abs(accZ_buf[2]-2000)<600)
-			if(abs(accZ_buf[0]-2000)<600&&abs(accZ_buf[1]-2000)<600&&abs(accZ_buf[2]-2000)<600)
-		  {
-			  accl++;
-		  }
-		  if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(accl>=2)&&(abs(accZ_buf[2]-2000)>500))
+			if(((FOB==1)&&(FrontTouchFlag!=2))||((FOB==2)&&(BackTouchFlag!=2)))
+			{
+			  BeginToCount=1;
+			}	
+			if(BeginToCount==1)
+			{
+				counter++;
+			}
+			if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800)&&(counter>=2))
 		  {
 			  ShoeTouchFlag=0;
+				FOB = 0;
+				//FrontMagLevel = 0;
+				//BackMagLevel = 0;
 		  }
 		  else
 		  {
@@ -213,15 +240,11 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	int32_t mag_temp[3];
 	static int16_t accZ_buf[3];
 	static uint8_t acc_index = 0;
-	static uint16_t mag_buf[3];
+	//static uint16_t mag_buf[3];
+	static int mag_buf[3];
 	static uint8_t mag_index = 0;
-	static int16_t K = 30000;
-	static int16_t counter = 0;
-	float min;
-	float max;
 	int step;
 	int calculate_flag = 0;
-	int accZ_flag;
   static int16_t FrontLiftPoint;
 	static int16_t FrontDropPoint;
 	static int16_t FrontTouchFlag=0;
@@ -235,8 +258,8 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	{
 		mag_temp[i] = (int32_t) (mag[i]);
 	}
-	mag_sqrt  = (uint16_t)(sqrt((float) (mag_temp[0] * mag_temp[0] + mag_temp[1] * mag_temp[1] + mag_temp[2] * mag_temp[2])));
-	
+	//mag_sqrt  = (uint16_t)(sqrt((float) (mag_temp[0] * mag_temp[0] + mag_temp[1] * mag_temp[1] + mag_temp[2] * mag_temp[2])));
+	mag_sqrt = abs(mag_temp[2]);
 //	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);
 	/*读入磁力计数据*/
 	if(mag_index >= 3)
@@ -257,12 +280,11 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	}
 	accZ_buf[acc_index++] = -acc_z;
 
-
+	
 	/*开始计算*/
 	if (calculate_flag == 1)
 	{
-
-		/*
+		
 		static uint32_t time =0;
 		if(TIME_GetTicks() - time >=1000){
 			time = TIME_GetTicks();
@@ -277,7 +299,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		SEGGER_RTT_printf(0,"f_mx=%d,f_my=%d,f_mz=%d,acc_z:%d\r\n",mag[0],mag[1],mag[2],acc_z);
 		
 		}
-		*/
+		
 				 
 		if(mag_buf[1] - mag_buf[0]>=0)
 	  {
@@ -289,7 +311,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	  }
 		
 		/*判断前后脚掌的状态*/
-    if(mag_buf[1] - FrontLiftPoint>2200)
+    if(mag_buf[1] - FrontLiftPoint>1500)
 	  {
 		  FrontTouchFlag = 2;
 	  }
@@ -305,14 +327,12 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		/*计算步数*/
 	  if (up_flag == 0)
 	  {
-
-			 if(FrontTouchFlag==2)
+			 if((FrontTouchFlag==2)&&(SD>60))
 			 {
 				 step = 1;
 				 up_flag = 1;
 				 SEGGER_RTT_printf(0,"step==%d\n",step);
-				 counter = 0;
-				 /*
+				 
 				 SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
 		     SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf[1]);
 		     SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf[0]);
@@ -326,27 +346,29 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		     SEGGER_RTT_printf(0,"FrontDropPoint==:%d...\n",FrontDropPoint);
 	 
 		     SEGGER_RTT_printf(0,"SD==:%d...\n",SD);
-         */
+//				 static char string[50];
+//				 sprintf(string,"1 min=%f,mag_buf=%d\n",min,mag_buf[2]);
+//				 SEGGER_RTT_printf(0,"%s",string);
+//				 SEGGER_RTT_printf(0,"step==%d\n",step);
+//				 nrf_delay_ms(1000);
+//				 while(1);
 			 }
 			 else
 			 {
 				 step = 0;
 				 up_flag = 0;
+//				 nrf_delay_ms(1000);
+//		     NVIC_SystemReset();
 			 }
 		
 	  }
 		else
 		{
 			step = 0;
-			if(abs(accZ_buf[0]-2000)<600&&abs(accZ_buf[1]-2000)<600&&abs(accZ_buf[2]-2000)<600)
-		  {
-			  counter++;
-		  }			
-			if((counter>=2)&&(abs(accZ_buf[2]-2000)>500))
+			if((FrontTouchFlag!=2)&&(accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))
 			{
-				counter=0;
-				up_flag = 0;
-			}
+				up_flag=0;
+			}	
 		}
 		
 		MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;

日常计步代码.txt

@@ -1 +0,0 @@
-。。。。。。。。。。。。