日常计步代码提交

detect_step_by_mag.c

@@ -0,0 +1,136 @@
+#include "detect_step_by_mag.h"
+#include "math.h"
+#include "stdlib.h"
+#include "hal_imu.h"
+#include "hal_attitude.h"
+
+uint16_t mag_sqrt =0;
+uint8_t detect_step_by_mag(int16_t *mag)
+{
+	static uint8_t up_flag = 0;
+	int32_t mag_temp[3];
+	static int16_t accZ_buf[3];
+	static uint8_t acc_index = 0;
+	static uint16_t mag_buf[3];
+	static uint8_t mag_index = 0;
+	int16_t D;
+	static int16_t K = 30000;
+	static int16_t counter = 0;
+	float min;
+	float max;
+	int step;
+	int calculate_flag = 0;
+	int accZ_flag;
+
+	//SEGGER_RTT_printf(0,"mag_sqrt==:%d...\n",1000);
+	D =-1500;
+	for(int i = 0; i < 3; i ++)
+	{
+		mag_temp[i] = (int32_t) (mag[i]);
+	}
+	mag_sqrt  = (uint16_t)(sqrt(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,%d,%d...\n",mag[0],mag[1],mag[2]);
+	
+	/*读入磁力计数据*/
+	if(mag_index >= 3)
+	{
+		mag_buf[0] = mag_buf[1];
+		mag_buf[1] = mag_buf[2];
+		mag_index = 2;
+		calculate_flag = 1;
+	}
+	mag_buf[mag_index++] = mag_sqrt;
+	//SEGGER_RTT_printf(0,"mag_buf0==:%d...\n",mag_buf[0]);
+	//SEGGER_RTT_printf(0,"mag_buf1==:%d...\n",mag_buf[1]);
+	//SEGGER_RTT_printf(0,"mag_buf2==:%d...\n",mag_buf[2]);
+	
+  /*读入xyZ轴加速度数据*/
+	if(acc_index >= 3)
+	{
+		accZ_buf[0] = accZ_buf[1];
+		accZ_buf[1] = accZ_buf[2];
+		acc_index = 2;
+	}
+	accZ_buf[acc_index++] = -hal_att_GetAcc_Z();
+	//SEGGER_RTT_printf(0,"accZ_buf0==:%d...\n",accZ_buf[0]);
+	//SEGGER_RTT_printf(0,"accZ_buf1==:%d...\n",accZ_buf[1]);
+	//SEGGER_RTT_printf(0,"accZ_buf2==:%d...\n",accZ_buf[2]);
+	/*计算站稳时的磁力计数值*/
+	/*
+	if((abs(accZ_buf[2]-accZ_buf[1])<50)&&(abs(accZ_buf[1]-accZ_buf[0])<50))
+	{
+		mag_stable = (mag_buf[0]+mag_buf[1]+mag_buf[2])/3;
+		SEGGER_RTT_printf(0,"mag_stable==:%d...\n",mag_stable);
+	}
+	*/
+	
+	//SEGGER_RTT_printf(0,"mag_buf[2]==:%d...accZ_buf[3] = %d \n",mag_sqrt, IMU_GetAcc_Z());
+	/*判断z轴加速度*/
+	accZ_flag = 0;
+	if((abs(accZ_buf[2]-accZ_buf[1])>100)||(abs(accZ_buf[1]-accZ_buf[0])>100)||(abs(accZ_buf[2]-accZ_buf[0])>100))
+	{
+		accZ_flag = 1;
+	}
+	/*开始计算*/
+	if (calculate_flag == 1)
+	{
+		/*求取最大值和最小值*/
+		max = mag_buf[0];
+		min = mag_buf[0];
+		
+		
+		for(int i=0;i<2;i++)
+		{
+		   if(max<mag_buf[i])
+       {
+				 max = mag_buf[i];
+			 } 				 
+			 
+			 if(min>mag_buf[i])
+			 {
+				 min = mag_buf[i];
+			 }
+			
+		}
+		/*计算步数*/
+	  if (up_flag == 0)
+	  {
+		   //if((mag_buf[2] - min > 1600)&&(accZ_flag==1))//min
+			 //K = mag_buf[2] - min;
+			 //SEGGER_RTT_printf(0,"K==%d\n",K);
+			  counter = 0;
+			 if((mag_buf[2] - min > 700)&&(accZ_flag==1))
+			 {
+				 step = 1;
+				 up_flag = 1;
+				 K = min;
+				 SEGGER_RTT_printf(0,"step==%d\n",step);
+			 }
+			 else
+			 {
+				 step = 0;
+				 up_flag = 0;
+			 }
+		
+	  }
+		else
+		{
+			step = 0;
+			counter++;
+			if(counter==15)
+			{
+				counter=0;
+				up_flag = 0;
+			}
+		}
+	}
+	else
+	{
+		step = 0;
+	}
+	
+	
+
+	return step;
+}

detect_step_by_mag.h

@@ -0,0 +1,22 @@
+#ifndef __DETECT_STEP_BY_MAG_H__
+#define __DETECT_STEP_BY_MAG_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include "sdk_common.h"
+#include "SEGGER_RTT.h"
+#include "usr_config.h"
+#include "nrf_gpio.h"
+
+
+
+uint8_t detect_step_by_mag(int16_t *mag);
+
+
+
+
+
+
+
+#endif