提交脚环的SDK

footRing_sdk_1.0/include/Aerobics.h

@@ -0,0 +1,72 @@
+
+
+#include "PublicSDKMotion.h"
+
+#include <deque>
+#define PRESS_WINDOW_SIZE 10
+
+//struct same_position_press
+//{
+//	int32_t left_mag;
+//	int32_t right_mag;
+//};
+
+class Aerobics : public PublicSDKMotion
+{
+public:
+	Aerobics();
+
+	void setResult(deque<shoes_data_cell>& step_data_vector, int LEFT_OR_RIGHT);
+
+	void ResetDenyDownTime(int time_stamp);
+
+	bool isValidDown(int time_stamp);
+
+	//float calVar(deque<same_position_press>& front_mag_deq);
+
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+
+	const float high_thresh = 0.1f;
+	const float plane_thresh = 0.2f;
+	
+	deque<shoes_data_cell> left_shoes_data_vector;
+	deque<shoes_data_cell> right_shoes_data_vector;
+
+	deque<shoes_data_cell> left_step_data_vector;
+	deque<shoes_data_cell> right_step_data_vector;
+
+	int last_down;
+	int last_jump;
+
+	int stretch_left_status;
+	int stretch_right_status;
+
+	int stretch_front_left_status;
+	int stretch_front_right_status;
+
+	int lift_left_status;
+	int lift_right_status;
+
+	int left_wait_time;
+	int right_wait_time;
+
+	int special_down;
+	int last_special_down;
+
+	int deny_down_time;
+
+
+	deque<int> left_acc_deque;
+	deque<int> right_acc_deque;
+
+	int wait_down = 0;
+
+	//deque<same_position_press> front_mag_deq;
+	//deque<same_position_press> back_mag_deq;
+
+};

footRing_sdk_1.0/include/BuildHouse.h

@@ -0,0 +1,50 @@
+
+#include "PublicSDKMotion.h"
+
+#include <deque>
+
+using namespace std;
+
+
+struct dualAccData {
+	int left_acc;
+	int right_acc;
+};
+//³¢ÊÔ½â¾öÉî¶×ÎÊÌâ
+class BuildHouse: public PublicSDKMotion {
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+	int back_press_up_trend(deque<int>& left_press_deque, deque<int>& right_press_deque);
+
+
+	deque<dualAccData> dual_acc;
+
+	deque<int> left_press_filter_window;
+	deque<int> right_press_filter_window;
+
+	deque<int> left_front_press_queue;
+	deque<int> left_back_press_queue;
+
+	deque<int> right_front_press_queue;
+	deque<int> right_back_press_queue;
+
+	int last_down = 0;
+	int last_front_down = 0;
+
+	int cmd_wait_time = 0;
+
+	int left_front_press_top = 0;
+	int right_front_press_top = 0;
+
+	int wait_press_top_unvalid = 0;
+
+	int left_back_press_top = 0;
+	int right_back_press_top = 0;
+	
+	int wait_back_press_top_unvalid = 0;
+
+	
+};

footRing_sdk_1.0/include/CutCake.h

@@ -0,0 +1,23 @@
+
+#include "PublicSDKMotion.h"
+
+
+class CutCake : public PublicSDKMotion
+{
+public:
+	CutCake();
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+
+	deque<int> left_acc_deque;
+	deque<int> right_acc_deque;
+	int jump_valid_time;
+	int jump_calm_down;
+
+	int last_jump;
+
+};

footRing_sdk_1.0/include/DanceGame.h

@@ -0,0 +1,99 @@
+
+#include "PublicSDKMotion.h"
+
+#define PRESS_WINDOW_SIZE 10
+
+#define RSSI_LEN 10
+
+
+struct origin_pos_struct {
+	int step_count;
+
+	float original_pos_x;
+
+	float original_pos_y;
+};
+
+enum DANCE_GAME_POINT {
+	ZERO_POINT = 0,
+	RIGHT_UP_POINT = 1,
+	LEFT_UP_POINT = 2,
+	RIGHT_DOWN_POINT = 3,
+	LEFT_DOWN_POINT = 4,
+
+};
+
+//缓存一定时间的rssi，中间缓存一步间的轨迹数据，最后再继续缓存rssi数据
+struct shoes_data_rssi_struct
+{
+	int rssi_count;
+
+	deque<shoes_data_cell> step_data_deque;
+};
+
+
+
+
+class DanceGame : public PublicSDKMotion
+{
+public:
+	DanceGame();
+
+	void set_shoes_data_with_rssi(deque<shoes_data_cell>& step_data_vector, deque<shoes_data_cell>& shoes_data_rssi_vector);
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+
+	deque<shoes_data_cell> left_shoes_data_vector;
+	deque<shoes_data_cell> right_shoes_data_vector;
+
+	deque<globalPos_struct> right_global_pos_vector;
+	deque<globalPos_struct> left_global_pos_vector;
+
+	deque<shoes_data_rssi_struct> left_shoes_data_rssi_vector;
+	deque<shoes_data_rssi_struct> right_shoes_data_rssi_vector;
+
+
+	float left_step_heading;
+	float right_step_heading;
+
+
+	deque<shoes_data_cell> left_step_data_vector;
+	deque<shoes_data_cell> right_step_data_vector;
+
+	float left_step_pos[3];
+	float right_step_pos[3];
+
+	int last_left_zupt;
+	int last_right_zupt;
+
+	float left_heading;
+	float right_heading;
+
+	int left_foot_has_init;
+	int right_foot_has_init;
+
+	int left_zupt_count;
+	int right_zupt_count;
+
+	int right_cmd_status;
+	int left_cmd_status;
+
+	int last_right_cmd_status;
+	int last_left_cmd_status;
+
+	int right_cmd_cur_status;
+	int left_cmd_cur_status;
+
+	const int rssi_threshold = 32;
+
+	int right_cmd_wait_time;
+	int left_cmd_wait_time;
+
+	int zero_point_wait_time;
+
+
+};

footRing_sdk_1.0/include/FingerGuessing.h

@@ -0,0 +1,70 @@
+
+#include "PublicSDKMotion.h"
+
+#define RSSI_LEN 5
+
+struct motion_rssi {
+
+	int rssi;
+	int motion;
+};
+
+
+class FingerGuessing : public PublicSDKMotion
+{
+public:
+	FingerGuessing();
+
+	void ProcessResult(float* global_pos, deque<shoes_data_cell>& step_data_vector,
+		deque<globalPos_struct>& global_pos_vector, int LEFT_OR_RIGHT);
+
+	int ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_data_vector, deque<shoes_data_cell> &right_cmd_step_data_vector,
+		float* left_cmd_global_pos, float* right_cmd_global_pos , int rssi);
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+
+	deque<shoes_data_cell> left_shoes_data_vector;
+	deque<shoes_data_cell> right_shoes_data_vector;
+
+	deque<globalPos_struct> right_global_pos_vector;
+	deque<globalPos_struct> left_global_pos_vector;
+
+
+	float left_cmd_global_pos[3];
+	float right_cmd_global_pos[3];
+
+	deque<shoes_data_cell> left_step_data_vector;
+	deque<shoes_data_cell> right_step_data_vector;
+	
+	deque<shoes_data_cell> left_cmd_step_data_vector;
+	deque<shoes_data_cell> right_cmd_step_data_vector;
+
+	float left_step_pos[3];
+	float right_step_pos[3];
+
+
+	float left_heading;
+	float right_heading;
+
+	int left_foot_has_init;
+	int right_foot_has_init;
+
+	int left_zupt_count;
+	int right_zupt_count;
+
+
+	int right_cmd_wait_time;
+	int left_cmd_wait_time;
+
+	int left_init;
+	int right_init;
+
+
+	const int rssi_threshold = 25;
+
+
+};

footRing_sdk_1.0/include/FootStep.h

@@ -0,0 +1,87 @@
+
+#ifndef _FootStep_H
+#define _FootStep_H
+
+#include <iostream>
+#include <cstring>
+#include <vector>
+#include <deque>
+
+using namespace std;
+
+#define LEAVE_TIME_STYPE 1
+#define CATCH_TIME_STYPE 2
+
+struct  FLOOR_TIME_STYPE {
+	int  time_type;
+	int  time;
+};
+
+
+class FootStep
+{
+public:
+
+	FootStep();
+
+	//void stepCal(int timeStamp, int pos[3], int zupt);
+
+	void stepCal(int timeStamp, int zupt, int front_mag, int back_mag, int *acc);
+
+	int getStepFreq();
+	int getStepStatus();
+
+	int getStepCount();
+
+private:
+
+	int stepStatus;//脚步状态 0 1 2
+	int stepFreq;	//脚步步频 分钟为单位
+	int stepTag;	//计算步数所用
+	int zuptCount;
+	
+	int localTime;
+	int last_time_stamp;
+
+	int leaveFloorTime;
+
+	int leaveFloorTime1;
+	int leaveFloorTime2;
+
+	int leaveTimeStatus;
+
+	int catchFloorTime;
+
+	int lastZupt;
+
+	int stepCount;
+
+	int last_pos[3];
+
+	vector<FLOOR_TIME_STYPE> time_vector;
+
+	float mean_vel;
+
+	float mean_window_vel;
+
+	float step_vel_buff[3];
+
+	float vel_sqrt_sum;
+	int vel_sqrt_num;
+
+	float vel_sqrt_buff[3];
+
+	int front_mag_min;
+	int back_mag_min;
+
+	int max_acc[3];
+	int min_acc[3];
+
+	int unzupt_count;
+
+	int leave_time_wait = 0;
+
+	deque<int> acc_z_deque;
+};
+
+#endif

footRing_sdk_1.0/include/Game.h

@@ -0,0 +1,152 @@
+
+#ifndef _Game_H_
+#define _Game_H_
+
+#include "pub.h"
+
+#include "PublicSDKMotion.h"
+#include "RunGame.h"
+#include "DanceGame.h"
+#include "Tricycle.h"
+#include "H5_Game.h"
+#include "Interaction.h"
+#include "FootStep.h"
+#include "FingerGuessing.h"
+#include "Aerobics.h"
+#include "Kongfu.h"
+#include "SportFunction.h"
+#include "BuildHouse.h"
+#include "CutCake.h"
+#include "JumpHouse.h"
+
+#include "StepVel.h"
+#include "TestGame.h"
+
+
+#include <fstream>
+#include <time.h>
+#include <sstream>
+
+#if defined(_WIN32)
+#if defined( SDK_EXPORTS)
+#define SDK_EXPORT __declspec(dllexport)
+#else
+#define SDK_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SDK_EXPORT
+#endif
+
+
+class SDK_EXPORT Game
+{
+		public:
+			Game(int gametype);
+
+			void GameProcess(int gameTime, int* right_pos_data, int* right_att_data, int* right_acc_data, int right_zupt, int right_front_mag, int right_back_mag,
+				int* left_pos_data, int* left_att_data, int* left_acc_data, int left_zupt, int left_front_mag, int left_back_mag,
+				int jump, int down, int rssi);
+
+			void GameProcessBuf(uint8_t* buf, int len);
+
+			void getGameResult(int* matrix);
+
+			void getFootAtt(int* left_att, int* right_att);
+
+			string getGameDataStr();
+
+			int getStepStatus(int left_or_right);
+			int getStepFreq(int left_or_right);
+			int getStepCount(int left_or_right);
+
+			float getGamePos(int left_or_right, int index);
+
+			string getVersion();
+
+			int getInteractionCMD();
+
+			int getGameStepVel();
+
+			void setGameDataStr();
+
+			int getMotionCount(int motion_type);
+
+			bool setShoesDataBuff(int &gameTime, int* right_pos_data, int* right_att_data, int* right_acc_data, int &right_zupt, int& right_front_mag, int& right_back_mag,
+				int* left_pos_data, int* left_att_data, int* left_acc_data, int& left_zupt, int& left_front_mag, int& left_back_mag,
+				int& jump, int& down, int& rssi);
+
+		private:
+
+			int GameType;
+
+			int leftStepRate;
+			int rightStepRate;
+			
+			int leftRate;
+			int rightRate;
+
+			int resultMatrix[RESULT_LEN];
+
+			float leftRotate[4];
+			
+			float rightRotate[4];
+
+			fstream foot_data_file;
+
+
+			int InteractionCMD;
+
+			int right_pos_data[3];
+			int right_att_data[3];
+
+			int left_pos_data[3];
+			int left_att_data[3];
+
+			int left_acc_data[3];
+			int right_acc_data[3];
+
+			int left_gyr_data[3];
+			int right_gyr_data[3];
+
+			int girl_shoes;
+
+			int s_zupt;
+			int h_zupt;
+
+			int down;
+			int jump;
+
+			int rssi;
+
+			int posTimeStamp;
+			int attTimeStamp;
+
+			uint32_t gameTime = 0;
+			int lastTimeStamp = -1;
+
+			uint16_t right_front_mag;
+			uint16_t right_back_mag;
+
+			uint16_t left_front_mag;
+			uint16_t left_back_mag;
+
+
+			string data_str;
+
+			int shoes_down;
+
+			PublicSDKMotion *game;
+
+			FootStep leftFootStep;
+			FootStep rightFootStep;
+			StepVel  stepVel;
+
+		    Interaction interaction;
+
+			int shoes_data_buff_size;
+
+			int setShoesDataBuff_count;
+
+};
+
+#endif

footRing_sdk_1.0/include/H5_Game.h

@@ -0,0 +1,32 @@
+
+#include "PublicSDKMotion.h"
+#include "pub.h"
+
+class H5 : public PublicSDKMotion
+{
+public:
+	H5();
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+
+private:
+
+	int last_left_zupt;
+	int last_right_zupt;
+
+	int last_jump;
+	int last_down;
+
+	int CANCLE_DOWN;
+	int CANCLE_JUMP;
+
+	int valid_down_wait_time;
+	int valid_jump_wait_time;
+
+	deque<int> left_front_press_deque;
+	deque<int> right_front_press_deque;
+
+};

footRing_sdk_1.0/include/Interaction.h

@@ -0,0 +1,50 @@
+
+
+class Interaction {
+
+public:
+
+	Interaction();
+
+	void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+	
+	int foot_on_floor(int _time, int& triggerring_time, int zupt, int& last_zupt, int rssi, float pitch, int left_or_right);
+
+	int enter_key_listen(float* acc, float* acc_x_buff, int& station_count, float* pitch_buff, float pitch, int& angle_has_reset);
+
+	int get_interative_cmd();
+
+	void setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt);
+
+	bool unzuptValid(int* max_acc, int* min_acc);
+
+private :
+	int left_Triggering_time ;
+	int right_Triggering_time ;
+
+	int last_left_zupt;
+	int last_right_zupt;
+
+	float left_acc_x_buff[10];
+	float right_acc_x_buff[10];
+
+	float left_pitch_buff[10];
+	float right_pitch_buff[10];
+
+	int left_station_count ;
+	int right_station_count ;
+
+	int left_angle_has_reset ;
+	int right_angle_has_reset ;
+
+	int max_acc_unzupt_left[3];
+	int min_acc_unzupt_left[3];
+
+	int max_acc_unzupt_right[3];
+	int min_acc_unzupt_right[3];
+
+	int cmd;
+
+};

footRing_sdk_1.0/include/JumpHouse.h

@@ -0,0 +1,42 @@
+#include "PublicSDKMotion.h"
+
+class JumpHouse : public PublicSDKMotion
+{
+	public:
+		JumpHouse();
+
+		virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+			int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+			int jump, int down, int rssi);
+
+		bool unzuptValid(int* max_acc, int* min_acc);
+	private:
+
+		deque<shoes_data_cell> left_shoes_data_vector;
+		deque<shoes_data_cell> right_shoes_data_vector;
+
+		deque<shoes_data_cell> left_step_data_vector;
+		deque<shoes_data_cell> right_step_data_vector;
+
+		int left_acc_buff[3][3];
+		int right_acc_buff[3][3];
+
+		deque<int> left_acc_x_vector;
+		deque<int> right_acc_x_vector;
+
+
+
+		float left_heading;
+		float right_heading;
+
+		int right_cmd_wait_time;
+		int left_cmd_wait_time;
+
+		int left_init;
+		int right_init;
+
+
+
+		const int rssi_threshold = 25;
+
+};

footRing_sdk_1.0/include/Kongfu.h

@@ -0,0 +1,56 @@
+
+#include "PublicSDKMotion.h"
+#include "pub.h"
+
+class Kongfu : public PublicSDKMotion
+{
+public:
+	Kongfu();
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+
+	deque<shoes_data_cell> left_shoes_data_vector;
+	deque<shoes_data_cell> right_shoes_data_vector;
+
+	vector<float> left_acc_x_buff;
+	vector<float> right_acc_x_buff;
+
+	int left_kick;
+	int last_left_kick;
+	int left_kick_wait;
+
+	int right_kick;
+	int last_right_kick;
+	int right_kick_wait;
+
+	int CANCLE_FLOOR_CMD_LEFT;
+	int CANCLE_FLOOR_CMD_RIGHT;
+
+	int last_left_zupt;
+	int last_right_zupt;
+
+	int last_jump;
+	int last_down;
+
+	int CANCLE_DOWN;
+	int CANCLE_JUMP;
+
+	int valid_down_wait_time;
+	int valid_jump_wait_time;
+
+	int cmd_jump_wait_time;
+	
+	const int CMD_JUMP_WAIT_TIME = 5;
+
+	const int  WAIT_TIME = 10;
+
+	 deque<int> left_acc_deque;
+	 deque<int> right_acc_deque;
+
+	 int wait_down = 0;
+
+};

footRing_sdk_1.0/include/MotionCount.h

@@ -0,0 +1,18 @@
+#include "pub.h"
+
+#define MOTION_COUNT_SIZE  20
+
+class MotionCount {
+public:
+	MotionCount();
+
+	void addMotionCount(int motion_type);
+
+	int  getMotionCount(int motion_type);
+
+private:
+
+
+	int	motionCount_matrix[MOTION_COUNT_SIZE];
+
+};

footRing_sdk_1.0/include/PublicInterface.h

@@ -0,0 +1,95 @@
+
+#include <iostream>
+#include<deque>
+
+#include "pub.h"
+
+#include "online_motion.h"
+
+#define RESULT_LEN 10
+
+using namespace std;
+
+struct shoes_data_cell {
+	//鞋子传上来的时间戳
+	int time_stamp;
+
+	//鞋子当前的位置
+	float pos_x;
+	float pos_y;
+	float pos_z;
+
+	//鞋子的姿态
+	float heading;
+	float pitch;
+	float roll;
+
+	int zupt;
+
+	int rssi;
+
+};
+
+/*
+* 公用接口类， 代码复用抽取
+*/
+class PublicInterface
+{
+public :
+	int result[RESULT_LEN]; //返回到手机命令数据
+
+	float left_global_pos[3];//描绘游戏过程中全局位置
+	float right_global_pos[3];
+
+	int max_acc_unzupt_left[3];//记录空中时候加速度的最大值最小值
+	int min_acc_unzupt_left[3];
+
+	int max_acc_unzupt_right[3];
+	int min_acc_unzupt_right[3];
+
+	online_motion extract_motion; //额外的补充动作,例如蹲、跳在鞋子里面处理并不适应每个游戏
+
+	PublicInterface() {
+		memset(result, 0, RESULT_LEN * sizeof(int));
+
+		memset(left_global_pos, 0, 3 * sizeof(float));
+
+		memset(right_global_pos, 0, 3 * sizeof(float));
+	};
+
+	void getResult(int* dec); //返回到手机命令数据接口
+
+	void setData(deque<shoes_data_cell>& shoes_data_vector, int time_stamp, float pos_x, float pos_y, float pos_z,
+		float heading, float pitch, float roll, int zupt, int rssi);//缓存空中的数据到shoes_data_vector中， 头尾都触地时候的数据
+
+	/*
+	* 计算全局位置的普通函数， 利用鞋子传上来的空中数据shoes_data_vector，换算为旋转过后的空中数据
+	* main_heading， 将传上来的方向旋转至鞋子的朝向
+	* extra_heading， 将当前鞋子朝向的轨迹数据，额外再做旋转，一般旋转大概估计身体朝向的位置
+	*/
+	void calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector, float* global_pos, 
+		float main_heading, float extra_heading, int LEFT_OR_RIGHT);
+
+	/*
+	* 获取全局位置的普通函数, 用于python脚本分析bug时候，查看游戏内的全局位置有没有符合期望。
+	*/
+	float getGamePos(int left_or_right, int index);
+
+
+	/*
+	* 寻找非触地时候的最大加速度和最小加速度
+	*/
+	void setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt);
+
+	/*
+	* 判断空中轨迹中加速度的最大值、最小债
+	*/
+	bool unzuptValid(int* max_acc, int* min_acc);
+
+	/*
+	* 根据小于rssi_thresh触地信号的个数累计到zupt_count，估计当前鞋子的方向以及将global_pos设置为0
+	*/
+	void resetZerosPointByRssi(int zupt, int zupt_count_thresh, int& zupt_count, int& has_init, float heading, float& rotate_heading, int rssi, int rssi_thresh,  float* global_pos);
+
+
+};

footRing_sdk_1.0/include/PublicSDKMotion.h

@@ -0,0 +1,32 @@
+
+
+#ifndef  PUBLIC_SDK_MOTION
+#define  PUBLIC_SDK_MOTION
+
+#define LEFT_FOOT 1
+#define RIGHT_FOOT 2
+
+#include<iostream>
+#include<vector>
+#include<deque>
+
+#include "pub.h"
+#include "MotionCount.h"
+#include "PublicInterface.h"
+
+using namespace std;
+
+
+class PublicSDKMotion : public MotionCount , public PublicInterface {
+public:
+
+	/*
+	* process函数，用于游戏的多态调用
+	*/
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press,int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+};
+
+#endif 

footRing_sdk_1.0/include/RunGame.h

@@ -0,0 +1,56 @@
+
+
+#include "PublicSDKMotion.h"
+
+class RunGame : public PublicSDKMotion {
+public:
+	RunGame();
+
+	void setStepHeading(float& cur_heading, float heading);
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+	int rssiFeature(deque<shoes_data_cell>& shoes_data_vector);
+
+	int getResultJump(int jump);
+	int getResultDown(int down);
+
+private:
+	deque<shoes_data_cell> left_shoes_data_vector;
+	deque<shoes_data_cell> right_shoes_data_vector;
+
+	float left_step_heading;
+	float right_step_heading;
+
+	deque<shoes_data_cell> left_step_data_vector;
+	deque<shoes_data_cell> right_step_data_vector;
+
+	float left_step_pos[3];
+	float right_step_pos[3];
+
+	int jump_count;
+	int down_count;
+
+	int last_down;
+	int last_canva_down;
+	int last_jump;
+
+	int down_wait_after_jump;
+
+	float left_heading;
+	float right_heading;
+
+	int left_zupt_count;
+	int right_zupt_count;
+
+	int last_left_zupt;
+	int last_right_zupt;
+
+	//仿造踢馆大师，特殊处理蹲看看效果
+	deque<int> left_acc_deque;
+	deque<int> right_acc_deque;
+	int wait_down = 0;
+
+};

footRing_sdk_1.0/include/SportFunction.h

@@ -0,0 +1,121 @@
+
+#include "PublicSDKMotion.h"
+
+
+struct sport_data_struct
+{
+	float pos_x;
+	float pos_y;
+	float pos_z;
+
+	float acc_x;
+	float acc_y;
+	float acc_z;
+
+	int front_press;
+	int back_press;
+
+	int jump;
+	int zupt;
+
+	float max_z;
+	float min_z;
+
+	float max_pos_z;
+
+	int not_on_floor;
+
+	int time_stamp;
+};
+
+struct single_jump_data_struct
+{
+	int jump_tag;
+	int jump_high;
+	int jump_length;
+
+	int start_time_stamp;
+};
+
+struct dual_jump_data_struct
+{
+	int live_time;
+
+	single_jump_data_struct  left_jump_data;
+	single_jump_data_struct  right_jump_data;
+};
+
+struct double_press_struct
+{
+	int front_press;
+	int back_press;
+
+	int up_flag;
+	int get_flag;
+};
+
+struct jump_data_struct
+{
+	float max_pos_z;
+	float max_pos_xy;
+};
+
+struct long_jump_data_struct
+{
+	int step_length;
+	int start_time;
+	int end_time;
+};
+
+
+
+class SportFunction : public PublicSDKMotion
+{
+public:
+	SportFunction();
+
+	void setData(deque<sport_data_struct>& shoes_data_vector, float pos_x, float pos_y, float pos_z,
+		float acc_x, float acc_y, float acc_z, int front_press, int back_press,
+		int jump, int zupt, int times_stamp);
+
+	void checkShoesData(deque<sport_data_struct>& shoes_data_vector);
+
+	int checkRopeSkip(deque<sport_data_struct>& left_shoes_data_vector, deque<sport_data_struct>& right_shoes_data_vector);
+
+	void getJumpHighLength(dual_jump_data_struct& dual_jump_data, deque<sport_data_struct>& shoes_data_vector, int left_or_right);
+
+	void processJumpHighLength(dual_jump_data_struct& dual_jump_data, int rssi);
+
+	void processStepData(deque<sport_data_struct>& shoes_data_vector, int left_or_right);
+
+	float getJumpLength(deque<sport_data_struct>& shoes_data_vector);
+
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+	deque<sport_data_struct> left_shoes_data_vector;
+	deque<sport_data_struct> right_shoes_data_vector;
+
+	dual_jump_data_struct dual_jump_data;
+
+	single_jump_data_struct left_jump_data;
+	single_jump_data_struct right_jump_data;
+
+	int left_step;
+	int right_step;
+
+	int left_step_length;
+	int right_step_length;
+
+	int step_length;
+
+	int jump_count;
+	int last_jump_count;
+
+	int jump_high;
+	int jump_length;
+
+};

footRing_sdk_1.0/include/StepVel.h

@@ -0,0 +1,55 @@
+
+
+#include "PublicSDKMotion.h"
+
+#define PRESS_WINDOW_SIZE 10
+
+struct vel_struct {
+	int time_stamp;
+
+	float vel;
+};
+
+class StepVel : public PublicSDKMotion
+{
+public:
+	StepVel();
+
+	void setData(deque<shoes_data_cell>& shoes_data_vector, int time_stamp, float pos_x, float pos_y, float pos_z, float heading, float pitch, float roll, int zupt, int rssi);
+
+	void calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector,
+		float* global_pos, int LEFT_OR_RIGHT);
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+	int getStepVel();
+
+	void  check_vel_vector(int time_stamp, deque<vel_struct>& vel_vector);
+	float cal_mean_vel(int end_time, int start_time, float max_val);
+	float get_vel_mean(int time_stamp, deque<vel_struct>& vel_vector, deque<shoes_data_cell>& step_data_vector);
+
+
+private:
+	deque<shoes_data_cell> left_shoes_data_vector;
+	deque<shoes_data_cell> right_shoes_data_vector;
+
+	deque<shoes_data_cell> left_step_data_vector;
+	deque<shoes_data_cell> right_step_data_vector;
+
+	deque<vel_struct> left_vel_vector;
+	deque<vel_struct> right_vel_vector;
+
+	float right_global_pos[3];
+	float left_global_pos[3];
+
+
+	int left_jump_cancel_update_vel;
+	int right_jump_cancel_update_vel;
+
+	float step_vel;
+
+	int both_zupt_count = 0;
+
+};

footRing_sdk_1.0/include/TestGame.h

@@ -0,0 +1,24 @@
+
+#include "PublicSDKMotion.h"
+
+
+class TestGame : public PublicSDKMotion
+{
+public:
+	TestGame();
+
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+private:
+
+	 deque<int> left_acc_deque;
+	 deque<int> right_acc_deque;
+	 int jump_valid_time;
+	 int jump_calm_down;
+
+	 int last_jump;
+
+};

footRing_sdk_1.0/include/Tricycle.h

@@ -0,0 +1,52 @@
+
+#include "PublicSDKMotion.h"
+#include "pub.h"
+
+class Tricycle : public PublicSDKMotion
+{
+public:
+	Tricycle();
+
+	virtual void Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+		int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+		int jump, int down, int rssi);
+
+	float calHeadingOffset(float init_heading, float cur_heading);
+
+private:
+
+	int left_result;
+	int right_result;
+
+	int left_zupt_count;
+	int right_zupt_count;
+
+
+	float left_foot_init_heading;
+	float right_foot_init_heading;
+
+	int left_trigger_time;
+	int right_trigger_time;
+
+	int last_left_zupt;
+	int last_right_zupt;
+
+	int last_left_pos[3];
+	int last_right_pos[3];
+
+	int left_jump_status;
+	int right_jump_status;
+
+	int has_init;
+
+	int left_pos_offset;
+	int right_pos_offset;
+
+	int last_jump;
+
+	int prop_status;
+
+	deque<int> left_acc_z_queue;
+	deque<int> right_acc_z_queue;
+
+};

footRing_sdk_1.0/include/online_motion.h

@@ -0,0 +1,62 @@
+
+#include <deque>
+using namespace std;
+
+//由于在鞋子里，判断蹲，并不完美，需要在SDK进行补充
+//后期考虑将蹲放置SDK上判断
+class online_motion
+{
+public:
+	int online_detect_down(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag,
+		int* right_acc, int* left_acc);
+
+	int online_detect_jump(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag,
+		int* right_acc, int* left_acc);
+
+	int online_detect_touch_floor(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag );
+
+	void push_press_queue(deque<int>& press_queue ,int press, int queue_size);
+
+	int check_press_queue_up(deque<int>& press_queue, int thresh_hold);
+
+	int  min_window_val(deque<int>& press_deque, int mid_window_size);
+
+	int back_press_up_trend(deque<int>& left_press_deque, deque<int>& right_press_deque);
+
+private:
+	deque<int> right_back_mag_queue;
+	deque<int> left_back_mag_queue;
+
+	deque<int> left_press_filter_window;
+	deque<int> right_press_filter_window;
+
+	deque<int> right_front_mag_queue;
+	deque<int> left_front_mag_queue;
+
+	deque<int> right_front_mag_canve_queue;
+	deque<int> left_front_mag_canve_queue;
+
+	deque<int> left_front_acc;
+	deque<int> right_front_acc;
+
+	int last_back_down = 0;
+	int last_front_down = 0;
+	int last_little_front_down = 0;
+
+	int left_top_back_press = 0;
+	int right_top_back_press = 0;
+
+	int top_back_press_valid_time = 0;
+
+	deque<int> left_front_press_queue_touch_floor;
+	deque<int> left_back_press_queue_touch_floor;
+
+	deque<int> right_front_press_queue_touch_floor;
+	deque<int> right_back_press_queue_touch_floor;
+
+	deque<int> right_front_jump_mag_queue;
+	deque<int> left_front_jump_mag_queue;
+
+	deque<int> right_acc_z_queue;
+	deque<int> left_acc_z_queue;
+};

footRing_sdk_1.0/include/pub.h

@@ -0,0 +1,145 @@
+#pragma once
+#ifndef _pub_h__
+#define _pub_h__
+
+#include <iostream>
+#include <math.h>
+#include <stdlib.h>
+#include <vector>
+#include <string> 
+#include <algorithm>
+
+using namespace std;
+
+#define LEFT_FOOT 1
+#define RIGHT_FOOT 2
+
+//#define DANCING_THRESH 5
+#define DANCING_THRESH 0
+#define CIRCLE_CENTER 22
+
+#define ORIGINTRAJ 0
+#define RUNGAME 1
+#define DANCEGAME 2
+#define H5GAME 3
+#define MONSTER 4
+#define RUINS 5
+#define CYCLE 6
+#define BIGFOOT 7
+#define FINGERGUESSING 8
+#define AEROBICS 9
+#define KONGFU 10
+#define SPORT 11
+#define BUILDHOUSE 12
+#define CUTCAKE 13
+#define JUMPHOUSE 14
+
+#define TESTGAME 100
+
+#define RSSI_LIMIT
+
+#define PI 3.141592653
+
+#define DANCEGAME_MIN_RSSI 26
+
+#define PRESS_MIN 41000
+
+#define GAME_VERSION "0.5.63"
+
+enum CMD_MOTION
+{
+    MOTION_STOP = 0,
+    MOTION_RUN, // 跑
+    MOTION_JUMP, // 跳
+    MOTION_DOWN, // 蹲
+    MOTION_LEFT, // 滑左
+    MOTION_RIGHT, // 滑右
+    MOTION_FRONT, // 滑前
+    MOTION_BACK, // 滑后
+    MOTION_LEFT_UP, // 点击-左上
+    MOTION_LEFT_DOWN, // 点击-左下
+    MOTION_RIGHT_UP, // 点击-右上
+    MOTION_RIGHT_DOWN, // 点击-右下
+    MOTION_STEP, // 点击-原地踩
+    MOTION_JUMP_OC, // 开合跳
+    MOTION_JUMP_VERTICAL, // 垂直跳
+    MOTION_ROCK, //石头
+    MOTION_SCISSORS, //剪刀
+    MOTION_PAPER, //布
+
+    MOTION_STRETCH_LEFT, //左伸腿
+    MOTION_LIFT_LEFT, //左抬腿
+
+    MOTION_STRETCH_RIGHT, //右伸腿
+    MOTION_LIFT_RIGHT, //右抬腿
+
+    MOTION_STRETCH_FRONT_LEFT,
+    MOTION_STRETCH_FRONT_RIGHT,
+
+    MOTION_DOWN_FRONT, //踮脚蹲
+
+    MOTION_LEFT_ON_FLOOR,//左踩地
+    MOTION_RIGHT_ON_FLOOR, //右踩地
+
+    MOTION_BOTH_ON_FLOOR_CLOSE, //合脚踩地
+    MOTION_BOTH_ON_FLOOR_OPEN, //岔开脚踩地
+
+    NUMBERS_OF_MOTION,
+
+    MOTION_CANCEL = 0x0100,
+};
+
+enum MOTION_COUNT_TYPE {
+    STEP_COUNT = 0,
+
+    JUMP_COUNT,
+    
+    DOWN_COUNT,
+
+    KICK_COUNT,
+
+    PAPER_COUNT,
+
+    SCISSORS_COUNT,
+
+    ROCK_COUNT,
+
+};
+
+enum CMD_INTERATION
+{
+   NONE_INTERATION = 0,
+   FRONT_LEFT,//左上键
+   FRONT_RIGHT,//右上键
+   BACK_LEFT,//左下键
+   BACK_RIGHT,//右下键
+   ENTER_KEY,//确认键
+   CANCLE_KEY,//取消键
+};
+
+struct posData
+{
+	float pos_x;
+	float pos_y;
+	float rssi;
+};
+
+struct FootActionState {
+
+    int FootState;
+
+    int StateCount;
+
+    uint32_t Triggering_time;
+};
+
+struct globalPos_struct {
+    int time_stamp;
+
+    float pos_x;
+    float pos_y;
+    float pos_z;
+};
+
+
+#endif

footRing_sdk_1.0/src/Aerobics.cpp

@@ -0,0 +1,246 @@
+#include "Aerobics.h"
+//踢馆大师
+
+Aerobics::Aerobics()
+{
+	last_down = 0;
+	last_jump = 0;
+
+	stretch_left_status = 0;
+	stretch_right_status = 0;
+
+	lift_left_status = 0;
+	lift_right_status = 0;
+
+	left_wait_time = 0;
+	right_wait_time = 0;
+
+	special_down = 0;
+	last_special_down = 0;
+
+	stretch_front_left_status = 0;
+	stretch_front_right_status = 0;
+
+	deny_down_time = 0;
+}
+
+void Aerobics::ResetDenyDownTime(int time_stamp)
+{
+	deny_down_time = time_stamp;
+}
+
+bool Aerobics::isValidDown(int time_stamp)
+{
+	if (time_stamp - deny_down_time < 10 && time_stamp - deny_down_time >= 0)
+	{
+		return false;
+	}
+
+	return true;
+}
+
+void Aerobics::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+	memset(result, -1, 4 * sizeof(int));
+
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+
+	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_shoes_data_vector[0].heading, 10.0f / 180 * PI, RIGHT_FOOT);
+
+	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_shoes_data_vector[0].heading, -10.0f / 180 * PI, LEFT_FOOT);
+
+
+	if (left_shoes_data_vector.size() == 1 && left_shoes_data_vector.back().zupt)
+	{
+		memset(left_global_pos, 0, 3 * sizeof(float));
+	}
+
+	if (right_shoes_data_vector.size() == 1 && right_shoes_data_vector.back().zupt)
+	{
+		memset(right_global_pos, 0, 3 * sizeof(float));
+	}
+
+
+	//设置单脚的空中命令状态，使得命令在空中只判断一次，区别于触地才能触发的命令。
+
+	//如果触发了抬脚命令，那么触地的时候，将不会触发下端，用一个禁止下蹲倒计来计时
+	/*if ((left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1 && lift_left_status == 0)
+		||(right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1 && lift_right_status == 0))
+	{
+		ResetDenyDownTime(time_stamp);
+	}*/
+
+	if (left_zupt)
+	{
+		stretch_left_status = 1;
+
+		stretch_front_left_status = 1;
+
+		lift_left_status = 1;
+	}
+
+	if (right_zupt)
+	{
+		stretch_right_status = 1;
+
+		stretch_front_right_status = 1;
+
+		lift_right_status = 1;
+	}
+
+	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 0)
+	{
+		setResult(left_step_data_vector, LEFT_FOOT);
+	}
+
+
+
+	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 0)
+	{
+		setResult(right_step_data_vector, RIGHT_FOOT);
+	}
+
+
+
+	//会有多个蹲命令触发
+	//在这个游戏单独处理蹲
+	left_acc_deque.push_back(sqrt(left_acc[0] * left_acc[0] + left_acc[1] * left_acc[1]));
+	right_acc_deque.push_back(sqrt(right_acc[0] * right_acc[0] + right_acc[1] * right_acc[1]));
+
+	if (left_acc_deque.size() > 10)
+	{
+		left_acc_deque.pop_front();
+		right_acc_deque.pop_front();
+	}
+
+	int acc_valid = 0;
+	if (left_acc_deque.size() == 10)
+	{
+		int left_max_val = *(std::max_element(left_acc_deque.begin(), left_acc_deque.end()));
+		int left_min_val = *(std::min_element(left_acc_deque.begin(), left_acc_deque.end()));
+
+		int right_max_val = *(std::max_element(right_acc_deque.begin(), right_acc_deque.end()));
+		int right_min_val = *(std::min_element(right_acc_deque.begin(), right_acc_deque.end()));
+
+
+		if (left_max_val - left_min_val < 512 && right_max_val - right_min_val < 512)
+		{
+			acc_valid = 1;
+		}
+	}
+	//std::cout << "time_stamp : " << time_stamp << endl;
+	//if (last_down == 0 && down == 1 && isValidDown(time_stamp))
+	//if (last_down == 0 && down != 0 && wait_down == 0 && deny_down_time == 0)
+	if (last_down == 0 && down != 0 )
+	{
+		addMotionCount(DOWN_COUNT);
+
+		std::cout << "normal down motion" << endl;
+		result[0] = MOTION_DOWN;
+
+	}
+
+
+	last_down = down;
+
+	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1 &&
+		unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
+	{
+		addMotionCount(STEP_COUNT);
+	}
+
+	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1 &&
+		unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
+	{
+		addMotionCount(STEP_COUNT);
+	}
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+}
+
+void Aerobics::setResult(deque<shoes_data_cell>& step_data_vector,int LEFT_OR_RIGHT)
+{
+	// 描绘空中轨迹
+	//其实可以实时检测到轨迹是否符合触发命令的，但是为了方便代码简单，就在触地的瞬间来触发命令检测流程算了
+	//1、先检测到Z轴的轨迹向量是否大于10cm, 大于就会触发判断命令了
+	//2、 同时在同一个循环里，判断最大的平面（X,Y）的点在哪里
+
+	float high = step_data_vector.back().pos_z;
+
+	float plane_length = sqrt(step_data_vector.back().pos_x * step_data_vector.back().pos_x 
+		+ step_data_vector.back().pos_y * step_data_vector.back().pos_y);
+
+	float pitch = step_data_vector.back().pitch;
+
+	float roll  = step_data_vector.back().roll;
+
+	//暂时先将抬脚的高度阈值为0.1m, 伸脚的距离设置为0.2m
+	if (high > high_thresh && plane_length > 0.2)
+	{
+		//if (LEFT_OR_RIGHT == LEFT_FOOT && stretch_left_status == 1 && roll > -2.9f && roll < 0.0f)
+		if (LEFT_OR_RIGHT == LEFT_FOOT && stretch_left_status == 1 && roll > 2.0f)
+		{
+			std::cout << "MOTION_STRETCH_LEFT" << endl;
+			result[0] = MOTION_STRETCH_LEFT;
+			stretch_left_status = 0;
+			lift_left_status = 0;
+		}
+		else if(LEFT_OR_RIGHT == RIGHT_FOOT && stretch_right_status == 1 && roll > 2.0f)
+		{
+			std::cout << "MOTION_STRETCH_RIGHT" << endl;
+			result[0] = MOTION_STRETCH_RIGHT;
+			stretch_right_status = 0;
+			lift_right_status = 0;
+		}
+
+		if (LEFT_OR_RIGHT == LEFT_FOOT && stretch_front_left_status == 1
+			&& step_data_vector.back().pos_y > 0.2f)
+		{
+			std::cout << "MOTION_STRETCH_FRONT_LEFT" << endl;
+			result[0] = MOTION_STRETCH_FRONT_LEFT;
+			stretch_front_left_status = 0;
+			lift_left_status = 0;
+		}
+		else if (LEFT_OR_RIGHT == RIGHT_FOOT && stretch_front_right_status == 1 
+			&& step_data_vector.back().pos_y > 0.2f)
+		{
+			std::cout << "MOTION_STRETCH_FRONT_RIGHT" << endl;
+			result[0] = MOTION_STRETCH_FRONT_RIGHT;
+			stretch_front_right_status = 0;
+			lift_right_status = 0;
+		}
+
+	}
+	else if (high > high_thresh && plane_length < 0.2f)
+	{
+		if (LEFT_OR_RIGHT == LEFT_FOOT && lift_left_status == 1)
+		{
+			std::cout << "MOTION_LIFT_LEFT" << endl;
+			result[0] = MOTION_LIFT_LEFT;
+			lift_left_status = 0;
+		}
+		else if (LEFT_OR_RIGHT == RIGHT_FOOT && lift_right_status == 1)
+		{
+			std::cout << "MOTION_LIFT_RIGHT" << endl;
+			result[0] = MOTION_LIFT_RIGHT;
+			lift_right_status = 0;
+		}
+	}
+
+	/*if (plane_length > 0.2)
+	{
+		std::cout << "test : plane_length > 0.2" << endl;
+	}
+
+	if (high > high_thresh)
+	{
+		std::cout << "high > high_thresh" << endl;
+	}*/
+}

footRing_sdk_1.0/src/BuildHouse.cpp

@@ -0,0 +1,167 @@
+
+
+#include "BuildHouse.h"
+
+bool isValidAcc(deque<dualAccData>& dual_acc, int start_index,  int thresh_hold)
+{
+	int left_max_val = dual_acc[start_index].left_acc;
+	int left_min_val = dual_acc[start_index].left_acc;
+
+	int right_max_val = dual_acc[start_index].right_acc;
+	int right_min_val = dual_acc[start_index].right_acc;
+
+	for (int i = start_index; i < dual_acc.size(); i++)
+	{
+		if (dual_acc[i].left_acc > left_max_val)
+		{
+			left_max_val = dual_acc[i].left_acc;
+		}
+		if (dual_acc[i].right_acc > right_max_val)
+		{
+			right_max_val = dual_acc[i].right_acc;
+		}
+
+		if (dual_acc[i].left_acc < left_min_val)
+		{
+			left_min_val = dual_acc[i].left_acc;
+		}
+		if (dual_acc[i].right_acc < right_min_val)
+		{
+			right_min_val = dual_acc[i].right_acc;
+		}
+		
+	}
+
+	if (left_max_val - left_min_val < thresh_hold && right_max_val - right_min_val < thresh_hold)
+	{
+		return true;
+	}
+
+	return false;
+}
+
+
+
+
+void detect_cancave_curve_top(deque<int>& dual_press_back, int& max_index, int& right_size_index, int& left_size_index)
+{
+	//1¡¢Ñ°ÕÒ×îСֵ
+	int k = dual_press_back.size() - 1;
+
+	right_size_index = k;
+
+	while (k > 2)
+	{
+		if (dual_press_back[k] < dual_press_back[k - 1] + 50
+			|| dual_press_back[k] < dual_press_back[k - 2]
+			|| dual_press_back[k] < dual_press_back[k - 3])
+		{
+			k--;
+		}
+		else
+		{
+			break;
+		}
+
+
+	}
+
+	max_index = k;
+
+
+	while (k > 2)
+	{
+		if (dual_press_back[k] > dual_press_back[k - 1] - 50
+			|| dual_press_back[k] > dual_press_back[k - 2]
+			|| dual_press_back[k] > dual_press_back[k - 3])
+		{
+			k--;
+		}
+		else
+		{
+			break;
+		}
+	}
+
+
+	left_size_index = k;
+
+	int distance = abs(dual_press_back[k] - dual_press_back[right_size_index]);
+
+	while (k < max_index)
+	{
+		if (distance > abs(dual_press_back[k] - dual_press_back[right_size_index]))
+		{
+			distance = abs(dual_press_back[k] - dual_press_back[right_size_index]);
+
+			left_size_index = k;
+		}
+		k++;
+	}
+}
+
+
+
+int mid_window_val(deque<int>& press_deque, int mid_window_size)
+{
+	deque<int> press_deque_temp = press_deque;
+
+	if (press_deque_temp.size() >= mid_window_size)
+	{
+		std::sort(press_deque_temp.begin(), press_deque_temp.end());
+	}
+	else
+	{
+		return press_deque.back();
+	}
+
+	//return press_deque[press_deque_temp.size() / 2];
+
+	return press_deque_temp[0];
+}
+
+int BuildHouse::back_press_up_trend(deque<int>& left_press_deque, deque<int>& right_press_deque)
+{
+	if (left_press_deque.size() > 20)
+	{
+		int i = left_press_deque.size() - 1;
+
+		while (i > 10 && (left_press_deque[i] >= left_press_deque[i - 1] || left_press_deque[i] >= left_press_deque[i - 2]
+			|| left_press_deque[i] >= left_press_deque[i - 3]) &&
+			(right_press_deque[i] >= right_press_deque[i - 1] || right_press_deque[i] >= right_press_deque[i - 2]
+				|| right_press_deque[i] >= right_press_deque[i - 3])
+			&& left_press_deque[i] > 12000 && right_press_deque[i] > 12000)
+		{
+			i--;
+		}
+
+		if (left_press_deque.back() > left_press_deque[i] + 1000&& right_press_deque.back() > right_press_deque[i] + 1000)
+		{
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+void BuildHouse::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	if (last_down == 0 && down == 1)
+	//if((front_down == 1 || back_down == 1))
+	{
+		result[0] = MOTION_DOWN;
+
+		addMotionCount(DOWN_COUNT);
+
+		std::cout << "BuidHouse : Down" << endl;
+	}
+
+
+
+	last_down = down;
+
+
+}

footRing_sdk_1.0/src/CutCake.cpp

@@ -0,0 +1,31 @@
+#include "CutCake.h"
+
+CutCake::CutCake()
+{
+	jump_valid_time = 0;
+	jump_calm_down = 0;
+
+	last_jump = 0;
+}
+
+
+void CutCake::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	result[0] = -1;
+
+	if (last_jump == 0 && jump == 1)
+	{
+		result[0] = MOTION_JUMP;
+		addMotionCount(JUMP_COUNT);
+
+		std::cout << "CutCake: jump" << endl;
+	}
+
+	last_jump = jump;
+
+
+}
+

footRing_sdk_1.0/src/DanceGame.cpp

@@ -0,0 +1,305 @@
+#include "DanceGame.h"
+
+DanceGame::DanceGame()
+{
+	left_foot_has_init = 0;
+	right_foot_has_init = 0;
+
+	left_heading = 0;
+	right_heading = 0;
+
+	left_zupt_count = 0;
+	right_zupt_count = 0;
+
+	left_cmd_status = -1;
+	right_cmd_status = -1;
+
+	last_left_cmd_status = -1;
+	last_right_cmd_status = -1;
+
+	right_cmd_cur_status = -1;
+	left_cmd_cur_status = -1;
+
+
+	zero_point_wait_time = 0;
+
+	//原点
+	left_global_pos_vector.push_back({ 0, 0, 0, 0 });
+	right_global_pos_vector.push_back({ 0, 0, 0, 0 });
+
+}
+
+
+
+
+void ProcessResult(float* global_pos, deque<shoes_data_cell>& step_data_vector,
+	deque<globalPos_struct>& global_pos_vector, int& cmd_status, int LEFT_OR_RIGHT)
+{
+
+	if (step_data_vector.size() > 1 && step_data_vector.front().zupt == 1 && step_data_vector.back().zupt == 1)
+	{
+		if (LEFT_OR_RIGHT == LEFT_FOOT)
+		{
+			std::cout << "Left Foot : " << endl << endl;
+		}
+		else
+		{
+			std::cout << "Right Foot : " << endl << endl;
+		}
+
+		//寻找到距离最小的值
+		int vector_length = global_pos_vector.size();
+
+		float distance_point = 0.1f;
+
+		int index = -1;
+
+
+		for (int i = 0; i < vector_length; i++)
+		{
+			//回溯寻找曾经踩过的点
+			float dx = global_pos[0] - global_pos_vector.at(i).pos_x;
+			float dy = global_pos[1] - global_pos_vector.at(i).pos_y;
+			float distance_temp = sqrt(dx * dx + dy * dy);
+
+			if ((i == 0 && distance_temp < 0.05f) || (i > 0 && distance_temp < distance_point) )
+			{
+				index = i;
+
+				distance_point = distance_temp;
+			}
+
+		}
+
+
+		if (index == -1)
+		{
+			std::cout << "no find similar point" << endl;
+			//没有找到基准点的时候，检测到往返点的时候,再做判断
+			//当检测到接近原点的时候，应当设置为0
+
+			global_pos_vector.push_back({ 0, global_pos[0], global_pos[1],global_pos[2] });
+
+
+		}
+		else
+		{
+			std::cout << "has find similar point" << endl;
+
+			global_pos[0] = global_pos_vector.at(index).pos_x;
+			global_pos[1] = global_pos_vector.at(index).pos_y;
+			global_pos[2] = global_pos_vector.at(index).pos_z;
+		}
+
+		std::cout << "cur_pos[0] : " << global_pos[0] << " , " << "cur_pos[1] : " << global_pos[1]
+			<< " , " << "cur_pos[2] : " << global_pos[2] << " , " << endl;
+
+		//还是根据rssi特征，或者step_data_vector跨越的象限来重新估计位置好了
+
+		if (step_data_vector.back().rssi < 26)
+		{
+			memset(global_pos, 0, 3 * sizeof(float));
+		}
+		
+		else if (fabsf(step_data_vector.back().pos_y) > 0.3f && (fabsf(step_data_vector.back().pos_y) >  fabsf(step_data_vector.back().pos_x))
+			&& step_data_vector.back().rssi >= 35 && step_data_vector.front().rssi >= 25)
+		{
+			global_pos[1] = 0.5 * step_data_vector.back().pos_y;
+
+			if (LEFT_OR_RIGHT == LEFT_FOOT)
+				global_pos[0] = -0.2f;
+			else
+				global_pos[0] = 0.2f;
+		}
+		else if (fabsf(step_data_vector.back().pos_y) > 0.3f &&step_data_vector.back().rssi >= 35 && step_data_vector.front().rssi >= 25)
+		{
+			global_pos[1] = 0.5 * step_data_vector.back().pos_y;
+		}
+
+
+
+		if (sqrt(global_pos[0] * global_pos[0] + global_pos[1] * global_pos[1]) < 0.1f)
+		{
+			std::cout << "result : " << "MOTION_STEP" << endl;
+
+			global_pos_vector.clear();
+
+			global_pos_vector.push_back({ 0, 0, 0, 0 });
+
+			memset(global_pos, 0, 3 * sizeof(float));
+
+			cmd_status = MOTION_STEP;
+		}
+		else
+		{
+			float angle;
+			if (LEFT_OR_RIGHT == LEFT_FOOT)
+			{
+				angle = atan2(global_pos[1], global_pos[0] - 0.1f);
+			}
+			else
+			{
+				angle = atan2(global_pos[1], global_pos[0] + 0.1f);
+			}
+
+			if (angle > 0.0f && angle < PI * 0.5f)
+			{
+				cmd_status = MOTION_RIGHT_UP;
+
+				std::cout << "result : " << "RIGHT_UP_POINT" << endl;
+			}
+			else if (angle > PI * 0.5f - 0.000001f && angle < PI)
+			{
+				cmd_status = MOTION_LEFT_UP;
+
+				std::cout << "result : " << "LEFT_UP_POINT" << endl;
+			}
+			else if (angle < 0.000001f && angle > -PI * 0.5f)
+			{
+				cmd_status = MOTION_RIGHT_DOWN;
+
+				std::cout << "result : " << "RIGHT_DOWN_POINT" << endl;
+			}
+			else
+			{
+				cmd_status = MOTION_LEFT_DOWN;
+
+				std::cout << "result : " << "LEFT_DOWN_POINT" << endl;
+			}
+		}
+
+	}
+
+	//为了防止误触点，用五个触地点来测试
+
+}
+
+void DanceGame::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+	//缓存空中轨迹数据
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+	
+	//计算全局的位置
+	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 10.0f / 180 * PI, RIGHT_FOOT);
+
+	//calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, LEFT_FOOT);
+	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, -10.0f / 180 * PI, LEFT_FOOT);
+
+
+	//左右脚估计需要选择的方向
+	resetZerosPointByRssi(left_zupt, 10, left_zupt_count, left_foot_has_init, left_att[0] * 0.0001f, left_heading, rssi, 25, left_global_pos);
+	resetZerosPointByRssi(right_zupt, 10, right_zupt_count, right_foot_has_init, right_att[0] * 0.0001f, right_heading, rssi, 25, right_global_pos);
+
+	/*
+	* 统计步数,跳舞游戏只统计步数
+	*/
+	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1
+		&& unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
+	{
+		//5代表，往后第五个rssi,才是该点对应的rssi
+		left_shoes_data_rssi_vector.push_back({ 5, left_step_data_vector });
+
+		addMotionCount(STEP_COUNT);
+	}
+	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1
+		&& unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
+	{
+		right_shoes_data_rssi_vector.push_back({ 5, right_step_data_vector });
+
+		addMotionCount(STEP_COUNT);
+	}
+
+	//计算命令
+	left_cmd_status = -1;
+	right_cmd_status = -1;
+
+	if (left_shoes_data_rssi_vector.size()!=0)
+	{
+		for (auto it = left_shoes_data_rssi_vector.begin(); it != left_shoes_data_rssi_vector.end(); it++)
+		{
+			if (it->rssi_count > 0)
+			{
+				it->step_data_deque.back().rssi = rssi;
+				it->rssi_count--;
+			}
+		}
+
+		if (left_shoes_data_rssi_vector.front().rssi_count == 0)
+		{
+			ProcessResult(left_global_pos, left_shoes_data_rssi_vector.front().step_data_deque, left_global_pos_vector, left_cmd_status, LEFT_FOOT);
+
+			left_shoes_data_rssi_vector.pop_front();
+		}
+
+	}
+
+	if (right_shoes_data_rssi_vector.size() !=0)
+	{
+		for (auto it = right_shoes_data_rssi_vector.begin(); it != right_shoes_data_rssi_vector.end(); it++)
+		{
+			if (it->rssi_count > 0)
+			{
+				it->step_data_deque.back().rssi = rssi;
+				it->rssi_count--;
+			}
+		}
+
+		if (right_shoes_data_rssi_vector.front().rssi_count == 0)
+		{
+			ProcessResult(right_global_pos, right_shoes_data_rssi_vector.front().step_data_deque, right_global_pos_vector, right_cmd_status, RIGHT_FOOT);
+
+			right_shoes_data_rssi_vector.pop_front();
+		}
+
+	}
+
+
+	//往下是处理长按命令所需
+	if (last_left_cmd_status != left_cmd_status && left_cmd_status != -1)
+	{
+		result[0] = left_cmd_status;
+
+		left_cmd_cur_status = left_cmd_status;
+	}
+	else
+	{
+		result[0] = -1;
+	}
+
+	if (last_left_zupt == 1 && left_zupt == 0)
+	{
+		result[0] = left_cmd_cur_status | MOTION_CANCEL;
+	}
+
+	if (last_right_cmd_status != right_cmd_status && right_cmd_status != -1)
+	{
+		result[1] = right_cmd_status;
+
+		right_cmd_cur_status = right_cmd_status;
+	}
+	else
+	{
+		result[1] = -1;
+	}
+
+	if (last_right_zupt == 1 && right_zupt == 0)
+	{
+		result[1] = right_cmd_cur_status | MOTION_CANCEL;
+	}
+
+
+	last_left_cmd_status = left_cmd_status;
+	last_right_cmd_status = right_cmd_status;
+
+	last_left_zupt = left_zupt;
+	last_right_zupt = right_zupt;
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+}

footRing_sdk_1.0/src/FingerGuessing.cpp

@@ -0,0 +1,396 @@
+#include "FingerGuessing.h"
+
+FingerGuessing::FingerGuessing()
+{
+	left_foot_has_init = 0;
+	right_foot_has_init = 0;
+
+	left_heading = 0;
+	right_heading = 0;
+
+	left_zupt_count = 0;
+	right_zupt_count = 0;
+
+
+	right_cmd_wait_time = 0;
+	left_cmd_wait_time = 0;
+
+	memset(left_cmd_global_pos, 0, 3 * sizeof(float));
+	memset(right_cmd_global_pos, 0, 3 * sizeof(float));
+
+	left_init = 0;
+	right_init = 0;
+
+	//原点
+	left_global_pos_vector.push_back({ 0, 0, 0, 0 });
+	right_global_pos_vector.push_back({ 0, 0, 0, 0 });
+
+}
+
+
+void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& step_data_vector,
+	deque<globalPos_struct>& global_pos_vector, int LEFT_OR_RIGHT)
+{
+
+	if (step_data_vector.size() > 1 && step_data_vector.front().zupt == 1 && step_data_vector.back().zupt == 1)
+	{
+		if (LEFT_OR_RIGHT == LEFT_FOOT)
+		{
+			std::cout << "Left Foot : " << endl << endl;
+		}
+		else
+		{
+			std::cout << "Right Foot : " << endl << endl;
+		}
+
+		std::cout << "FingerGuessing::ProcessResult" << endl;
+		std::cout << endl;
+
+		//寻找到距离最小的值
+		int vector_length = global_pos_vector.size();
+
+		float distance_point = 0.05f;
+
+		int index = -1;
+
+
+		for (int i = 0; i < vector_length; i++)
+		{
+			//回溯寻找曾经踩过的点
+			float dx = global_pos[0] - global_pos_vector.at(i).pos_x;
+			float dy = global_pos[1] - global_pos_vector.at(i).pos_y;
+
+			float distance_temp = sqrt(dx * dx + dy * dy);
+
+			float global_pos_distance = sqrt(global_pos_vector.at(i).pos_x * global_pos_vector.at(i).pos_x + global_pos_vector.at(i).pos_y * global_pos_vector.at(i).pos_y);
+
+			if (i == 0 && distance_temp < distance_point)
+			{
+
+				index = i;
+
+				distance_point = distance_temp;
+
+			}
+			else if (i != 0 && distance_temp < distance_point)
+			{
+
+				index = i;
+
+				distance_point = distance_temp;
+
+			}
+		}
+
+
+		if (index == -1)
+		{
+			std::cout << "no find similar point" << endl;
+			//没有找到基准点的时候，检测到往返点的时候,再做判断
+			//当检测到接近原点的时候，应当设置为0
+
+			global_pos_vector.push_back({ 0, global_pos[0], global_pos[1],global_pos[2] });
+
+			std::cout << " pos_x : " << global_pos_vector.back().pos_x
+				<< " pos_y : " << global_pos_vector.back().pos_y
+				<< " pos_z : " << global_pos_vector.back().pos_z << endl;
+
+		}
+		else
+		{
+			std::cout << "has find similar point, index : " << index << endl;
+
+			global_pos[0] = global_pos_vector.at(index).pos_x;
+			global_pos[1] = global_pos_vector.at(index).pos_y;
+			global_pos[2] = global_pos_vector.at(index).pos_z;
+		}
+
+
+		/*if ( sqrt(global_pos[0] * global_pos[0] + global_pos[1] * global_pos[1]) < 0.1f)
+		{
+
+			std::cout << "if (sqrt(global_pos[0] * global_pos[0] + global_pos[1] * global_pos[1]) < 0.1f)" << endl;
+
+			global_pos_vector.clear();
+
+			global_pos_vector.push_back({ 0, 0, 0, 0 });
+
+			memset(global_pos, 0, 3 * sizeof(float));
+		}*/
+
+
+	}
+
+	//为了防止误触点，用五个触地点来测试
+
+}
+
+int FingerGuessing::ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_data_vector, deque<shoes_data_cell>& right_cmd_step_data_vector,
+	float* left_cmd_global_pos, float* right_cmd_global_pos, int rssi)
+{
+	if (!(left_cmd_step_data_vector.size() > 2 && right_cmd_step_data_vector.size() > 2))
+	{
+		return -1;
+	}
+
+	float left_dx = left_cmd_step_data_vector.back().pos_x - left_cmd_step_data_vector.front().pos_x;
+	float right_dx = right_cmd_step_data_vector.back().pos_x - right_cmd_step_data_vector.front().pos_x;
+
+	float left_dy = left_cmd_step_data_vector.back().pos_y - left_cmd_step_data_vector.front().pos_y;
+	float right_dy = right_cmd_step_data_vector.back().pos_y - right_cmd_step_data_vector.front().pos_y;
+
+	if (sqrt(left_dx * left_dx + left_dy * left_dy) < 0.1f || sqrt(right_dx * right_dx + right_dy * right_dy) < 0.1f)
+	{
+		return -1;
+	}
+
+	//还是判断拳头
+
+	if (rssi > 30)
+	{
+
+		float dx = right_dx - left_dx;
+		float dy = right_dy - left_dy;
+
+		if (right_dx - left_dx > 0.3f)
+		{
+			left_cmd_global_pos[0] = -0.25f; left_cmd_global_pos[1] = 0.0f;
+			right_cmd_global_pos[0] = 0.25f; right_cmd_global_pos[1] = 0.0f;
+
+			return MOTION_PAPER;
+		}
+
+		//判断剪刀有点麻烦
+		if (right_dx - left_dx < -0.3f)
+		{
+
+			if (left_dy - right_dy < -0.3f)
+			{
+				left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = -0.15f;
+				right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = 0.15f;
+				return MOTION_SCISSORS;
+			}
+
+			if (left_dy - right_dy > 0.3f)
+			{
+				left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = 0.15f;
+				right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = -0.15f;
+				return MOTION_SCISSORS;
+			}
+
+		}
+	}
+
+
+	return -1;
+}
+
+
+
+void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	//由于刚触地的时候,会偶尔丢掉触地信号的关键帧，现在在线补充一下
+
+	//int online_zupt = extract_motion.online_detect_touch_floor(right_front_press, left_front_press, right_back_press, left_back_press);
+
+	//if (online_zupt & 0x01)
+	//{
+	//	std::cout << "test :: left_zupt" << endl;
+	//	left_zupt = 1;
+	//}
+	//if (online_zupt & 0x02)
+	//{
+	//	std::cout << "test :: right_zupt" << endl;
+	//	right_zupt = 1;
+	//}
+
+
+
+	//缓存一步内的空中数据
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+
+	//根据新来的数据，计算的全局位置
+	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 0, RIGHT_FOOT);
+
+	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
+
+
+	//不断根据rssi和触地信号来估计归位，以及方向
+	resetZerosPointByRssi(left_zupt, 10, left_zupt_count, left_foot_has_init, left_att[0] * 0.0001f, left_heading, rssi, 25, left_global_pos);
+	resetZerosPointByRssi(right_zupt, 10, right_zupt_count, right_foot_has_init, right_att[0] * 0.0001f, right_heading, rssi, 25, right_global_pos);
+
+
+	//通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳，这个时间戳是起延时判断的效果，延时是为了等合适的RSSI
+	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
+	{
+		if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
+		{
+			//根据过往记录的位置点来，来优化
+			//ProcessResult(left_global_pos, left_step_data_vector, left_global_pos_vector, LEFT_FOOT);
+
+			left_cmd_step_data_vector = left_step_data_vector;
+
+			float dy = left_step_data_vector.back().pos_y - left_step_data_vector.front().pos_y;
+			float dx = left_step_data_vector.back().pos_x - left_step_data_vector.front().pos_x;
+
+			if (sqrt(dx * dx + dy * dy) < 0.05)
+			{
+				left_global_pos[0] -= dx;
+				left_global_pos[1] -= dy;
+			}
+			memcpy(left_cmd_global_pos, left_global_pos, 3 * sizeof(float));
+
+			left_cmd_wait_time = time_stamp;
+
+			left_init = 1;
+		}
+
+	}
+
+	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
+	{
+		if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
+		{
+			//根据过往记录的位置点来，来优化
+			//ProcessResult(right_global_pos, right_step_data_vector, right_global_pos_vector, RIGHT_FOOT);
+
+			right_cmd_step_data_vector = right_step_data_vector;
+
+			float dy = right_step_data_vector.back().pos_y - right_step_data_vector.front().pos_y;
+			float dx = right_step_data_vector.back().pos_x - right_step_data_vector.front().pos_x;
+
+			if (sqrt(dx * dx + dy * dy) < 0.05)
+			{
+				right_global_pos[0] -= dx;
+				right_global_pos[1] -= dy;
+			}
+
+			memcpy(right_cmd_global_pos, right_global_pos, 3 * sizeof(float));
+
+			right_cmd_wait_time = time_stamp;
+
+			right_init = 1;
+		}
+
+	}
+
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+
+	//重置命令
+	memset(result, -1, 4 * sizeof(int));
+
+	//判断双脚触地的时候, rssi < 23 必须归位，不然也没办法处理
+	//看起来rssi的延迟有够烂的
+
+
+	if (left_init == 1 && right_init == 1 &&
+		time_stamp - left_cmd_wait_time > 9 && time_stamp - left_cmd_wait_time < 40 && time_stamp - right_cmd_wait_time > 9 && time_stamp - right_cmd_wait_time < 40)
+	{
+
+		//rssi 有延迟，判断延迟50ms, 直接判断如果这时候的RSSI 小于阈值， 直接设置为0
+		if (rssi < rssi_threshold)
+		{
+			for (int i = 0; i < 3; i++)
+			{
+				left_global_pos[i] -= left_cmd_global_pos[i];
+				right_global_pos[i] -= right_cmd_global_pos[i];
+			}
+			memset(left_cmd_global_pos, 0, 3 * sizeof(float));
+			memset(right_cmd_global_pos, 0, 3 * sizeof(float));
+
+			right_global_pos_vector.clear();
+			left_global_pos_vector.clear();
+
+			left_global_pos_vector.push_back({ 0,0,0,0 });
+			right_global_pos_vector.push_back({ 0,0,0,0 });
+		}
+
+		//尝试用特殊位移来计算结果， 左右想外移动，就肯定是布，如果左右脚收缩，上下有移动，rssi超过阈值就是剪刀了
+		if (ProcessDualStepVector(left_cmd_step_data_vector, right_cmd_step_data_vector, left_cmd_global_pos, right_cmd_global_pos, rssi) != -1)
+		{
+			memcpy(right_global_pos, right_cmd_global_pos, 3 * sizeof(float));
+			memcpy(left_global_pos, left_cmd_global_pos, 3 * sizeof(float));
+		}
+
+
+
+		float dx = left_cmd_global_pos[0] - right_cmd_global_pos[0];
+
+		float dy = left_cmd_global_pos[1] - right_cmd_global_pos[1];
+
+		//根据位移来先处理
+		if (sqrt(dx * dx + dy * dy) < 0.1f)
+		{
+			addMotionCount(ROCK_COUNT);
+
+			result[0] = MOTION_ROCK;
+
+			std::cout << "FingureResult : MOTION_ROCK" << endl;
+		}
+		else
+		{
+
+			std::cout << "dx : " << dx << " dy : " << dy << endl;
+
+			if ((dx > 0.12f || dx < -0.12f) && fabsf(dy) < 3.0 / 4 * fabsf(dx))
+			{
+				//统计步数
+				addMotionCount(PAPER_COUNT);
+
+				result[0] = MOTION_PAPER;
+
+				std::cout << "MOTION_PAPER, RSSI : " << rssi << endl;
+
+				std::cout << "FingureResult : MOTION_PAPER" << endl;
+
+			}
+			else if ((dy > 0.12f || dy < -0.12f) && fabsf(dx) < 3.0 / 4 * fabsf(dy))
+			{
+				//统计步数
+				addMotionCount(SCISSORS_COUNT);
+
+				result[0] = MOTION_SCISSORS;
+
+				std::cout << "FingureResult : MOTION_SCISSORS" << endl;
+
+
+			}
+			/*else if (dy > 0.3f || dy < -0.3f || dx > 0.3f || dx < -0.3f)*/
+			else
+			{
+				if (fabsf(dy) > fabsf(dx))
+				{
+					addMotionCount(SCISSORS_COUNT);
+
+					result[0] = MOTION_SCISSORS;
+
+					std::cout << "dy > dx FingureResult : MOTION_SCISSORS" << endl;
+				}
+				else
+				{
+					addMotionCount(PAPER_COUNT);
+
+					result[0] = MOTION_PAPER;
+
+					std::cout << "dy < dx FingureResult : MOTION_PAPER" << endl;
+				}
+			}
+
+
+		}
+
+		left_cmd_wait_time = 0;
+		right_cmd_wait_time = 0;
+
+	}
+
+}

footRing_sdk_1.0/src/FootStep.cpp

@@ -0,0 +1,410 @@
+#include "FootStep.h"
+#include <algorithm>
+
+FootStep::FootStep()
+{
+	/*
+		初始化均为0
+	*/
+	stepStatus = 0;
+	stepFreq = 0;
+	stepTag = 0;
+	zuptCount = 0;
+
+	lastZupt = 1;
+
+	leaveFloorTime1 = 0;
+	leaveFloorTime2 = 0;
+
+	leaveFloorTime = 0;
+
+	leaveTimeStatus = 1;
+
+	catchFloorTime = 0;
+
+	localTime = 0;
+
+	last_time_stamp = 0;
+
+	stepCount = 0;
+
+	memset(last_pos, 0, 3 * sizeof(int));
+
+	memset(step_vel_buff, 0, 3 * sizeof(float));
+
+	mean_vel = 0;
+
+	mean_window_vel = 0;
+
+
+	front_mag_min = 40000;
+	back_mag_min = 40000;
+
+	unzupt_count = 0;
+	memset(max_acc, 0, 3 * sizeof(float));
+	memset(min_acc, 0, 3 * sizeof(float));
+}
+
+void calStepFreq(vector<FLOOR_TIME_STYPE>& time_vector, float &mean_step_vel)
+{
+	int length = time_vector.size();
+
+	if (length < 2)
+	{
+		cout << "FootStep::calStepFreq vector_length < 2" << endl;
+
+		return;
+	}
+
+	FLOOR_TIME_STYPE cur_floor_time_stype = time_vector[length - 1];
+
+	FLOOR_TIME_STYPE last_floor_time_stype = time_vector[length - 3];
+
+	if (cur_floor_time_stype.time_type != LEAVE_TIME_STYPE || last_floor_time_stype.time_type != LEAVE_TIME_STYPE
+		|| time_vector[length - 2].time_type != CATCH_TIME_STYPE)
+	{
+		cout << "FootStep::calStepFreq time_stype is not valid " << endl;
+
+		for (int i = 0; i < length; i++)
+		{
+			cout << time_vector[i].time << ", ";
+			if (time_vector[i].time_type == LEAVE_TIME_STYPE)
+			{
+				cout << "LEAVE_TIME_STYPE," << endl;
+			}
+			else
+			{
+				cout << "CATCH_TIME_STYPE," << endl;
+			}
+		}
+	}
+	else
+	{
+		mean_step_vel = 100.f / (cur_floor_time_stype.time - last_floor_time_stype.time);
+	}
+
+	//删除所有的点
+	time_vector.clear();
+	time_vector.push_back(cur_floor_time_stype);
+
+} 
+
+int check_time_vector(vector<FLOOR_TIME_STYPE>& time_vector)
+{
+	int length = time_vector.size();
+
+	if (length < 2)
+	{
+		return 0;
+	}
+
+	FLOOR_TIME_STYPE cur_floor_time_stype = time_vector[length - 1];
+
+	vector<FLOOR_TIME_STYPE> time_vector_tmp(time_vector);
+
+	cout << "FootStep::time_vector before" << endl;
+
+	for (int i = 0; i < time_vector.size(); i++)
+	{
+		cout << time_vector[i].time << ", ";
+
+		if (time_vector[i].time_type == LEAVE_TIME_STYPE)
+		{
+			cout << "LEAVE_TIME_STYPE, ";
+		}
+		else
+		{
+			cout << "CATCH_TIME_STYPE, ";
+		}
+	}
+	cout << endl;
+
+
+	time_vector.clear();
+
+	time_vector.push_back(time_vector_tmp[0]);
+	
+	for (int i = 1; i < time_vector_tmp.size(); i++)
+	{
+
+		//按照人步伐，也不可能触地以及在空中的时间是小于50ms的
+		if (time_vector_tmp[i].time - time_vector_tmp[i - 1].time >= 5)
+		{
+			time_vector.push_back(time_vector_tmp[i]);
+		}
+		else
+		{
+			std::cout << "delect time_vector_tmp cell " << endl;
+		}
+
+	}
+
+	cout << "FootStep::time_vector mid " << endl;
+
+	for (int i = 0; i < time_vector.size(); i++)
+	{
+
+		cout << time_vector[i].time << ", ";
+
+		if (time_vector[i].time_type == LEAVE_TIME_STYPE)
+		{
+			cout << "LEAVE_TIME_STYPE, " ;
+		}
+		else
+		{
+			cout << "CATCH_TIME_STYPE, ";
+		}
+	}
+	cout << endl;
+
+	time_vector_tmp = time_vector;
+	time_vector.clear();
+	time_vector.push_back(time_vector_tmp[0]);
+	
+	int index = 1;
+	while (index < time_vector_tmp.size())
+	{
+		if (time_vector_tmp[index].time_type != time_vector[time_vector.size() - 1].time_type)
+		{
+			time_vector.push_back(time_vector_tmp[index]);
+		}
+
+		index++;
+	}
+
+	cout << "FootStep::time_vector end" << endl;
+
+	for (int i = 0; i < time_vector.size(); i++)
+	{
+
+		cout << time_vector[i].time << ", ";
+
+		if (time_vector[i].time_type == LEAVE_TIME_STYPE)
+		{
+			cout << "LEAVE_TIME_STYPE, ";
+		}
+		else
+		{
+			cout << "CATCH_TIME_STYPE, ";
+		}
+	}
+	cout << endl;
+
+
+
+	if (time_vector.size() < 3)
+	{
+		return 0;
+	}
+
+	//std::cout << "time_vector.size() : " << time_vector.size() << endl;
+
+	if (time_vector[time_vector.size() - 1].time_type == LEAVE_TIME_STYPE && 
+		time_vector[time_vector.size() - 2].time_type == CATCH_TIME_STYPE &&
+		time_vector[time_vector.size() -3].time_type == LEAVE_TIME_STYPE)
+	{
+		return 1;
+	}
+	//std::cout << "time_vector[time_vector.size() -3].time_type == LEAVE_TIME_STYPE " << endl;
+
+	return 0;
+}
+
+void FootStep::stepCal(int timeStamp, int zupt, int front_mag, int back_mag, int* acc)
+{
+
+	vel_sqrt_num++;
+
+	int timeDistance;
+
+	if (timeStamp - last_time_stamp < 0)
+	{
+		timeDistance = 256 + timeStamp - last_time_stamp;
+	}
+	else
+	{
+		timeDistance = timeStamp - last_time_stamp;
+	}
+
+	last_time_stamp = timeStamp;
+
+	localTime += timeDistance;
+
+
+	//计算触地时间
+	if (lastZupt == 0 && zupt == 1)
+	{
+		if (max_acc[0] - min_acc[0] > 2048 || max_acc[1] - min_acc[1] > 2048 || max_acc[2] - min_acc[2] > 2048)
+		{
+			catchFloorTime = localTime;
+
+			time_vector.push_back({ CATCH_TIME_STYPE, localTime });
+		}
+		//catchFloorTime = localTime;
+
+		//time_vector.push_back({ CATCH_TIME_STYPE, localTime });
+	}
+
+
+	if (zupt == 0)
+	{
+		for (int i = 0; i < 3; i++)
+		{
+			if (max_acc[i] < acc[i])
+			{
+				max_acc[i] = acc[i];
+			}
+
+			if (min_acc[i] > acc[i])
+			{
+				min_acc[i] = acc[i];
+			}
+		}
+		unzupt_count++;
+
+	}
+	else
+	{
+		memcpy(max_acc, acc, 3 * sizeof(int));
+		memcpy(min_acc, acc, 3 * sizeof(int));
+
+		unzupt_count = 0;
+	}
+
+
+
+	if (zupt == 0 && lastZupt == 1)
+	{
+		leave_time_wait = 10;
+
+		leaveFloorTime = localTime;
+
+		time_vector.push_back({ LEAVE_TIME_STYPE, localTime });
+	}
+
+	if (leave_time_wait > 0)
+	{
+		if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)
+		{
+			//std::cout << "if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)" << endl;
+			if (check_time_vector(time_vector))
+			{
+
+				//std::cout << "if (check_time_vector(time_vector))" << endl;
+
+				calStepFreq(time_vector, mean_vel);
+
+				memcpy(step_vel_buff, step_vel_buff + 1, 2 * sizeof(float));
+
+				step_vel_buff[2] = mean_vel;
+
+				cout << "now step mean_vel is " << mean_vel << endl;
+
+				mean_window_vel = (step_vel_buff[0] + step_vel_buff[1] + step_vel_buff[2]) / 3.0f;
+
+				stepFreq = mean_window_vel * 60;
+
+				cout << "now step mean_window_vel is " << mean_window_vel << endl;
+
+				memcpy(vel_sqrt_buff, vel_sqrt_buff + 1, 2 * sizeof(float));
+
+				vel_sqrt_buff[2] = vel_sqrt_sum / vel_sqrt_num * mean_vel;
+
+				cout << "now step vel_sqrt_buff_mean is " << (vel_sqrt_buff[0] + vel_sqrt_buff[1] + vel_sqrt_buff[2]) / 3.0f << endl;
+
+				vel_sqrt_sum = 0;
+				vel_sqrt_num = 0;
+
+				stepCount++;
+
+				std::cout << "step_count : " << stepCount << endl;
+			}
+
+			leave_time_wait = 0;
+
+			std::cout << "leave_time_wait = 0;" << endl;
+		}
+
+		leave_time_wait--;
+		if (leave_time_wait == 0 && time_vector.size() > 0)
+		{
+			//std::cout << "if (leave_time == 0 && time_vector.size() > 0)" << endl;
+			time_vector.pop_back();
+		}
+	}
+
+
+
+	lastZupt = zupt;
+
+	//空中悬停也应当判断为不动
+	if (acc_z_deque.size() > 9)
+	{
+		acc_z_deque.pop_front();
+	}
+
+	acc_z_deque.push_back(acc[0]);
+
+
+	if (zupt == 1 ||
+		(acc_z_deque.size() == 10
+		&& (*(std::max_element(acc_z_deque.begin(), acc_z_deque.end())) - *(std::min_element(acc_z_deque.begin(), acc_z_deque.end())) < 512))
+		)
+	{
+		zuptCount ++;
+
+	}
+	else
+	{
+		zuptCount = 0;
+	}
+
+	
+	if (zuptCount > 50)
+	{
+		memcpy(step_vel_buff, step_vel_buff + 1, 2 * sizeof(float));
+
+		step_vel_buff[2] = 0.0f;
+
+		mean_window_vel = (step_vel_buff[0] + step_vel_buff[1] + step_vel_buff[2]) / 3.0f;
+
+		stepFreq = mean_window_vel * 60;
+
+		cout << "stop signal : now step mean_window_vel is " << mean_window_vel <<endl;
+
+		cout << step_vel_buff[0] << ", " << step_vel_buff[1] << ", " << step_vel_buff[2] << endl;
+
+		zuptCount = 0;
+
+		memcpy(vel_sqrt_buff, vel_sqrt_buff + 1, 2 * sizeof(float));
+
+		vel_sqrt_buff[2] = 0;
+
+		vel_sqrt_sum = 0;
+		vel_sqrt_num = 0;
+
+	}
+
+}
+
+int FootStep::getStepFreq()
+{
+	return mean_window_vel * 60;
+}
+
+int FootStep::getStepStatus()
+{
+	if (stepFreq == 0)
+		return 0;
+
+	else if (stepFreq > 110)
+		return 2;
+
+	else
+		return 1;
+}
+
+int FootStep::getStepCount()
+{
+	return stepCount;
+}

footRing_sdk_1.0/src/Game.cpp

@@ -0,0 +1,312 @@
+#include "Game.h"
+
+Game::Game(int gametype)
+{
+	GameType = gametype;
+
+	InteractionCMD = -1;
+
+	shoes_down = 0;
+
+	setShoesDataBuff_count = 0;
+
+	shoes_data_buff_size = 5;
+
+	leftFootStep = FootStep();
+	rightFootStep = FootStep();
+	stepVel = StepVel();
+
+	interaction = Interaction();
+
+	memset(resultMatrix, 0, RESULT_LEN * sizeof(int));
+
+	if (gametype == RUNGAME)
+	{
+		game = new RunGame();
+	}
+	else if (gametype == DANCEGAME)
+	{
+		game = new DanceGame();
+	}
+	else if (gametype == CYCLE)
+	{
+		game = new Tricycle();
+	}
+	else if (gametype == H5GAME)
+	{
+		game = new H5();
+	}
+	else if (gametype == FINGERGUESSING)
+	{
+		game = new FingerGuessing();
+	}
+	else if (gametype == AEROBICS)
+	{
+		game = new Aerobics();
+	}
+	else if (gametype == KONGFU)
+	{
+		game = new Kongfu();
+	}
+	else if (gametype == SPORT)
+	{
+		game = new SportFunction();
+	}
+	else if (gametype == BUILDHOUSE)
+	{
+		game = new BuildHouse();
+	}
+	else if (gametype == CUTCAKE)
+	{
+		game = new CutCake();
+	}
+	else if (gametype == JUMPHOUSE)
+	{
+		game = new JumpHouse();
+	}
+	else
+	{
+		game = new TestGame();
+	}
+}
+
+int Game::getInteractionCMD()
+{
+	return interaction.get_interative_cmd();
+}
+
+int Game::getMotionCount(int motion_type)
+{
+	return game->getMotionCount(motion_type);
+}
+
+
+void Game::GameProcess(int gameTime, int* right_pos_data, int* right_att_data, int* right_acc_data, int right_zupt, int right_front_mag, int right_back_mag,
+	int* left_pos_data, int* left_att_data, int* left_acc_data, int left_zupt, int left_front_mag, int left_back_mag, 
+	int jump, int down, int rssi)
+{
+
+	//游戏的主逻辑入口
+	game->Process(gameTime, right_pos_data, right_att_data, right_acc_data, right_zupt, right_front_mag, right_back_mag,
+		left_pos_data, left_att_data, left_acc_data, left_zupt, left_front_mag, left_back_mag, jump, down, rssi);
+	
+	//获取游戏结果
+	game->getResult(resultMatrix);
+
+
+	/////*交互命令计算*/
+	//interaction.Process(gameTime, right_pos_data, right_att_data, right_acc_data, right_zupt, right_front_mag, right_back_mag,
+	//	left_pos_data, left_att_data, left_acc_data, left_zupt, left_front_mag, left_back_mag, jump, down, rssi);
+
+
+	/////*步频计算*/
+	//leftFootStep.stepCal(gameTime, left_zupt, left_front_mag, left_back_mag, left_acc_data);
+
+	//rightFootStep.stepCal(gameTime, right_zupt, right_front_mag, right_back_mag, right_acc_data);
+
+	///*利用脚步抬高的高度以及步频 平衡出一个速度*/
+	//stepVel.Process(gameTime, right_pos_data, right_att_data, right_acc_data, right_zupt, right_front_mag, right_back_mag,
+		//left_pos_data, left_att_data, left_acc_data, left_zupt, left_front_mag, left_back_mag, jump, down, rssi);
+
+
+}
+
+void Game::getGameResult(int* matrix)
+{
+	memcpy(matrix, resultMatrix, RESULT_LEN * sizeof(int));
+}
+
+int Game::getStepStatus(int left_or_right)
+{
+	if (left_or_right == LEFT_FOOT)
+		return leftFootStep.getStepStatus();
+
+	else if (left_or_right == RIGHT_FOOT)
+		return rightFootStep.getStepStatus();
+
+	else
+		return -1;
+}
+
+int Game::getStepFreq( int left_or_right)
+{
+	if (left_or_right == LEFT_FOOT)
+		return leftFootStep.getStepFreq();
+
+	else if (left_or_right == RIGHT_FOOT)
+		return rightFootStep.getStepFreq();
+
+	else
+		return -1;
+}
+
+int Game::getGameStepVel()
+{
+	return stepVel.getStepVel();
+}
+
+
+int Game::getStepCount(int left_or_right)
+{
+	if (left_or_right == LEFT_FOOT)
+		return leftFootStep.getStepCount();
+
+	else if (left_or_right == RIGHT_FOOT)
+		return rightFootStep.getStepCount();
+
+	else
+		return -1;
+}
+//
+float Game::getGamePos(int left_or_right, int index)
+{
+	if (index < 0 || index >2)
+		return -1;
+
+	return game->getGamePos(left_or_right, index);
+
+}
+
+string Game::getVersion()
+{
+	return GAME_VERSION;
+}
+
+string Game::getGameDataStr()
+{
+	string data_str_temp = "";
+
+	//先记录右边
+
+	data_str_temp = data_str;
+
+	data_str = "";
+
+	return data_str_temp;
+
+}
+
+
+void Game::setGameDataStr()
+{
+	if (data_str.size() != 0)
+	{
+		data_str = "";
+	}
+	data_str += (to_string(right_pos_data[0]) + "," + to_string(right_pos_data[1]) + "," + to_string(right_pos_data[2]) + ",");
+	data_str += (to_string(right_att_data[0]) + "," + to_string(right_att_data[1]) + "," + to_string(right_att_data[2]) + ",");
+	data_str += (to_string(right_acc_data[0]) + "," + to_string(right_acc_data[1]) + "," + to_string(right_acc_data[2]) + ",");
+
+	data_str += (to_string(right_front_mag) + "," + to_string(right_back_mag) + "," + to_string(s_zupt) + ",");
+
+	data_str += (to_string(left_pos_data[0]) + "," + to_string(left_pos_data[1]) + "," + to_string(left_pos_data[2]) + ",");
+	data_str += (to_string(left_att_data[0]) + "," + to_string(left_att_data[1]) + "," + to_string(left_att_data[2]) + ",");
+	data_str += (to_string(left_acc_data[0]) + "," + to_string(left_acc_data[1]) + "," + to_string(left_acc_data[2]) + ",");
+
+	data_str += (to_string(left_front_mag) + "," + to_string(left_back_mag) + "," + to_string(h_zupt) + ",");
+
+	data_str += (to_string(shoes_down) + "," + to_string(jump) + "," + to_string(rssi) + "," + to_string(posTimeStamp) + ",");
+
+	data_str += (to_string(right_gyr_data[0]) + "," + to_string(right_gyr_data[1]) + "," + to_string(right_gyr_data[2]) + ",");
+
+	data_str += (to_string(left_gyr_data[0]) + "," + to_string(left_gyr_data[1]) + "," + to_string(left_gyr_data[2]));
+
+}
+
+void Game::getFootAtt(int* left_att, int* right_att)
+{
+	memcpy(right_att, right_att_data, 3 * sizeof(int));
+	memcpy(left_att, left_att_data, 3 * sizeof(int));
+	
+}
+
+
+//解析鞋子的数据
+void Game::GameProcessBuf(uint8_t* buf, int len) {
+
+	right_pos_data[0] = int32_t(buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3] << 0);
+	right_pos_data[1] = int32_t(buf[4] << 24 | buf[5] << 16 | buf[6] << 8 | buf[7] << 0);
+	right_pos_data[2] = int32_t(buf[8] << 24 | buf[9] << 16 | buf[10] << 8 | buf[11] << 0);
+
+	left_pos_data[0] = int32_t(buf[12] << 24 | buf[13] << 16 | buf[14] << 8 | buf[15] << 0);
+	left_pos_data[1] = int32_t(buf[16] << 24 | buf[17] << 16 | buf[18] << 8 | buf[19] << 0);
+	left_pos_data[2] = int32_t(buf[20] << 24 | buf[21] << 16 | buf[22] << 8 | buf[23] << 0);
+
+	right_att_data[0] = int16_t(buf[24] << 8 | buf[25] << 0);
+	right_att_data[1] = int16_t(buf[26] << 8 | buf[27] << 0);
+	right_att_data[2] = int16_t(buf[28] << 8 | buf[29] << 0);
+
+	left_att_data[0] = int16_t(buf[30] << 8 | buf[31] << 0);
+	left_att_data[1] = int16_t(buf[32] << 8 | buf[33] << 0);
+	left_att_data[2] = int16_t(buf[34] << 8 | buf[35] << 0);
+
+	right_acc_data[0] = int16_t(buf[36] << 8 | buf[37] << 0);
+	right_acc_data[1] = int16_t(buf[38] << 8 | buf[39] << 0);
+	right_acc_data[2] = int16_t(buf[40] << 8 | buf[41] << 0);
+
+	left_acc_data[0] = int16_t(buf[42] << 8 | buf[43] << 0);
+	left_acc_data[1] = int16_t(buf[44] << 8 | buf[45] << 0);
+	left_acc_data[2] = int16_t(buf[46] << 8 | buf[47] << 0);
+
+	girl_shoes = (buf[48] & 0x16) >> 4;
+
+	s_zupt = (buf[48] & 0x08) >> 3;
+
+	h_zupt = (buf[48] & 0x04) >> 2;
+
+	shoes_down = (buf[48] & 0x02) >> 1;
+
+	jump = (buf[48] & 0x01) >> 0;
+
+	rssi = (int) buf[49];
+
+	posTimeStamp = buf[50];
+
+	if (lastTimeStamp == -1)
+	{
+		lastTimeStamp = posTimeStamp;
+
+		return;
+
+	}
+
+	if (posTimeStamp - lastTimeStamp > - 100 && posTimeStamp - lastTimeStamp <=0)
+	{
+		std::cout << "receive old shoes package ( lastTimeStamp : " << lastTimeStamp << " posTimeStamp : " << posTimeStamp << " )" <<endl;
+
+		//lastTimeStamp = posTimeStamp;
+
+		return;
+	}
+
+	gameTime += (posTimeStamp - lastTimeStamp < 0 ? posTimeStamp - lastTimeStamp + 256 : posTimeStamp - lastTimeStamp);
+
+	//std::cout << "gameTime : " << gameTime <<endl;
+
+	lastTimeStamp = posTimeStamp;
+
+	right_front_mag = uint16_t(buf[51] << 8 | buf[52] << 0);
+	right_back_mag =  uint16_t(buf[53] << 8 | buf[54] << 0);
+
+	left_front_mag = uint16_t(buf[55] << 8 | buf[56] << 0);
+	left_back_mag =  uint16_t(buf[57] << 8 | buf[58] << 0);
+
+	if (len > 59)
+	{
+		right_gyr_data[0] = int16_t(buf[59] << 8 | buf[60] << 0);
+		right_gyr_data[1] = int16_t(buf[61] << 8 | buf[62] << 0);
+		right_gyr_data[2] = int16_t(buf[63] << 8 | buf[64] << 0);
+
+		left_gyr_data[0] =  int16_t(buf[65] << 8 | buf[66] << 0);
+		left_gyr_data[1] =  int16_t(buf[67] << 8 | buf[68] << 0);
+		left_gyr_data[2] =  int16_t(buf[69] << 8 | buf[70] << 0);
+	}
+
+	setGameDataStr();
+
+	GameProcess(gameTime, right_pos_data, right_att_data, right_acc_data, s_zupt, right_front_mag, right_back_mag,
+		left_pos_data, left_att_data, left_acc_data, h_zupt, left_front_mag, left_back_mag, jump, shoes_down, rssi);
+
+
+}
+

footRing_sdk_1.0/src/H5_Game.cpp

@@ -0,0 +1,132 @@
+#include "H5_Game.h"
+#include <string.h>
+
+//last_jump and now_jump need wait 200ms
+
+#define WAIT_TIME 10
+
+H5::H5()
+{
+
+	last_left_zupt = 1;
+	last_right_zupt = 1;
+
+	CANCLE_DOWN = 0;
+	CANCLE_JUMP = 0;
+
+	valid_down_wait_time = 0;
+	valid_jump_wait_time = 0;
+
+	last_jump = 0;
+
+	last_down = 0;
+}
+
+void H5::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+
+	//获取触地
+	if (last_left_zupt == 0 && left_zupt == 1)
+	{
+		//统计步数
+		addMotionCount(STEP_COUNT);
+
+		std::cout << "H5GAME:: MOTION_LEFT" << endl;
+		result[0] = MOTION_LEFT;
+	}
+	else
+	{
+		result[0] = -1;
+	}
+
+	if (last_right_zupt == 0 && right_zupt == 1)
+	{
+		//统计步数
+		addMotionCount(STEP_COUNT);
+
+		std::cout << "H5GAME:: MOTION_RIGHT" << endl;
+		result[1] = MOTION_RIGHT;
+	}
+	else
+	{
+		result[1] = -1;
+	}
+
+
+	if (last_jump == 0 && jump == 1)
+	{
+		//统计步数
+		addMotionCount(JUMP_COUNT);
+
+		std::cout << "H5GAME:: MOTION_JUMP" << endl;
+
+		result[2] = MOTION_JUMP;
+
+		valid_jump_wait_time = WAIT_TIME;
+	}
+	else
+	{
+		result[2] = -1;
+	}
+
+	//reset jump wait time when both foot isn't on the floor
+	if (( left_zupt == 0 && right_zupt == 0) || jump == 1)
+	{
+		valid_jump_wait_time = WAIT_TIME;
+	}
+
+	if (valid_jump_wait_time > 0)
+	{
+		CANCLE_JUMP = 1;
+
+		valid_jump_wait_time--;
+	}
+	else
+	{
+		CANCLE_JUMP = 0;
+	}
+
+
+	//same process with jump
+	if (last_down == 0 && down == 1 )
+	{
+		//统计步数
+		addMotionCount(DOWN_COUNT);
+
+		std::cout << "H5GAME:: MOTION_DOWN" << endl;
+
+		result[3] = MOTION_DOWN;
+
+		valid_down_wait_time = WAIT_TIME;
+	}
+	else
+	{
+		result[3] = -1;
+	}
+
+	//if ((valid_jump_wait_time > 0) || down == 1)
+	//{
+	//	valid_down_wait_time = WAIT_TIME;
+	//}
+
+	if (valid_down_wait_time > 0)
+	{
+		CANCLE_DOWN = 1;
+
+		valid_down_wait_time--;
+	}
+	else
+	{
+		CANCLE_DOWN = 0;
+	}
+
+
+	last_left_zupt = left_zupt;
+	last_right_zupt = right_zupt;
+
+	last_jump = jump;
+	last_down = down;
+}

footRing_sdk_1.0/src/Interaction.cpp

@@ -0,0 +1,238 @@
+#pragma once
+
+#include "Interaction.h"
+#include "pub.h"
+
+Interaction::Interaction()
+{
+	 left_Triggering_time = 0;
+	 right_Triggering_time = 0;
+
+	 last_left_zupt = 1;
+	 last_right_zupt = 1;
+
+	 memset(left_acc_x_buff, 0, 10 * sizeof(float));
+	 memset(right_acc_x_buff, 0, 10 * sizeof(float));
+
+	 memset(left_pitch_buff, 0, 10 * sizeof(float));
+	 memset(right_pitch_buff, 0, 10 * sizeof(float));
+
+	 left_station_count = 0;
+	 right_station_count = 0;
+
+	 left_angle_has_reset = 0;
+	 right_angle_has_reset = 0;
+
+	 cmd = -1;
+}
+
+int Interaction::foot_on_floor(int _time,  int& triggerring_time, int zupt, int &last_zupt, int rssi, float pitch, int left_or_right)
+{
+	if (pitch > 0.3f)
+	{
+		return -1;
+	}
+
+	int result = -1;
+
+	if (last_zupt == 0 && zupt == 1)
+	{
+		if (_time - triggerring_time > 25)
+		{
+			if (left_or_right == LEFT_FOOT)
+			{
+				result = BACK_LEFT;
+			}
+			else
+			{
+				std::cout << "_time - triggerring_time  : " << _time - triggerring_time << endl;
+				result = BACK_RIGHT;
+			}
+		}
+	}
+
+	if (zupt)
+	{
+		triggerring_time = _time;
+	}
+
+	last_zupt = zupt;
+
+	return result;
+}
+
+
+int acc_is_valid(float* acc_window, int length)
+{
+	//要求窗口窗口长度大于10
+	//先判断最后五个元素是平稳的，阈值设置为0.06
+	float station_max = acc_window[length - 1];
+	float station_min = acc_window[length - 1];
+
+	float window_max = acc_window[length - 1];
+	float window_min = acc_window[length - 1];
+
+	int window_max_index = length - 1;
+	int window_min_index = length - 1;
+
+	for (int i = length - 1; i >= 0; i--)
+	{
+		if (acc_window[i] > window_max)
+		{
+			window_max = acc_window[i];
+			window_max_index = i;
+		}
+
+		if (acc_window[i] < window_min)
+		{
+			window_min = acc_window[i];
+			window_min_index = i;
+		}
+	}
+
+
+	for (int i = length - 1; i >= length - 3; i--)
+	{
+		if (acc_window[i] > station_max)
+		{
+			station_max = acc_window[i];
+		}
+
+		if (acc_window[i] < station_min)
+		{
+			station_min = acc_window[i];
+		}
+	}
+
+	if (window_max - window_min < 0.1f)
+	{
+		return 1;
+	}
+
+	return 0;
+}
+int Interaction::enter_key_listen( float* acc, float* acc_x_buff,int &station_count,float *pitch_buff,float pitch, int& angle_has_reset)
+{
+	memcpy(acc_x_buff, acc_x_buff + 1, 9 * sizeof(float));
+
+	acc_x_buff[9] = sqrt(acc[0] * acc[0]  + acc[1] * acc[1] + acc[2] * acc[2]);
+
+	memcpy(pitch_buff, pitch_buff + 1, 9 * sizeof(float));
+
+	pitch_buff[9] = pitch;
+
+	//先判断俯仰角是否正常着地，这样才能判断确认键
+	if (pitch < 0.3f)
+	{
+		angle_has_reset = 1;
+	}
+
+	if (acc_is_valid(acc_x_buff, 10) && *max_element(pitch_buff, pitch_buff + 10) - *min_element(pitch_buff, pitch_buff + 10) < 0.05)
+	{
+		station_count++;
+	}
+	else
+	{
+		station_count = 0;
+	}
+
+	//x轴速度剧烈抖动，而且俯仰角大于45度，则证明是踮脚
+	if (station_count > 10 && pitch > 0.3f  && angle_has_reset == 1)
+	{
+		angle_has_reset = 0;
+
+		return 1;
+	}
+
+	return -1;
+}
+
+void Interaction::setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt)
+{
+	if (zupt != 1)
+	{
+		for (int i = 0; i < 3; i++)
+		{
+			if (max_acc[i] < acc[i])
+			{
+				max_acc[i] = acc[i];
+			}
+
+			if (min_acc[i] > acc[i])
+			{
+				min_acc[i] = acc[i];
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < 3; i++)
+		{
+			max_acc[i] = acc[i];
+			min_acc[i] = acc[i];
+		}
+	}
+
+}
+
+bool Interaction::unzuptValid(int* max_acc, int* min_acc)
+{
+	if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)
+	{
+		return true;
+	}
+
+	return false;
+}
+
+void Interaction::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+	int interative_cmd = -1;
+
+	float left_acc_f[3];
+	float right_acc_f[3];
+
+	for (int i = 0; i < 3; i++)
+	{
+		left_acc_f[i] = left_acc[i] / 2048.f;
+		right_acc_f[i] = right_acc[i] / 2048.f;
+	}
+
+	if (foot_on_floor(time_stamp, left_Triggering_time, left_zupt, last_left_zupt, rssi, left_att[1] * 0.0001f, LEFT_FOOT) != -1
+		&& unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
+	{
+		std::cout << "left interation" << endl;
+		interative_cmd = BACK_LEFT;
+	}
+
+	if (foot_on_floor(time_stamp, right_Triggering_time, right_zupt, last_right_zupt, rssi, right_att[1] * 0.0001f, RIGHT_FOOT) != -1
+		&& unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
+	{
+		std::cout << "right interation" << endl;
+		interative_cmd = BACK_RIGHT;
+	}
+
+	if (enter_key_listen(left_acc_f, left_acc_x_buff, left_station_count, left_pitch_buff, left_att[1] * 0.0001f, left_angle_has_reset) != -1)
+	{
+		std::cout << "cancel key" << endl;
+		interative_cmd = CANCLE_KEY;
+	}
+
+	if (enter_key_listen(right_acc_f, right_acc_x_buff, right_station_count, right_pitch_buff, right_att[1] * 0.0001f, right_angle_has_reset) != -1)
+	{
+		std::cout << "OK key" << endl;
+		interative_cmd = ENTER_KEY; 
+	}
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+
+	cmd = interative_cmd;
+}
+
+int Interaction::get_interative_cmd()
+{
+	return cmd;
+}

footRing_sdk_1.0/src/JumpHouse.cpp

@@ -0,0 +1,186 @@
+#include "JumpHouse.h"
+
+JumpHouse::JumpHouse()
+{
+	left_init = 0;
+	right_init = 0;
+
+	left_heading = 0;
+	right_heading = 0;
+
+	for (int i = 1; i < 3; i++)
+	{
+		for (int k = 0; k < 3; k++)
+		{
+			left_acc_buff[i ][k] = 0;
+			right_acc_buff[i ][k] = 0;
+		}
+	}
+
+
+}
+
+bool JumpHouse::unzuptValid(int* max_acc, int* min_acc)
+{
+	if ((max_acc[0] - min_acc[0] > 2048 || max_acc[1] - min_acc[1] > 2048 || max_acc[2] - min_acc[2] > 2048) && max_acc[0] > 3000)
+	{
+		std::cout << "JumpHouse::unzuptValid" << endl;
+		return true;
+	}
+
+	return false;
+}
+
+
+void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	//翘脚视为不在地上
+	// 	   
+	if (left_zupt == 1 && left_att[1] * 0.0001f > 0.4f)
+	{
+		std::cout << "if (left_zupt == 1 && left_att[1] * 0.0001f > 0.2f) " << left_att[1] * 0.0001f <<  endl;
+		left_zupt = 0;
+	}
+	if (right_zupt == 1 && right_att[1] * 0.0001f > 0.4f)
+	{
+		std::cout << "if (right_zupt == 1 && right_att[1] * 0.0001f > 0.2f) " << right_att[1] * 0.0001f << endl;
+		right_zupt = 0;
+	}
+
+	//缓存一步内的空中数据
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+
+	//根据新来的数据，计算的全局位置
+	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 0, RIGHT_FOOT);
+
+	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
+
+	//通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳，这个时间戳是起延时判断的效果，延时是为了等合适的RSSI
+	if (left_step_data_vector.size() > 4 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
+	{
+		//if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) && press_valid)
+		if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
+		{
+			left_cmd_wait_time = time_stamp;
+
+			left_init = 1;
+
+		}
+
+	}
+
+	if (right_step_data_vector.size() > 4 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
+	{
+		//if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) && press_valid)
+		if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
+		{
+
+			right_cmd_wait_time = time_stamp;
+
+			right_init = 1;
+
+		}
+
+	}
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+	
+
+	//重置命令
+	memset(result, -1, 4 * sizeof(int));
+
+	//判断双脚触地的时候, rssi < 23 必须归位，不然也没办法处理
+	//看起来rssi的延迟有够烂的
+
+	if (left_init == 1 && time_stamp - left_cmd_wait_time >= 9 && time_stamp - left_cmd_wait_time < 30)
+	{
+		if (right_init == 1)
+		{
+			if (rssi < rssi_threshold)
+			{
+				result[0] = MOTION_BOTH_ON_FLOOR_CLOSE;
+
+				std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << rssi << std::endl;
+			}
+			else
+			{
+				result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
+				std::cout << " MOTION_BOTH_ON_FLOOR_OPEN "<< rssi << std::endl;
+			}
+
+		}
+		else
+		{
+			result[0] = MOTION_LEFT_ON_FLOOR;
+			std::cout << " MOTION_LEFT_ON_FLOOR "  <<time_stamp << std::endl;
+		
+		}
+
+		right_init = 0;
+		right_cmd_wait_time = 0;
+		left_init = 0;
+		left_cmd_wait_time = 0;
+
+
+	}
+	else if (right_init == 1 && time_stamp - right_cmd_wait_time >= 9 && time_stamp - right_cmd_wait_time < 30)
+	{
+		if (left_init == 1)
+		{
+			if (rssi < rssi_threshold)
+			{
+				result[0] = MOTION_BOTH_ON_FLOOR_CLOSE;
+
+				std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE "  << " "<< rssi << std::endl;
+			}
+			else
+			{
+				result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
+				std::cout << " MOTION_BOTH_ON_FLOOR_OPEN "  << " " << rssi << std::endl;
+			}
+
+			left_init = 0;
+
+			left_cmd_wait_time = 0;
+		}
+		else
+		{
+			result[0] = MOTION_RIGHT_ON_FLOOR;
+			std::cout << " MOTION_RIGHT_ON_FLOOR "  << time_stamp <<  std::endl;
+			
+		}
+		right_init = 0;
+		right_cmd_wait_time = 0;
+		left_init = 0;
+		left_cmd_wait_time = 0;
+
+	}
+
+	if (right_init == 1 && time_stamp - right_cmd_wait_time >= 30)
+	{
+		right_init = 0;
+		right_cmd_wait_time = 0;
+	}
+
+	if (left_init == 1 && time_stamp - left_cmd_wait_time >= 30)
+	{
+		left_init = 0;
+		left_cmd_wait_time = 0;
+	}
+
+
+	if (result[0] != -1)
+	{
+		addMotionCount(JUMP_COUNT);
+	}
+
+}

footRing_sdk_1.0/src/Kongfu.cpp

@@ -0,0 +1,238 @@
+#include "Kongfu.h"
+
+//功夫小羊游戏
+
+Kongfu::Kongfu()
+{
+	last_left_zupt = 1;
+	last_right_zupt = 1;
+
+	last_jump = 0;
+	last_down = 0;
+	
+	CANCLE_DOWN = 0;
+	CANCLE_JUMP = 0;
+
+	valid_down_wait_time = 0;
+	valid_jump_wait_time = 0;
+
+	CANCLE_FLOOR_CMD_LEFT = 0;
+	CANCLE_FLOOR_CMD_RIGHT = 0;
+
+	cmd_jump_wait_time = CMD_JUMP_WAIT_TIME;
+
+}
+
+
+int getKickByAcc(int zupt, int& kick, int& last_kick, int& kick_wait, float acc_x, vector<float>& acc_x_buff, deque<shoes_data_cell> &shoes_data_vector)
+{
+
+	kick = 0;
+
+	acc_x_buff.push_back(acc_x);
+
+	if (zupt)
+	{
+		acc_x_buff.clear();
+	}
+	//利用在空中判断一次
+	if (shoes_data_vector.size() >  1)
+	{
+		float start_x = shoes_data_vector.front().pos_x;
+		float start_y = shoes_data_vector.front().pos_y;
+
+		float end_x = shoes_data_vector.back().pos_x;
+		float end_y = shoes_data_vector.back().pos_y;
+
+		float dx = end_x - start_x;
+		float dy = end_y - start_y;
+
+		if (sqrt(dx * dx + dy * dy) > 0.3)
+		{
+			kick = 1;
+
+			shoes_data_vector.pop_front();
+		}
+
+	}
+	//利用x轴加速度来判断是否踢脚, 左右脚应该是翻转过来的
+	if (acc_x_buff.size() > 1)
+	{
+		if (acc_x_buff.back() - acc_x_buff[acc_x_buff.size() - 2] > 0.0f)
+		{
+			acc_x_buff.clear();
+		}
+		else
+		{
+			if (acc_x_buff.front() - acc_x_buff.back() > 3.0f)
+			{
+				kick = 1;
+			}
+		}
+	}
+
+	//过滤持续判断踢的动作命令
+	if (last_kick == 0 && kick == 1)
+	{
+		kick_wait = 4;
+	}
+
+	last_kick = kick;
+
+	if (zupt == 1)
+	{
+		kick_wait = 0;
+	}
+
+	//利用三个数据点的时间长度，避免触地时候踢的动作
+	if ((kick_wait--) == 1)
+	{
+		return 1;
+	}
+
+	return 0;
+}
+
+
+
+void Kongfu::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+
+
+	//filter down jump
+
+	if (last_jump == 0 && jump == 1)
+	{
+		result[2] = MOTION_JUMP;
+
+		addMotionCount(JUMP_COUNT);
+
+		CANCLE_JUMP = 20;
+
+		std::cout << "KONGFU : motion_jump" << std::endl;
+	}
+	else
+	{
+		result[2] = -1;
+	}
+
+	//down = 0;
+
+	//if (extract_motion.online_detect_down(right_front_press, left_front_press, right_back_press, left_back_press,
+	//	right_acc, left_acc))
+	//{
+	//	down = 1;
+	//}
+
+	//设置重置为-1
+	result[0] = -1;
+	result[1] = -1;
+
+	if (getKickByAcc(left_zupt, left_kick, last_left_kick, left_kick_wait, left_acc[2] / 2048.f, left_acc_x_buff, left_shoes_data_vector)
+		&& unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) && CANCLE_JUMP == 0)
+	{
+		//统计踢的动作
+		addMotionCount(KICK_COUNT);
+
+		CANCLE_FLOOR_CMD_LEFT = 1;
+
+		result[0] = MOTION_LEFT;
+	}
+
+	if (getKickByAcc(right_zupt, right_kick, last_right_kick, right_kick_wait, right_acc[2] / 2048.f, right_acc_x_buff, right_shoes_data_vector)
+		&& unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) && CANCLE_JUMP == 0)
+	{
+		//统计踢的动作
+		addMotionCount(KICK_COUNT);
+
+		CANCLE_FLOOR_CMD_RIGHT = 1;
+
+		result[1] = MOTION_RIGHT;
+	}
+
+
+	//获取触地
+	if (last_left_zupt == 0 && left_zupt == 1 && unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) && CANCLE_JUMP == 0)
+	{
+		//统计步数
+		addMotionCount(STEP_COUNT);
+
+		if (CANCLE_FLOOR_CMD_LEFT == 0)
+		{
+			result[0] = MOTION_LEFT;
+		}
+	}
+
+
+	if (last_left_zupt == 1 && left_zupt == 0)
+	{
+		CANCLE_FLOOR_CMD_LEFT = 0;
+	}
+
+
+	if (last_right_zupt == 0 && right_zupt == 1 && unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) &&CANCLE_JUMP == 0)
+	{
+		//统计步数
+		addMotionCount(STEP_COUNT);
+
+		if (CANCLE_FLOOR_CMD_RIGHT == 0)
+		{
+			result[1] = MOTION_RIGHT;
+		}
+	}
+
+
+	if (last_right_zupt == 1 && right_zupt == 0)
+	{
+		CANCLE_FLOOR_CMD_RIGHT = 0;
+	}
+
+	if (result[0] == MOTION_LEFT)
+	{
+		std::cout << "left motion is kick" << endl;
+	}
+
+	if (result[1] == MOTION_RIGHT)
+	{
+		std::cout << "right motion is kick" << endl;
+	}
+
+
+	
+
+	result[3] = -1;
+	//same process with jump
+	if (last_down == 0 && down == 1)
+	{
+		std::cout << "H5GAME:: MOTION_DOWN" << endl;
+
+		result[3] = MOTION_DOWN;
+
+		addMotionCount(DOWN_COUNT);
+
+	}
+
+
+
+	last_left_zupt = left_zupt;
+	last_right_zupt = right_zupt;
+
+	last_jump = jump;
+	last_down = down;
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+
+	if (CANCLE_JUMP > 0)
+	{
+		CANCLE_JUMP--;
+	}
+}

footRing_sdk_1.0/src/MotionCount.cpp

@@ -0,0 +1,31 @@
+
+#include "MotionCount.h"
+
+MotionCount::MotionCount()
+{
+	memset(motionCount_matrix, 0, MOTION_COUNT_SIZE * sizeof(int));
+}
+
+int MotionCount::getMotionCount(int motion_type)
+{
+	std::cout << "int MotionCount::getMotionCount(int motion_type)" << endl;
+	if (motion_type > -1 && motion_type < 20)
+	{
+		return motionCount_matrix[motion_type];
+	}
+
+	return -1;
+}
+
+void MotionCount::addMotionCount(int motion_type)
+{
+	if (motion_type > -1 && motion_type < 20)
+	{
+		motionCount_matrix[motion_type] ++;
+
+		if (motion_type == STEP_COUNT)
+		{
+			std::cout << "STEP_COUNT : "<<motionCount_matrix[motion_type] << endl;
+		}
+	}
+}

footRing_sdk_1.0/src/PublicInterface.cpp

@@ -0,0 +1,187 @@
+#include "PublicInterface.h"
+
+void PublicInterface::getResult(int* dec)
+{
+	memcpy(dec, result, RESULT_LEN * sizeof(int));
+}
+
+void PublicInterface::setData(deque<shoes_data_cell>& shoes_data_vector, int time_stamp, float pos_x, float pos_y, float pos_z, float heading, float pitch, float roll, int zupt, int rssi)
+{
+	//保持所有的空中数据，保持队列头尾是触地
+	if (shoes_data_vector.size() > 0)
+	{
+		if (shoes_data_vector.front().zupt == 0)
+		{
+			shoes_data_vector.clear();
+		}
+		else if (shoes_data_vector.back().zupt == 1 && zupt == 1)
+		{
+			shoes_data_vector.clear();
+		}
+		else if (shoes_data_vector.back().zupt == 1 && shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 1 && zupt == 0)
+		{
+			shoes_data_cell temp = shoes_data_vector.back();
+			shoes_data_vector.clear();
+			shoes_data_vector.push_back(temp);
+		}
+	}
+	shoes_data_vector.push_back({ time_stamp,  pos_x,  pos_y,  pos_z, heading, pitch, roll, zupt, rssi });
+}
+
+void PublicInterface::calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector, float* global_pos,
+	float main_heading, float extra_heading,int LEFT_OR_RIGHT)
+{
+	int length = shoes_data_vector.size();
+
+	if (length == 0)
+	{
+		step_data_vector.clear();
+
+		return;
+	}
+
+	shoes_data_cell shoes_data_temp = shoes_data_vector.back();
+
+	if (length == 1)
+	{
+		step_data_vector.clear();
+
+		shoes_data_temp.pos_x = 0;
+		shoes_data_temp.pos_y = 0;
+		shoes_data_temp.pos_z = 0;
+
+		step_data_vector.push_back(shoes_data_temp);
+
+		return;
+	}
+
+	//规避只有一个触地信号的情况
+	if (length == 2)
+	{
+		step_data_vector.clear();
+
+		shoes_data_cell shoes_data_temp_front = shoes_data_vector.front();
+
+		shoes_data_temp_front.pos_x = 0;
+		shoes_data_temp_front.pos_y = 0;
+		shoes_data_temp_front.pos_z = 0;
+
+		step_data_vector.push_back(shoes_data_temp_front);
+	}
+
+	float pos_offset[3];
+
+	pos_offset[0] = shoes_data_vector.back().pos_x - shoes_data_vector.front().pos_x;
+	pos_offset[1] = shoes_data_vector.back().pos_y - shoes_data_vector.front().pos_y;
+	pos_offset[2] = shoes_data_vector.back().pos_z - shoes_data_vector.front().pos_z;
+
+	float posTemp[3];
+
+	float heading = main_heading;
+
+
+	posTemp[0] = cos(heading) * pos_offset[0] + sin(heading) * pos_offset[1];
+	posTemp[1] = -sin(heading) * pos_offset[0] + cos(heading) * pos_offset[1];
+
+
+	pos_offset[0] = cos(extra_heading) * posTemp[0] + sin(extra_heading) * posTemp[1];
+	pos_offset[1] = -sin(extra_heading) * posTemp[0] + cos(extra_heading) * posTemp[1];
+
+
+	//描述全局的跳舞位置
+
+	global_pos[0] -= step_data_vector.back().pos_x; global_pos[1] -= step_data_vector.back().pos_y; global_pos[2] -= step_data_vector.back().pos_z;
+
+	shoes_data_temp.pos_x = step_data_vector.front().pos_x + pos_offset[0];
+	shoes_data_temp.pos_y = step_data_vector.front().pos_y + pos_offset[1];
+	shoes_data_temp.pos_z = step_data_vector.front().pos_z + pos_offset[2];
+
+	step_data_vector.push_back(shoes_data_temp);
+
+	global_pos[0] += step_data_vector.back().pos_x; global_pos[1] += step_data_vector.back().pos_y; global_pos[2] += step_data_vector.back().pos_z;
+
+}
+
+
+float PublicInterface::getGamePos(int left_or_right, int index)
+{
+	if (index < 0 || index > 2)
+		return -1;
+
+	if (left_or_right == LEFT_FOOT)
+	{
+		return left_global_pos[index];
+	}
+	else
+	{
+		return right_global_pos[index];
+	}
+}
+
+void PublicInterface::setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt)
+{
+	if (zupt != 1)
+	{
+		for (int i = 0; i < 3; i++)
+		{
+			if (max_acc[i] < acc[i])
+			{
+				max_acc[i] = acc[i];
+			}
+
+			if (min_acc[i] > acc[i])
+			{
+				min_acc[i] = acc[i];
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < 3; i++)
+		{
+			max_acc[i] = acc[i];
+			min_acc[i] = acc[i];
+		}
+	}
+
+}
+
+bool PublicInterface::unzuptValid(int* max_acc, int* min_acc)
+{
+	if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)
+	{
+		return true;
+	}
+
+	return false;
+}
+
+void  PublicInterface::resetZerosPointByRssi(int zupt, int zupt_count_thresh, int& zupt_count, int& has_init, float heading, float& rotate_heading, int rssi, int rssi_thresh, float* global_pos)
+{
+	if (zupt && rssi < rssi_thresh)
+	{
+		zupt_count++;
+	}
+	else
+	{
+		zupt_count = 0;
+	}
+
+	//需要站立大于1秒
+	if (zupt_count > zupt_count_thresh)
+	{
+		has_init = 1;
+
+		rotate_heading = heading;
+
+		memset(global_pos, 0, 3 * sizeof(float));
+
+		zupt_count = 0;
+
+	}
+
+	if (has_init == 0)
+	{
+		memset(global_pos, 0, 3 * sizeof(float));
+	}
+}

footRing_sdk_1.0/src/PublicSDKMotion.cpp

@@ -0,0 +1,8 @@
+#include "PublicSDKMotion.h"
+
+void PublicSDKMotion::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+	//ÔËË㹫ÓýӿÚ
+}

footRing_sdk_1.0/src/RunGame.cpp

@@ -0,0 +1,291 @@
+#include "RunGame.h"
+
+RunGame::RunGame()
+{
+	memset(left_global_pos, 0, 3 * sizeof(float));
+
+	memset(right_global_pos, 0, 3 * sizeof(float));
+
+	down_wait_after_jump = -1;
+
+	last_left_zupt = 1;
+	last_right_zupt = 1;
+
+	last_canva_down = 0;
+}
+
+
+
+
+void RunGame::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	// 额外处理蹲
+	/*down = 0;
+	int online_down = extract_motion.online_detect_down(right_front_press, left_front_press, right_back_press, left_back_press,
+		right_acc, left_acc);
+	if (online_down)
+	{
+		down = online_down;
+	}
+
+	int online_jump = extract_motion.online_detect_jump(right_front_press, left_front_press, right_back_press, left_back_press,
+		right_acc, left_acc);
+	if (online_jump)
+	{
+		jump = online_jump;
+	}*/
+
+	//存放一步的空中数据到shoes_data_vector 队列中， 头必须是触地时刻信息
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+
+	//计算全局位置，选择当前的旋转量为这一步即将离地时候的方向，额外的旋转量为10度
+	//hoes_data_vector存的是鞋子上传的数据，它的方向是鞋子烧录时候的方向，并不是玩游戏时候的方向，需要旋转至玩游戏的方向
+	//step_data_vector 的位置已经选择到游戏的方向了，位置是相对位移，即相对于left_step_data_vector[0]的位置偏移
+	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_shoes_data_vector[0].heading, 10.0f / 180 * PI, RIGHT_FOOT);
+	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_shoes_data_vector[0].heading, -10.0f / 180 * PI, LEFT_FOOT);
+
+	//这里为了脚本容易观测，设置全局位置为0
+	if (left_step_data_vector.front().zupt == 0)
+	{
+		memset(left_global_pos, 0, 3 * sizeof(float));
+	}
+
+	if (right_step_data_vector.front().zupt == 0)
+	{
+		memset(right_global_pos, 0, 3 * sizeof(float));
+	}
+
+
+	//判断命令触发
+	int left_cmd = -1;
+	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1)
+	{
+		std::cout << left_step_data_vector.back().pos_x << ", " << left_step_data_vector.back().pos_y << endl;
+		//跑酷游戏只要有空中的数据（不想其他游戏一样需要落地时候判断就好），就需要进行游戏
+		if (left_step_data_vector.back().zupt || left_step_data_vector.back().pitch < 0.3f)
+		{
+			//垫脚跑不触发命令。如果硬要垫脚跑，那么就等到他触地的时候再判断了
+			float max_val = left_step_data_vector.front().pos_x;
+
+			for (int i = 0; i < left_step_data_vector.size(); i++)
+			{
+				if (left_step_data_vector[i].pos_x > max_val)
+				{
+					max_val = left_step_data_vector[i].pos_x;
+				}
+			}
+			//有效距离为0.23m
+			if (max_val - left_step_data_vector.back().pos_x > 0.23f)
+			{
+				left_cmd = MOTION_LEFT;
+				std::cout << "motion: left_side" << std::endl;
+
+				left_step_data_vector.pop_front();
+			}
+		}
+	}
+
+	int right_cmd = -1;
+	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1)
+	{
+
+		if (right_step_data_vector.back().zupt || right_step_data_vector.back().pitch < 0.3f)
+		{
+
+			float min_val = right_step_data_vector.front().pos_x;
+
+			for (int i = 0; i < right_step_data_vector.size(); i++)
+			{
+				if (right_step_data_vector[i].pos_x < min_val)
+				{
+					min_val = right_step_data_vector[i].pos_x;
+				}
+			}
+
+			if (right_step_data_vector.back().pos_x - min_val > 0.23f)
+			{
+				right_cmd = MOTION_RIGHT;
+
+				std::cout << "motion: right_side" << std::endl;
+
+				right_step_data_vector.pop_front();
+			}
+		}
+	}
+
+	int jump_cmd = -1;
+
+	jump_cmd = getResultJump(jump);
+
+	int down_cmd = -1;
+
+	result[3] = -1;
+
+	down_cmd = getResultDown(down);
+
+
+	left_acc_deque.push_back(left_acc[2]);
+	right_acc_deque.push_back(right_acc[2]);
+
+	if (left_acc_deque.size() > 5)
+	{
+		left_acc_deque.pop_front();
+		right_acc_deque.pop_front();
+	}
+
+	int acc_valid = 0;
+	if (left_acc_deque.size() == 5)
+	{
+		int left_max_val = *(std::max_element(left_acc_deque.begin(), left_acc_deque.end()));
+		int left_min_val = *(std::min_element(left_acc_deque.begin(), left_acc_deque.end()));
+
+		int right_max_val = *(std::max_element(right_acc_deque.begin(), right_acc_deque.end()));
+		int right_min_val = *(std::min_element(right_acc_deque.begin(), right_acc_deque.end()));
+
+
+		if (left_max_val - left_min_val < 512 && right_max_val - right_min_val < 512)
+		{
+			acc_valid = 1;
+		}
+	}
+
+	if (down_cmd == 1)
+	{
+		wait_down = 10;
+	}
+	if (down_cmd == MOTION_DOWN)
+	{
+		addMotionCount(DOWN_COUNT);
+
+		down_wait_after_jump = 20;
+
+		result[3] = MOTION_DOWN;
+
+		wait_down = 0;
+	}
+	//std::cout << "test_down" << endl;
+	//if (wait_down == 1  && acc_valid == 1)
+	//if (wait_down == 10)
+	if(down)
+	{
+
+		addMotionCount(DOWN_COUNT);
+
+		down_wait_after_jump = 20;
+
+		result[3] = MOTION_DOWN;
+
+		wait_down = 0;
+	}
+
+
+	if (wait_down > 0)
+	{
+		wait_down--;
+	}
+
+	result[0] = left_cmd;
+	result[1] = right_cmd;
+	result[2] = jump_cmd;
+	//result[3] = down_cmd;
+
+	//续一个空中不能触发蹲的命令
+	/*if (left_zupt == 0 && right_zupt == 0 && down_wait_after_jump > 0)
+	{
+		down_wait_after_jump = 10;
+	}*/
+
+	//计步
+	if (last_left_zupt == 0 && left_zupt == 1 && unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
+	{
+		addMotionCount(STEP_COUNT);
+	}
+
+	if (last_right_zupt == 0 && right_zupt == 1 && unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
+	{
+		addMotionCount(STEP_COUNT);
+	}
+
+	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
+	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
+
+	last_left_zupt = left_zupt;
+	last_right_zupt = right_zupt;
+
+}
+
+int RunGame::getResultDown(int down)
+{
+	int isDown = -1;
+
+	int canva_down = ((down & 0x04) > 0 ? 1 : 0);
+	
+	if (last_down == 0 && down > 0)
+	{
+		isDown = 1;
+
+		addMotionCount(DOWN_COUNT);
+
+		std::cout << "motion : down" << std::endl;
+
+		//down_count++;
+	}
+	/*
+	* down_wait_after_jump:避免前脚蹲后，接着后脚压下来的情况，当down_wait_after_jump == -1时候，判断蹲生效
+	* 当一直处于蹲的时候，额外加拖延时间，在这个时间不允许触发蹲。
+	*/
+
+	if (down == 0 && down_wait_after_jump >=0)
+	{
+		down_wait_after_jump--;
+	}
+
+	last_down = down;
+
+	last_canva_down = canva_down;
+
+
+	return isDown;
+
+}
+
+int RunGame::getResultJump(int jump)
+{
+	int isJump = 0;
+
+	if (last_jump == 0 && jump == 1)
+	{
+		isJump = MOTION_JUMP;
+
+		addMotionCount(JUMP_COUNT);
+
+		std::cout << "motion : jump" << std::endl;
+
+		jump_count++;
+	}
+	/*
+	* 跳之后不能立即接着蹲，用down_wait_after_jump来避免
+	*/
+	if (jump)
+	{
+		down_wait_after_jump = 20;
+	}
+
+	last_jump = jump;
+
+	return isJump;
+}
+
+
+
+void RunGame::setStepHeading(float& cur_heading, float heading)
+{
+	cur_heading = heading;
+}

footRing_sdk_1.0/src/SportFunction.cpp

@@ -0,0 +1,342 @@
+#include "SportFunction.h"
+
+SportFunction::SportFunction()
+{
+	memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));
+	memset(&right_jump_data, 0, sizeof(single_jump_data_struct));
+	memset(&left_jump_data, 0, sizeof(single_jump_data_struct));
+
+
+	left_step = 0;
+	right_step = 0;
+
+	left_step_length = 0;
+	right_step_length = 0;
+
+	jump_count = 0;
+	last_jump_count = 0;
+
+	jump_high = 0;
+	jump_length = 0;
+
+
+}
+
+
+void SportFunction::setData(deque<sport_data_struct>& shoes_data_vector, float pos_x, float pos_y, float pos_z,
+	float acc_x, float acc_y, float acc_z, int front_press, int back_press,
+	int jump, int zupt, int times_stamp)
+{
+	if (shoes_data_vector.size() > 0)
+	{
+		if (shoes_data_vector.front().zupt == 0)
+		{
+			shoes_data_vector.clear();
+		}
+		else if (shoes_data_vector.back().zupt == 1 && zupt == 1)
+		{
+			shoes_data_vector.clear();
+		}
+		else if (shoes_data_vector.front().zupt == 1 && shoes_data_vector.back().zupt == 1 && zupt == 0)
+		{
+			sport_data_struct sport_temp = shoes_data_vector.back();
+
+			sport_temp.max_z = acc_z; sport_temp.min_z = acc_z;
+			sport_temp.not_on_floor = 0;
+			sport_temp.max_pos_z = 0.0f;
+
+			shoes_data_vector.clear();
+			shoes_data_vector.push_back(sport_temp);
+		}
+	}
+	//缓存鞋子的数据
+	shoes_data_vector.push_back({ pos_x,  pos_y,  pos_z, acc_x, acc_y, acc_z,
+		front_press, back_press, jump, zupt, acc_z,acc_z, 0.0f, 0,times_stamp });
+}
+
+void SportFunction::checkShoesData(deque<sport_data_struct>& shoes_data_vector)
+{
+	int len = shoes_data_vector.size();
+
+	//假定真正的触地一定伴随着压力的上升。
+
+	if (shoes_data_vector.back().zupt == 1 && len > 1)
+	{
+		int circle_len = 10 > len - 1 ? len - 1 : 10;
+
+		int front_min_press = shoes_data_vector.back().front_press;
+		int back_min_press = shoes_data_vector.back().back_press;
+
+		for (auto it = shoes_data_vector.rbegin(); it < shoes_data_vector.rbegin() + circle_len; it++)
+		{
+			if (it->front_press < front_min_press)
+			{
+				front_min_press = it->front_press;
+			}
+
+			if (it->back_press > back_min_press)
+			{
+				back_min_press = it->back_press;
+			}
+		}
+
+		if (!(shoes_data_vector.back().front_press - front_min_press > 500 || shoes_data_vector.front().front_press - back_min_press > 500))
+		{
+			shoes_data_vector.back().zupt = 0;
+		}
+	}
+
+	//缓存最大Z轴值， 最小值
+	if (len > 1)
+	{
+		shoes_data_vector[len - 1].max_z = shoes_data_vector[len - 1].acc_z > shoes_data_vector[len - 2].max_z ? shoes_data_vector[len - 1].acc_z : shoes_data_vector[len - 2].max_z;
+		shoes_data_vector[len - 1].min_z = shoes_data_vector[len - 1].acc_z < shoes_data_vector[len - 2].min_z ? shoes_data_vector[len - 1].acc_z : shoes_data_vector[len - 2].min_z;
+
+		//当发现有剧烈的抖动，视为上升趋势
+		if (shoes_data_vector[len - 1].max_z - shoes_data_vector[len - 1].min_z > 1.0f || shoes_data_vector[len - 2].not_on_floor == 1)
+		{
+			shoes_data_vector[len - 1].not_on_floor = 1;
+		}
+
+		shoes_data_vector.back().max_pos_z = max(shoes_data_vector[len - 2].max_pos_z, shoes_data_vector.back().pos_z - shoes_data_vector.front().pos_z);
+	}
+}
+
+int SportFunction::checkRopeSkip(deque<sport_data_struct>& left_shoes_data_vector, deque<sport_data_struct>& right_shoes_data_vector)
+{
+	if (left_shoes_data_vector.back().zupt == 1 && left_shoes_data_vector.front().zupt == 1 && left_shoes_data_vector.size() > 2 &&
+		left_shoes_data_vector.back().not_on_floor == 1 && left_shoes_data_vector.back().time_stamp - left_shoes_data_vector.front().time_stamp > 5)
+	{
+		if (right_shoes_data_vector.back().not_on_floor == 1)
+		{
+			return 1;
+		}
+	}
+	else if (right_shoes_data_vector.back().zupt == 1 && right_shoes_data_vector.front().zupt == 1 && right_shoes_data_vector.size() > 2 && right_shoes_data_vector.back().not_on_floor == 1
+		&& right_shoes_data_vector.back().time_stamp - right_shoes_data_vector.front().time_stamp > 5)
+	{
+		if (left_shoes_data_vector.back().not_on_floor == 1)
+		{
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+void SportFunction::getJumpHighLength(dual_jump_data_struct& dual_jump_data, deque<sport_data_struct>& shoes_data_vector, int left_or_right)
+{
+	if (left_or_right == LEFT_FOOT)
+	{
+		if (dual_jump_data.live_time > 0 && dual_jump_data.left_jump_data.jump_tag == 0)
+		{
+
+			if (shoes_data_vector.back().zupt == 1 &&shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 2)
+			{
+				dual_jump_data.left_jump_data.jump_tag = 1;
+
+				dual_jump_data.left_jump_data.jump_length = getJumpLength(shoes_data_vector) * 1000.f;
+
+				dual_jump_data.left_jump_data.jump_high = shoes_data_vector.back().max_pos_z * 1000.f;
+
+				dual_jump_data.left_jump_data.start_time_stamp = shoes_data_vector.front().time_stamp;
+
+			}
+		}
+	}
+	else
+	{
+		if (dual_jump_data.live_time > 0 && dual_jump_data.right_jump_data.jump_tag == 0)
+		{
+			if (shoes_data_vector.back().zupt == 1 && shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 2)
+			{
+				dual_jump_data.right_jump_data.jump_tag = 1;
+
+				dual_jump_data.right_jump_data.jump_length = getJumpLength(shoes_data_vector) * 1000.f;
+
+				dual_jump_data.right_jump_data.jump_high = shoes_data_vector.back().max_pos_z * 1000.f;
+
+				dual_jump_data.right_jump_data.start_time_stamp = shoes_data_vector.front().time_stamp;
+			}
+		}
+	}
+}
+
+void SportFunction::processJumpHighLength(dual_jump_data_struct& dual_jump_data, int rssi)
+{
+
+	//当发现dual_jump_data还处于有效时间，而且左右脚都已经触地了，那么意味着是正常的跳，输出双脚的最小值即可
+	if (dual_jump_data.live_time > 0 && dual_jump_data.left_jump_data.jump_tag && dual_jump_data.right_jump_data.jump_tag)
+	{
+		if (abs(dual_jump_data.left_jump_data.jump_length - dual_jump_data.right_jump_data.jump_length) <= 100 || rssi < 20)
+		{
+			jump_length = max(dual_jump_data.left_jump_data.jump_length, dual_jump_data.right_jump_data.jump_length);
+		}
+		else
+		{
+			jump_length = min(dual_jump_data.left_jump_data.jump_length, dual_jump_data.right_jump_data.jump_length);
+		}
+
+		jump_high = min(dual_jump_data.left_jump_data.jump_high, dual_jump_data.right_jump_data.jump_high);
+
+		result[3] = jump_high;
+
+		result[4] = jump_length;
+
+		std::cout << std::endl;
+
+		std::cout << "left_jump_length : " << dual_jump_data.left_jump_data.jump_length
+			<< " ,right_jump_length : " << dual_jump_data.right_jump_data.jump_length << endl;
+
+		std::cout << "jump_length : " << jump_length << endl;
+
+		std::cout << "jump_high : " << jump_high << endl;
+
+
+		memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));
+
+
+
+	}
+	else if (dual_jump_data.live_time == 0 && dual_jump_data.left_jump_data.jump_tag)
+	{
+		jump_length = dual_jump_data.left_jump_data.jump_length;
+		jump_high = dual_jump_data.left_jump_data.jump_high;
+
+		result[3] = jump_high;
+
+		result[4] = jump_length;
+
+
+		std::cout << "jump_length : " << jump_length << endl;
+
+		std::cout << "jump_high : " << jump_high << endl;
+
+		memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));
+
+	}
+	else if (dual_jump_data.live_time == 0 && dual_jump_data.right_jump_data.jump_tag)
+	{
+
+
+		jump_length = dual_jump_data.right_jump_data.jump_length;
+		jump_high = dual_jump_data.right_jump_data.jump_high;
+
+		result[3] = jump_high;
+
+		result[4] = jump_length;
+
+		std::cout << "jump_length : " << jump_length << endl;
+
+		std::cout << "jump_high : " << jump_high << endl;
+
+		memset(&dual_jump_data, 0, sizeof(dual_jump_data_struct));
+	}
+
+
+	if (dual_jump_data.live_time > 0)
+	{
+		dual_jump_data.live_time--;
+	}
+
+}
+
+
+void SportFunction::processStepData(deque<sport_data_struct>& shoes_data_vector, int left_or_right)
+{
+	if (shoes_data_vector.back().zupt == 1 && shoes_data_vector.front().zupt == 1 && shoes_data_vector.size() > 2)
+	{
+		if (shoes_data_vector.back().not_on_floor == 1 && shoes_data_vector.back().time_stamp - shoes_data_vector.front().time_stamp > 10)
+		{
+			if (left_or_right == LEFT_FOOT)
+			{
+				left_step_length = getJumpLength(shoes_data_vector) * 1000.f;
+				left_step++;
+
+				result[0] = left_step_length;
+
+				result[2] = left_step + right_step;
+
+				std::cout << "left_step : " << left_step << ", left_step_length : " << left_step_length * 0.001f << endl;
+			}
+			else
+			{
+				right_step_length = getJumpLength(shoes_data_vector) * 1000.f;
+				right_step++;
+
+				result[1] = right_step_length;
+
+				result[2] = left_step + right_step;
+
+				std::cout << "right_step : " << right_step << ", right_step_length : " << right_step_length * 0.001f << endl;
+			}
+		}
+	}
+}
+
+float SportFunction::getJumpLength(deque<sport_data_struct>& shoes_data_vector)
+{
+	if (shoes_data_vector.back().zupt == 0)
+	{
+		return 0.0f;
+	}
+	float dx = shoes_data_vector.back().pos_x - shoes_data_vector.front().pos_x;
+	float dy = shoes_data_vector.back().pos_y - shoes_data_vector.front().pos_y;
+
+	return sqrt(dx * dx + dy * dy);
+}
+
+
+void SportFunction::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	//将结果全部置为-1，供上层的判断
+
+	memset(result, -1, 6 * sizeof(int));
+	/*
+	* 缓存鞋子的数据到shoes_data_vector， 方便数据分析和测试
+	*/
+	setData(right_shoes_data_vector, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_acc[0] / 2048.f, right_acc[1] / 2048.f, right_acc[2] / 2048.f,
+		right_front_press, right_back_press, jump, right_zupt, time_stamp);
+
+	setData(left_shoes_data_vector, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_acc[0] / 2048.f, left_acc[1] / 2048.f, left_acc[2] / 2048.f,
+		left_front_press, left_back_press, jump, left_zupt, time_stamp);
+
+	//检测shoes_data_vector，避免最后一个元素为假触地元素
+	checkShoesData(right_shoes_data_vector);
+	checkShoesData(left_shoes_data_vector);
+
+	//统计跳的数目
+	if (checkRopeSkip(left_shoes_data_vector, right_shoes_data_vector))
+	{
+		result[5] = ++jump_count;
+
+		std::cout << "rope skip jump count  + 1 : " << jump_count << endl;
+
+	}
+
+	if (left_shoes_data_vector.back().not_on_floor == 1 && right_shoes_data_vector.back().not_on_floor == 1
+		&& (left_shoes_data_vector.back().zupt == 1 || right_shoes_data_vector.back().zupt == 1)
+		&&  (abs(left_shoes_data_vector.front().time_stamp - right_shoes_data_vector.front().time_stamp)) < 30)
+	{
+		dual_jump_data.live_time = 10;
+	}
+
+	//在这里处理跳的高度 和长度
+	getJumpHighLength(dual_jump_data, left_shoes_data_vector, LEFT_FOOT);
+	getJumpHighLength(dual_jump_data, right_shoes_data_vector, RIGHT_FOOT);
+
+	processJumpHighLength(dual_jump_data, rssi);
+
+	//处理每一步的长度以及记步
+	processStepData(left_shoes_data_vector, LEFT_FOOT);
+
+	processStepData(right_shoes_data_vector, RIGHT_FOOT);
+
+
+}
+

footRing_sdk_1.0/src/StepVel.cpp

@@ -0,0 +1,351 @@
+#include "StepVel.h"
+
+StepVel::StepVel()
+{
+	step_vel = 0;
+
+	left_jump_cancel_update_vel = 0;
+	right_jump_cancel_update_vel = 0;
+
+	both_zupt_count = 0;
+}
+
+void StepVel::setData(deque<shoes_data_cell>& shoes_data_vector, int time_stamp, float pos_x, float pos_y, float pos_z, float heading, float pitch, float roll, int zupt, int rssi)
+{
+	if (shoes_data_vector.size() > 0)
+	{
+		if (shoes_data_vector.front().zupt == 0)
+		{
+			shoes_data_vector.clear();
+		}
+		else if (shoes_data_vector.back().zupt == 1 && zupt == 1)
+		{
+			shoes_data_vector.clear();
+		}
+	}
+	shoes_data_vector.push_back({ time_stamp,  pos_x,  pos_y,  pos_z, heading, pitch, roll, zupt, rssi });
+}
+
+void StepVel::calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector, float* global_pos, int LEFT_OR_RIGHT)
+{
+	int length = shoes_data_vector.size();
+
+	if (length == 0)
+	{
+		step_data_vector.clear();
+
+		return;
+	}
+
+	shoes_data_cell shoes_data_temp = shoes_data_vector.back();
+
+	if (length == 1)
+	{
+		step_data_vector.clear();
+
+		shoes_data_temp.pos_x = 0;
+		shoes_data_temp.pos_y = 0;
+		shoes_data_temp.pos_z = 0;
+
+		step_data_vector.push_back(shoes_data_temp);
+
+		return;
+	}
+
+	float pos_offset[3];
+
+	pos_offset[0] = shoes_data_vector.at(length - 1).pos_x - shoes_data_vector.at(length - 2).pos_x;
+	pos_offset[1] = shoes_data_vector.at(length - 1).pos_y - shoes_data_vector.at(length - 2).pos_y;
+	pos_offset[2] = shoes_data_vector.at(length - 1).pos_z - shoes_data_vector.at(length - 2).pos_z;
+
+	float posTemp[3];
+
+	float heading = shoes_data_vector[0].heading;
+
+	posTemp[0] = pos_offset[0];
+	posTemp[1] = pos_offset[1];
+
+	pos_offset[0] = posTemp[0];
+	pos_offset[1] = posTemp[1];
+
+	/*for (int i = 0; i < 3; i++)
+	{
+		global_pos[i] += pos_offset[i];
+	}*/
+
+	shoes_data_temp.pos_x = step_data_vector.back().pos_x + pos_offset[0];
+	shoes_data_temp.pos_y = step_data_vector.back().pos_y + pos_offset[1];
+	shoes_data_temp.pos_z = step_data_vector.back().pos_z + pos_offset[2];
+
+	step_data_vector.push_back(shoes_data_temp);
+}
+
+void StepVel::check_vel_vector(int time_stamp, deque<vel_struct> &vel_vector)
+{
+	if (!vel_vector.empty())
+	{
+
+		//一步内
+		if (time_stamp - vel_vector.back().time_stamp > 100)
+		{
+			vel_vector.clear();
+		}
+
+		int length = vel_vector.size();
+
+		if (length > 1 &&  vel_vector.back().time_stamp - vel_vector[length - 2].time_stamp > 50)
+		{
+			vel_struct  temp = vel_vector.back();
+
+			vel_vector.clear();
+
+			vel_vector.push_back(temp);
+		}
+
+		if (!vel_vector.empty())
+		{
+			while (vel_vector.back().time_stamp - vel_vector.front().time_stamp > 100)
+			{
+				vel_vector.pop_front();
+			}
+		}
+
+		if (vel_vector.empty())
+		{
+			std::cout << "long time no move  and  step vel 0.0f" << endl;
+		}
+	}
+}
+
+float StepVel::cal_mean_vel(int end_time, int start_time, float max_val)
+{
+	if (max_val < 0.02f)
+	{
+		max_val = 0.02f;
+	}
+
+	float mean_vel = max_val / (end_time - start_time) * 100.0f;
+
+	float attenuation_coefficient;
+
+	if (end_time - start_time > 80)
+	{
+		attenuation_coefficient = 0.01f;
+	}
+	else if (end_time - start_time < 25 && end_time - start_time > 15)
+	{
+		attenuation_coefficient = 1.5f;
+	}
+	else if (end_time - start_time <= 15)
+	{
+		attenuation_coefficient = 3.0f;
+	}
+	else
+	{
+		attenuation_coefficient = 1.0f - (end_time - start_time) / 80;
+	}
+	mean_vel *= attenuation_coefficient;
+
+	return mean_vel;
+}
+
+float StepVel::get_vel_mean(int time_stamp, deque<vel_struct>& vel_vector, deque<shoes_data_cell> &step_data_vector)
+{
+	//vel_vector 保存的是 1s间的速度， 必须保证相邻两个数据之间小于50ms, 否则清空数据只留当前的数据
+	//当检测到超过一秒的速度就应该扔掉
+	check_vel_vector(time_stamp, vel_vector);
+
+	if (step_data_vector.size() > 1 && step_data_vector.front().zupt == 1 && step_data_vector.back().zupt == 1)
+	{
+		//利用一步间的最高高度来估算出一个角度
+		if (step_data_vector.back().pos_z < 0.0f)
+		{
+			//鞋子算出来的高度有误差，需要补偿丢失的高度
+			int start_time = step_data_vector.front().time_stamp;
+
+			int end_time = step_data_vector.back().time_stamp;
+
+			float compensate_vel = -step_data_vector.back().pos_z / (end_time - start_time) * 100.0f;
+
+
+			float max_val = 0.0f;
+			for (int i = 0; i < step_data_vector.size(); i++)
+			{
+				step_data_vector[i].pos_z += (compensate_vel) * (step_data_vector[i].time_stamp - start_time) * 0.01f;
+
+				if (max_val < step_data_vector[i].pos_z)
+				{
+					max_val = step_data_vector[i].pos_z;
+				}
+
+			}
+			//算速度的时候，若是高步频则需要添加增益因子
+			//若是低步频则是添加减益因子
+		
+			float mean_vel = cal_mean_vel(end_time, start_time, max_val);
+
+			vel_vector.push_back({ time_stamp, mean_vel });
+
+			check_vel_vector(time_stamp, vel_vector);
+
+		}
+		else
+		{
+			int start_time = step_data_vector.front().time_stamp;
+
+			int end_time = step_data_vector.back().time_stamp;
+
+			float max_val = 0.0f;
+
+			for (int i = 0; i < step_data_vector.size(); i++)
+			{
+
+				if (max_val < step_data_vector[i].pos_z)
+				{
+					max_val = step_data_vector[i].pos_z;
+				}
+
+			}
+
+			float mean_vel = cal_mean_vel(end_time, start_time, max_val);
+
+			vel_vector.push_back({ time_stamp, mean_vel });
+
+			check_vel_vector(time_stamp, vel_vector);
+
+		}
+	}
+
+	float step_vel = 0.0f;
+
+	if (vel_vector.empty())
+	{
+		step_vel = 0.0f;
+	}
+	else
+	{
+		//输出速度就是一秒内的滑动窗口的平均速度
+		for (int i = 0; i < vel_vector.size(); i++)
+		{
+			step_vel += vel_vector[i].vel;
+		}
+	}
+
+	return step_vel;
+}
+
+void StepVel::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+	setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
+		right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
+
+	setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
+		left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
+
+	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, RIGHT_FOOT);
+
+	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, LEFT_FOOT);
+
+
+	if (jump == 1)
+	{
+		left_jump_cancel_update_vel = 1;
+		right_jump_cancel_update_vel = 1;
+	}
+
+	if ((left_jump_cancel_update_vel && left_step_data_vector.size() > 1) ||
+		(right_jump_cancel_update_vel && right_step_data_vector.size() > 1))
+	{
+		if (!left_vel_vector.empty())
+		{
+			int dt = time_stamp - left_vel_vector.back().time_stamp;
+
+			for (int i = 0; i < left_vel_vector.size(); i++)
+			{
+				left_vel_vector[i].time_stamp += dt;
+			}
+
+		}
+	}
+	//过程中有跳，就不要处理跳过程的数据了， 并且利用时间戳来规避速度的更新
+	if (left_jump_cancel_update_vel && left_step_data_vector.size() > 1 && left_step_data_vector.back().zupt == 1)
+	{
+		std::cout << "left_step_data_vector.clear();" << endl;
+
+		left_jump_cancel_update_vel = 0;
+
+		shoes_data_cell shoes_data_temp = left_step_data_vector.back();
+
+		left_step_data_vector.clear();
+
+		left_step_data_vector.push_back(shoes_data_temp);
+	}
+
+	if (right_jump_cancel_update_vel && right_step_data_vector.size() > 1 && right_step_data_vector.back().zupt == 1)
+	{
+		std::cout << "right_step_data_vector.clear();" << endl;
+
+		right_jump_cancel_update_vel = 0;
+
+		shoes_data_cell shoes_data_temp = right_step_data_vector.back();
+
+		right_step_data_vector.clear();
+
+		right_step_data_vector.push_back(shoes_data_temp);
+	}
+
+	float left_step_vel = get_vel_mean(time_stamp, left_vel_vector, left_step_data_vector);
+	float right_step_vel = get_vel_mean(time_stamp, left_vel_vector, right_step_data_vector);
+
+	float step_vel_temp = 0.0f;
+	
+	for (int i = 0; i < left_vel_vector.size(); i++)
+	{
+		step_vel_temp += left_vel_vector[i].vel;
+	}
+
+	if (step_vel_temp > 5.0f)
+	{
+		step_vel_temp = 5.0f;
+	}
+
+	//这里在加个触地的衰减，让速度更加平滑
+	
+	float both_zupt_weak = 1.0f;
+
+	if (left_zupt && right_zupt)
+	{
+		both_zupt_count++;
+	}
+	else
+	{
+		both_zupt_count = 0;
+	}
+
+	if (both_zupt_count >= 5)
+	{
+		both_zupt_weak = (25 - both_zupt_count) / 20.f;
+	}
+
+	if (both_zupt_count >= 25)
+	{
+		both_zupt_weak = 0.0f;
+	}
+
+	step_vel_temp *= both_zupt_weak;
+
+	//std::cout << "step_vel : " << step_vel_temp << endl;
+
+	step_vel = step_vel_temp * 100.0f;
+
+}
+
+int StepVel::getStepVel()
+{
+	return step_vel;
+}
+
+
+

footRing_sdk_1.0/src/TestGame.cpp

@@ -0,0 +1,60 @@
+#include "TestGame.h"
+
+TestGame::TestGame()
+{
+	 jump_valid_time = 0;
+	 jump_calm_down = 0;
+
+	 last_jump = 0;
+}
+
+
+void TestGame::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+
+	result[0] = -1;
+	
+	if (left_acc_deque.size() > 10)
+	{
+		left_acc_deque.pop_front();
+		right_acc_deque.pop_front();
+	}
+
+	left_acc_deque.push_back(left_acc[2]);
+	right_acc_deque.push_back(right_acc[2]);
+
+	int max_left_acc = *(std::max_element(left_acc_deque.begin(), left_acc_deque.end()));
+	int max_right_acc = *(std::max_element(right_acc_deque.begin(), right_acc_deque.end()));
+
+
+	if (jump_calm_down == 0 && max_left_acc - left_acc[2] > 2048 && max_right_acc - right_acc[2] > 2048
+		&& max_left_acc > 3096 && max_right_acc > 3096 && left_acc[2] < 1024 && right_acc[2] < 1024 && 
+		left_acc_deque.front() > 1500 && right_acc_deque.front() > 1500)
+	{
+
+		jump = 1;
+		//result[0] = MOTION_JUMP;
+		left_acc_deque.clear();
+		right_acc_deque.clear();
+
+		jump_calm_down = 15;
+	}
+
+
+	if (jump_calm_down > 0)
+	{
+		jump_calm_down--;
+	}
+
+	if (last_jump == 0 && jump == 1)
+	{
+		result[0] = MOTION_JUMP;
+	}
+
+	last_jump = jump;
+
+
+}
+

footRing_sdk_1.0/src/Tricycle.cpp

@@ -0,0 +1,264 @@
+#include "Tricycle.h"
+
+Tricycle::Tricycle()
+{
+	memset(result, -1, 4 * sizeof(int));
+
+	result[0] = 0;
+	result[1] = 0;
+
+	left_zupt_count = 0;
+	right_zupt_count = 0;
+
+	left_foot_init_heading = 0.0f;
+	right_foot_init_heading = 0.0f;
+
+	left_trigger_time = 0;
+	right_trigger_time = 0;
+
+	last_left_zupt = 1;
+	last_right_zupt = 1;
+
+	memset(last_left_pos, 0, 3 * sizeof(int));
+	memset(last_right_pos, 0, 3 * sizeof(int));
+
+	left_jump_status = 0;
+	right_jump_status = 0;
+
+	has_init = 0;
+
+	last_jump = 0;
+
+	prop_status = 0;
+
+}
+
+void Tricycle::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
+	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
+	int jump, int down, int rssi)
+{
+	//1、确定初始的方向
+
+
+	if (left_acc_z_queue.size() > 10)
+	{
+		left_acc_z_queue.pop_front();
+		right_acc_z_queue.pop_front();
+	}
+	
+	left_acc_z_queue.push_back(left_acc[2]);
+	right_acc_z_queue.push_back(right_acc[2]);
+
+	if (has_init == 0)
+	{
+		if (*(max_element(left_acc_z_queue.begin(), left_acc_z_queue.end())) - *(min_element(left_acc_z_queue.begin(), left_acc_z_queue.end())) < 512
+			&& *(max_element(right_acc_z_queue.begin(), right_acc_z_queue.end())) - *(min_element(right_acc_z_queue.begin(), right_acc_z_queue.end())) < 512)
+		{
+			left_zupt = 1;
+			right_zupt = 1;
+		}
+	}
+
+
+	if (left_zupt)
+	{
+		left_zupt_count++;
+	}
+	else
+	{
+		left_zupt_count = 0;
+	}
+
+	if (right_zupt)
+	{
+		right_zupt_count++;
+	}
+	else
+	{
+		right_zupt_count = 0;
+	}
+
+
+	float cur_left_heading = left_att[0] * 0.0001f;
+	float cur_right_heading = right_att[0] * 0.0001f;
+
+
+	//计算初始方向
+	if (left_zupt_count > 100 && right_zupt_count > 100)
+	{
+		left_foot_init_heading = cur_left_heading;
+		right_foot_init_heading = cur_right_heading;
+
+		if (has_init == 0)
+		{
+			std::cout << "Cycle has inited !!!" << endl;
+		}
+
+		has_init = 1;
+
+	}
+
+
+	float left_heading_offset = calHeadingOffset(left_foot_init_heading, cur_left_heading);
+
+	float right_heading_offset = calHeadingOffset(right_foot_init_heading, cur_right_heading);
+
+	//std::cout << "left_heading_offset : " << left_heading_offset << " right_heading_offset : " << right_heading_offset << endl;
+
+	if (fabsf(calHeadingOffset(left_heading_offset, right_heading_offset)) > PI / 3.0f)
+	{
+		std::cout<< "dual_foot_heading appear max angle" <<endl;
+	}
+
+
+	float game_heading_offset = (left_heading_offset + right_heading_offset) * 0.5f;
+
+	//std::cout << "前进度数 ：" << game_heading_offset / PI * 180.0f<<endl;
+
+	//2、控制释放道具
+	//先设置一个触发命令的时间点，即怎么判定双脚同时触地，现在设置0.5S左右的大小来等待触地信号的同时到达
+
+	int down_prop = -1;
+
+	int up_prop = -1;
+
+	if (last_jump == 0 && jump)
+	{
+		left_jump_status = 1;
+		right_jump_status = 1;
+
+		//统计跳
+		addMotionCount(JUMP_COUNT);
+
+		std::cout << "过程中出现跳" << endl;
+	}
+
+	//统计步数
+	if (left_zupt && last_left_zupt == 0)
+	{
+		result[0] = MOTION_LEFT_ON_FLOOR;
+
+		addMotionCount(STEP_COUNT);
+	}
+	else
+	{
+		result[0] = -1;
+	}
+
+	if (right_zupt && last_right_zupt == 0)
+	{
+		result[1] = MOTION_RIGHT_ON_FLOOR;
+		addMotionCount(STEP_COUNT);
+	}
+	else
+	{
+		result[1] = -1;
+	}
+
+	if ((left_zupt && last_left_zupt == 0) || (right_zupt && last_right_zupt == 0))
+	{
+		if (left_pos_offset < -150 && right_pos_offset > 150)
+		{
+			up_prop = 1;
+
+			prop_status = up_prop;
+
+			std::cout << "上层 发放物品" << endl;
+		}
+		else if (left_jump_status == 1 && right_jump_status == 1)
+		{
+			if (abs(left_pos_offset) < 100 && abs(right_pos_offset) < 100 && prop_status == 1)
+			{
+				up_prop = 1;
+				std::cout << "因为上个状态是 上层发放物品，所以 这次也是 上层发放物品" << endl;
+			}
+			else if (abs(left_pos_offset) < 80 && abs(right_pos_offset) < 80)
+			{
+				down_prop = 1;
+
+				prop_status = 0;
+
+				std::cout << "下层 发放物品" << endl;
+			}
+			else
+			{
+				prop_status = 0;
+			}
+			std::cout << "left_pos_offset : " << left_pos_offset << "  right_pos_offset : " << right_pos_offset << endl;
+		}
+
+		if (left_pos_offset > 80 && right_pos_offset < -80)
+		{
+			prop_status = 0;
+		}
+
+		left_jump_status = 0;
+		right_jump_status = 0;
+	}
+
+
+
+
+	if (left_zupt)
+	{
+
+		for (int i = 0; i < 3; i++)
+		{
+			last_left_pos[i] = left_pos[i];
+		}
+
+	}
+
+	left_pos_offset = left_pos[0] - last_left_pos[0];
+
+	if (right_zupt)
+	{
+		for (int i = 0; i < 3; i++)
+		{
+			last_right_pos[i] = right_pos[i];
+		}
+	}
+
+	right_pos_offset = right_pos[0] - last_right_pos[0];
+
+	last_left_zupt = left_zupt;
+	last_right_zupt = right_zupt;
+
+	last_jump = jump;
+
+
+	//result[0] = (int)(game_heading_offset * 10000.0f);
+
+	//result[1] = result[0];
+
+	left_att[0] = (int)(game_heading_offset * 10000.0f);
+
+	result[2] = (down_prop == 1 ? MOTION_JUMP_OC : -1);
+	result[3] = (up_prop == 1 ? MOTION_JUMP_VERTICAL : -1);
+
+	result[4] = left_att[0];
+
+	if (has_init == 0)
+	{
+		/*result[0] = 0;
+		result[1] = 0;*/
+		left_att[0] = 0;
+	}
+
+}
+
+float Tricycle::calHeadingOffset(float init_heading, float cur_heading)
+{
+	float heading_offset = (cur_heading  - init_heading);
+
+	if (heading_offset > PI)
+	{
+		heading_offset = -(2 * PI - heading_offset);
+	}
+	else if (heading_offset < -PI)
+	{
+		heading_offset = (2 * PI + heading_offset);
+	}
+
+	return heading_offset;
+}

footRing_sdk_1.0/src/online_motion.cpp

@@ -0,0 +1,482 @@
+
+#include "online_motion.h"
+#include <iostream>
+#include <algorithm>
+#include "math.h"
+
+void top_point(deque<int> &left_front_mag_queue , int &top_index, int &left_index, int &right_index)
+{
+	int left_top_index = left_front_mag_queue.size() - 1;
+	//std::cout << "int left_top_index = left_front_mag_queue.size() - 1" << std::endl;
+	while (left_top_index > 2 &&
+		(left_front_mag_queue[left_top_index] < left_front_mag_queue[left_top_index - 1] || 
+			left_front_mag_queue[left_top_index] < left_front_mag_queue[left_top_index - 2] ||
+			left_front_mag_queue[left_top_index] < left_front_mag_queue[left_top_index - 3]))
+	{
+		left_top_index--;
+
+		//std::cout << "left_top_index--;" << std::endl;
+	}
+
+	int k = left_top_index;
+
+	left_index = k;
+	int dist = 40000;
+	while (k > 2)
+	{
+
+		if (left_front_mag_queue[k] > left_front_mag_queue[k - 1] || 
+			left_front_mag_queue[k] > left_front_mag_queue[k - 2] ||
+			left_front_mag_queue[k] > left_front_mag_queue[k - 3])
+		{
+			if (dist > abs(left_front_mag_queue.back() - left_front_mag_queue[k - 1]))
+			{
+				dist = abs(left_front_mag_queue.back() - left_front_mag_queue[k - 1]);
+				left_index = k-1;
+			}
+			k--;
+		}
+		else
+		{
+			break;
+		}
+
+	}
+
+	k = left_front_mag_queue.size() - 1;
+
+	right_index = k;
+
+	/*while (k > left_top_index)
+	{
+		if (dist > abs(left_front_mag_queue[k] - left_front_mag_queue[left_index]))
+		{
+			dist = abs(left_front_mag_queue[k] - left_front_mag_queue[left_index]);
+
+			right_index = k;
+		}
+		k--;
+	}*/
+
+	top_index = left_top_index;
+
+}
+
+//最小值滤波
+int online_motion::min_window_val(deque<int>& press_deque, int mid_window_size)
+{
+	deque<int> press_deque_temp = press_deque;
+
+	if (press_deque_temp.size() >= mid_window_size)
+	{
+		std::sort(press_deque_temp.begin(), press_deque_temp.end());
+	}
+	else
+	{
+		return press_deque.back();
+	}
+
+	//return press_deque[press_deque_temp.size() / 2];
+
+	return press_deque_temp[0];
+}
+
+int online_motion::back_press_up_trend(deque<int>& left_press_deque, deque<int>& right_press_deque)
+{
+	if (left_press_deque.size() >= 20)
+	{
+		int i = left_press_deque.size() - 1;
+
+		while (i > 10 && (left_press_deque[i] >= left_press_deque[i - 1] || left_press_deque[i] >= left_press_deque[i - 2]
+			|| left_press_deque[i] >= left_press_deque[i - 3]) &&
+			(right_press_deque[i] >= right_press_deque[i - 1] || right_press_deque[i] >= right_press_deque[i - 2]
+				|| right_press_deque[i] >= right_press_deque[i - 3])
+			&& left_press_deque[i] > 12000 && right_press_deque[i] > 12000)
+		{
+			i--;
+		}
+
+		if (left_press_deque.back() > left_press_deque[i] + 1000 && right_press_deque.back() > right_press_deque[i] + 1000)
+		{
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+int online_motion::online_detect_down(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag,
+	int* right_acc, int* left_acc)
+{
+
+	//滑动窗口补充
+	left_press_filter_window.push_back(left_back_mag);
+	right_press_filter_window.push_back(right_back_mag);
+
+	//采用最小值滤波， 较好的过滤毛刺
+	if (left_press_filter_window.size() > 5)
+	{
+		left_press_filter_window.pop_front();
+		right_press_filter_window.pop_front();
+
+		left_back_mag = min_window_val(left_press_filter_window, 5);
+		right_back_mag = min_window_val(right_press_filter_window, 5);
+
+	}
+
+	if (right_back_mag_queue.size() > 20)
+	{
+		right_back_mag_queue.pop_front();
+		left_back_mag_queue.pop_front();
+
+		right_front_mag_queue.pop_front();
+		left_front_mag_queue.pop_front();
+
+		left_front_acc.pop_front();
+		right_front_acc.pop_front();
+
+	}
+	right_back_mag_queue.push_back(right_back_mag);
+	left_back_mag_queue.push_back(left_back_mag);
+
+	right_front_mag_queue.push_back(right_front_mag);
+	left_front_mag_queue.push_back(left_front_mag);
+
+	/*left_front_acc.push_back(left_acc[2]);
+	right_front_acc.push_back(right_acc[2]);*/
+
+	left_front_acc.push_back(sqrt(left_acc[0] * left_acc[0] + left_acc[1] * left_acc[1]));
+	right_front_acc.push_back(sqrt(right_acc[0] * right_acc[0] + right_acc[1] * right_acc[1]));
+
+
+	deque<int> right_back_temp{ right_back_mag };
+	deque<int> left_back_temp{ left_back_mag };
+
+	int left_acc_z_max = left_front_acc.back();
+	int left_acc_z_min = left_front_acc.back();
+
+	int right_acc_z_max = right_front_acc.back();
+	int right_acc_z_min = right_front_acc.back();
+
+	int back_down = 0;
+	if (right_back_mag_queue.size() >= 20)
+	{
+		back_down = back_press_up_trend(left_back_mag_queue, right_back_mag_queue);
+
+		if (back_down)
+		{
+			if (top_back_press_valid_time == 0)
+			{
+				left_top_back_press = left_back_mag_queue.back();
+				right_top_back_press = right_back_mag_queue.back();
+
+				top_back_press_valid_time = 60;
+			}
+			else
+			{
+
+				if (left_top_back_press > left_back_mag_queue.back() || right_top_back_press > right_back_mag_queue.back())
+				{
+					back_down = 0;
+				}
+				else
+				{
+					left_top_back_press = left_back_mag_queue.back();
+					right_top_back_press = right_back_mag_queue.back();
+
+					top_back_press_valid_time = 60;
+				}
+			}
+		}
+	}
+
+	if (top_back_press_valid_time > 0)
+	{
+		top_back_press_valid_time--;
+	}
+
+
+	 deque<int> right_front_temp{ right_front_mag };
+	 deque<int> left_front_temp{ left_front_mag };
+
+	 left_acc_z_max = left_front_acc.back();
+	 left_acc_z_min = left_front_acc.back();
+
+	 right_acc_z_max = right_front_acc.back();
+	 right_acc_z_min = right_front_acc.back();
+
+
+	
+	int front_down = 0;
+	if (right_front_mag_queue.size() > 20)
+	for (int i = right_front_mag_queue.size() - 1; i > right_front_mag_queue.size() -11; i--)
+	{
+		if ((left_front_mag_queue[i] > left_front_mag_queue[i - 1]+100 || left_front_mag_queue[i] > left_front_mag_queue[i - 2] + 100
+			|| left_front_mag_queue[i] > left_front_mag_queue[i - 3]) + 100 &&
+			(right_front_mag_queue[i] > right_front_mag_queue[i - 1] + 100 || right_front_mag_queue[i] > right_front_mag_queue[i - 2] + 100
+				|| right_front_mag_queue[i] < right_front_mag_queue[i - 3] + 100))
+		{
+			right_front_temp.push_back(right_front_mag_queue[i]);
+			left_front_temp.push_back(left_front_mag_queue[i]);
+		}
+		else
+		{
+			break;
+		}
+
+		if (*max_element(left_front_acc.begin() + 10 , left_front_acc.end()) - *min_element(left_front_acc.begin() + 10, left_front_acc.end()) < 256
+			&& *max_element(right_front_acc.begin() + 10, right_front_acc.end()) - *min_element(right_front_acc.begin() + 10, right_front_acc.end()) < 256)
+		{
+			if (right_front_temp.size() > 5 && left_front_temp.size() > 5 && left_front_temp.front() >  left_front_temp.back() + 1500
+				&& right_front_temp.front() > right_front_temp.back() + 1500
+				&& abs(left_front_temp.front() - right_front_temp.front()) < 6000)
+			{
+				front_down = 1;
+			}
+		}
+
+	}
+
+	//继续补充 微小变动的
+	int little_front_down = 0;
+
+
+
+	if (left_front_acc.size() > 20)
+	{
+		if (*max_element(left_front_acc.end() - 5, left_front_acc.end())
+			- *min_element(left_front_acc.end() - 5, left_front_acc.end()) < 256
+			&& *max_element(right_front_acc.end() - 5, right_front_acc.end())
+			- *min_element(right_front_acc.end() - 5, right_front_acc.end()) < 256)
+		{
+	
+			if (right_front_mag_canve_queue.size() == 0)
+			{
+				for (int k = right_front_mag_queue.size() - 6; k < right_front_mag_queue.size(); k++)
+				{
+					right_front_mag_canve_queue.push_back(right_front_mag_queue[k]);
+					left_front_mag_canve_queue.push_back(left_front_mag_queue[k]);
+				}
+			}
+			else
+			{
+				right_front_mag_canve_queue.push_back(right_front_mag_queue.back());
+				left_front_mag_canve_queue.push_back(left_front_mag_queue.back());
+			}
+
+		}
+		else
+		{
+			right_front_mag_canve_queue.clear();
+			left_front_mag_canve_queue.clear();
+		}
+	}
+
+	if (right_front_mag_canve_queue.size() > 20)
+	{
+		right_front_mag_canve_queue.pop_front();
+		left_front_mag_canve_queue.pop_front();
+	}
+
+
+	if (left_front_mag_canve_queue.size() > 9)
+	{
+		//1、补充蹲，检测到一个完成的波视为蹲就好
+
+		int left_top_index;
+		int left_foot_left_side_index, left_foot_right_side_index;
+		top_point(left_front_mag_canve_queue, left_top_index, left_foot_left_side_index, left_foot_right_side_index);
+
+		int right_top_index;
+		int right_foot_left_side_index, right_foot_right_side_index;
+		top_point(right_front_mag_canve_queue, right_top_index, right_foot_left_side_index, right_foot_right_side_index);
+
+		if (
+			abs(left_top_index - right_top_index) < 4    //保持定点位置不要相差太多
+			&&left_front_mag_canve_queue[left_top_index] > left_front_mag_canve_queue[left_foot_left_side_index] + 700
+			&& left_front_mag_canve_queue[left_top_index] > left_front_mag_canve_queue[left_foot_right_side_index] + 700
+			&& right_front_mag_canve_queue[right_top_index] > right_front_mag_canve_queue[right_foot_left_side_index] + 700
+			&& right_front_mag_canve_queue[right_top_index] > right_front_mag_canve_queue[right_foot_right_side_index] + 700)
+		{
+			little_front_down = 1;
+		}
+	}
+
+	if (right_front_mag_queue.size() > 20)
+	{
+		int i = right_front_mag_queue.size() - 1;
+
+		if ((left_front_mag_queue[i] > left_front_mag_queue[i - 1] ) &&
+			(right_front_mag_queue[i] > right_front_mag_queue[i - 1]))
+		{
+			if (last_back_down || last_front_down)
+			{
+				front_down = 1;
+			}
+		}
+	}
+
+	if (right_front_mag_queue.size() > 20)
+	{
+		int i = right_front_mag_queue.size() - 1;
+
+		if ((left_front_mag_queue[i] < left_front_mag_queue[i - 1] || left_front_mag_queue[i] < left_front_mag_queue[i - 2]
+			|| left_front_mag_queue[i] < left_front_mag_queue[i - 3]) &&
+			(right_front_mag_queue[i] < right_front_mag_queue[i - 1] || right_front_mag_queue[i] < right_front_mag_queue[i - 2]
+				|| right_front_mag_queue[i] < right_front_mag_queue[i - 3]))
+		{
+			if (last_little_front_down)
+			{
+				little_front_down = 1;
+			}
+		}
+	}
+
+
+
+	int res = 0;
+	if (back_down == 1)
+	{
+		res |= 0x02;
+	}
+
+
+	if (front_down == 1)
+	{
+		res |= 0x01;
+	}
+
+	if (little_front_down == 1)
+	{
+		res |= 0x04;
+	}
+
+	last_back_down = back_down;
+	last_front_down = front_down;
+	last_little_front_down = little_front_down;
+
+
+
+
+	return res;
+
+}
+
+/*
+* 检测触地的一个东西
+*/
+
+void online_motion::push_press_queue(deque<int>& press_queue, int press, int queue_size)
+{
+	press_queue.push_back(press);
+
+	if (press_queue.size() > queue_size)
+	{
+		press_queue.pop_front();
+	}
+}
+
+int online_motion::check_press_queue_up(deque<int>& press_queue, int thresh_hold)
+{
+	int k = press_queue.size() - 1;
+
+	while (k > 0)
+	{
+		if (!(press_queue[k] >= press_queue[k - 1]))
+		{
+			break;
+		}
+		k--;
+	}
+
+	if(press_queue.back() - press_queue[k] > thresh_hold)
+	{
+		return 1;
+	}
+
+	return 0;
+}
+
+
+int online_motion::online_detect_touch_floor(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag)
+{
+	push_press_queue(left_front_press_queue_touch_floor, left_front_mag, 10);
+
+	push_press_queue(right_front_press_queue_touch_floor, right_front_mag, 10);
+
+	push_press_queue(left_back_press_queue_touch_floor, left_back_mag, 10);
+
+	push_press_queue(right_back_press_queue_touch_floor, right_back_mag, 10);
+
+	int right_foot_on_floor = 0;
+	if (check_press_queue_up(right_front_press_queue_touch_floor, 1000) || check_press_queue_up(right_back_press_queue_touch_floor, 1000))
+	{
+		right_foot_on_floor = 1;
+	}
+
+	int left_foot_on_floor = 0;
+	if (check_press_queue_up(left_front_press_queue_touch_floor, 1000) || check_press_queue_up(left_back_press_queue_touch_floor, 1000))
+	{
+		left_foot_on_floor = 1;
+	}
+
+	return  right_foot_on_floor << 1 | left_foot_on_floor;
+
+
+}
+
+
+/*
+* 在线检测跳
+*/
+
+int online_motion::online_detect_jump(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag,
+	int* right_acc, int* left_acc)
+{
+	right_front_jump_mag_queue.push_back(right_front_mag);
+	left_front_jump_mag_queue.push_back(left_front_mag);
+
+	right_acc_z_queue.push_back(right_acc[2]);
+	left_acc_z_queue.push_back(left_acc[2]);
+
+	if (right_front_jump_mag_queue.size() > 10)
+	{
+		right_front_jump_mag_queue.pop_front();
+		left_front_jump_mag_queue.pop_front();
+
+		right_acc_z_queue.pop_front();
+		left_acc_z_queue.pop_front();
+	}
+
+	int k = left_front_jump_mag_queue.size() - 1;
+	while (k > 0)
+	{
+		if (!(left_front_jump_mag_queue[k] < left_front_jump_mag_queue[k - 1] + 100
+			&& right_front_jump_mag_queue[k] < right_front_jump_mag_queue[k - 1] + 100))
+		{
+			break;
+		}
+		k--;
+	}
+
+	int res = 0;
+
+	if (left_front_jump_mag_queue[k] - left_front_jump_mag_queue.back() > 4000
+		&& right_front_jump_mag_queue[k] - right_front_jump_mag_queue.back() > 4000)
+	{
+		auto left_max_element = max_element(left_acc_z_queue.begin(), left_acc_z_queue.end());
+		auto left_min_element = min_element(left_acc_z_queue.begin(), left_acc_z_queue.end());
+
+		auto right_max_element = max_element(right_acc_z_queue.begin(), right_acc_z_queue.end());
+		auto right_min_element = min_element(right_acc_z_queue.begin(), right_acc_z_queue.end());
+
+		if (left_max_element < left_min_element && right_max_element < right_min_element &&
+			*left_max_element - *left_min_element > 1500 && *right_max_element - *right_min_element > 1500)
+		{
+			res = 1;
+		}
+	}
+	return res;
+
+}
+