优化脚环 跳格子游戏

footRing_sdk_1.0/include/JumpHouse.h

@@ -12,6 +12,17 @@ class JumpHouse : public PublicSDKMotion
 		bool unzuptValid(int* max_acc, int* min_acc);
 	private:
 
+		deque<int> rssi_vec;
+		deque<int> pitch_vec;
+		deque<int> roll_vec;
+		deque<int> left_acc_vec;
+		deque<int> right_acc_vec;
+
+		int pitch_reference_val = 0;
+		int roll_reference_val = 0;
+
+		int has_init = 0;
+
 		deque<shoes_data_cell> left_shoes_data_vector;
 		deque<shoes_data_cell> right_shoes_data_vector;
 
@@ -35,6 +46,8 @@ class JumpHouse : public PublicSDKMotion
 		int left_init;
 		int right_init;
 
+		int left_zupt_extension_time = 0;
+		int right_zupt_extension_time = 0;
 
 
 		const int rssi_threshold = 25;

footRing_sdk_1.0/src/JumpHouse.cpp

@@ -22,7 +22,7 @@ JumpHouse::JumpHouse()
 
 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)
+	if ((max_acc[0] - min_acc[0] > 2048 || max_acc[1] - min_acc[1] > 2048 || max_acc[2] - min_acc[2] > 2048) && min_acc[0] < 500)
 	{
 		std::cout << "JumpHouse::unzuptValid, " << max_acc[0] << ", " << min_acc[0] << ", " << max_acc[1] << ", "
 			<< min_acc[1] << ", " << max_acc[2] << ", " << min_acc[2] << ", " <<endl;
@@ -40,7 +40,51 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 {
 
 	//翘脚视为不在地上
-	// 	   
+	// 
+
+	if (has_init == 0)
+	{
+		rssi_vec.push_back(rssi);
+		pitch_vec.push_back(left_att[1] - right_att[1]);
+		roll_vec.push_back((left_att[2] + right_att[2]) * 0.5f);
+		left_acc_vec.push_back(sqrt(left_acc[0] * left_acc[0] + left_acc[1] * left_acc[1] + left_acc[2] * left_acc[2]));
+		right_acc_vec.push_back(sqrt(right_acc[0] * right_acc[0] + right_acc[1] * right_acc[1] + right_acc[2] * right_acc[2]));
+
+
+		if (rssi_vec.size() > 50)
+		{
+			rssi_vec.pop_front();
+			pitch_vec.pop_front();
+			roll_vec.pop_front();
+			left_acc_vec.pop_front();
+			right_acc_vec.pop_front();
+		}
+
+		if (rssi_vec.size() == 50)
+		{
+			int max_rssi = *max_element(rssi_vec.begin(), rssi_vec.end());
+			int left_max_acc = *max_element(left_acc_vec.begin(), left_acc_vec.end());
+			int left_min_acc = *min_element(left_acc_vec.begin(), left_acc_vec.end());
+
+			int right_max_acc = *max_element(right_acc_vec.begin(), right_acc_vec.end());
+			int right_min_acc = *min_element(right_acc_vec.begin(), right_acc_vec.end());
+
+			if (max_rssi < 25 && left_max_acc - left_min_acc < 510 && right_max_acc - right_min_acc < 510)
+			{
+				for (int i = 0; i < 50; i++)
+				{
+					pitch_reference_val += pitch_vec[i];
+					roll_reference_val += pitch_vec[i];
+				}
+				pitch_reference_val /= 50;
+				roll_reference_val /= 50;
+
+				std::cout << "init roll and pitch  !!!!" << endl;
+			}
+
+		}
+
+	}
 	if (left_zupt == 1 && left_att[1] * 0.0001f > 0.5f)
 	{
 		std::cout << "if (left_zupt == 1 && left_att[1] * 0.0001f > 0.6f) " << left_att[1] * 0.0001f <<  endl;
@@ -52,6 +96,30 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 		right_zupt = 0;
 	}
 
+	if (left_zupt)
+	{
+		left_zupt_extension_time = 5;
+	}
+
+	if (right_zupt)
+	{
+		right_zupt_extension_time = 5;
+	}
+
+	if (left_zupt_extension_time > 0)
+	{
+		left_zupt = 1;
+
+		left_zupt_extension_time--;
+	}
+
+	if (right_zupt_extension_time > 0)
+	{
+		right_zupt = 1;
+
+		right_zupt_extension_time--;
+	}
+
 	//缓存一步内的空中数据
 	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);
@@ -70,6 +138,8 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 		//if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) && press_valid)
 		if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
 		{
+			left_zupt_extension_time = 20;
+
 			left_cmd_wait_time = time_stamp;
 
 			left_init = 1;
@@ -83,6 +153,7 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 		//if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) && press_valid)
 		if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
 		{
+			right_zupt_extension_time = 20;
 
 			right_cmd_wait_time = time_stamp;
 
@@ -101,11 +172,13 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 	//重置命令
 	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)
 	{
+		std::cout  << " left_att[1] : " << left_att[1] << " right_att[1] : " << right_att[1] << std::endl;
 		if (right_init == 1)
 		{
 			if (rssi < rssi_threshold)
@@ -124,7 +197,8 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 		}
 		else
 		{
-			if (abs(left_att[1] - right_att[1]) < 500)
+			std::cout << "pitch_reference_val : " << pitch_reference_val << endl;
+			if (abs(left_att[1] - right_att[1] - pitch_reference_val) < 2000)
 			{
 				if (rssi < rssi_threshold)
 				{
@@ -137,6 +211,9 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 					result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
 					std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << rssi << ", " << " 开脚" << std::endl;
 				}
+
+				right_shoes_data_vector.pop_front();
+				right_step_data_vector.pop_front();
 			}
 			else
 			{
@@ -155,6 +232,7 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 	}
 	else if (right_init == 1 && time_stamp - right_cmd_wait_time >= 9 && time_stamp - right_cmd_wait_time < 30)
 	{
+		std::cout  << " left_att[1] : " << left_att[1] << " right_att[1] : " << right_att[1] << std::endl;
 		if (left_init == 1)
 		{
 			if (rssi < rssi_threshold)
@@ -175,7 +253,8 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 		}
 		else
 		{
-			if (abs(left_att[1] - right_att[1]) < 500)
+			std::cout << "pitch_reference_val : " << pitch_reference_val << endl;
+			if (abs(left_att[1] - right_att[1] - pitch_reference_val) < 2000)
 			{
 				if (rssi < rssi_threshold)
 				{
@@ -188,6 +267,9 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
 					result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
 					std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << rssi << ", " << " 开脚" << std::endl;
 				}
+
+				left_shoes_data_vector.pop_front();
+				left_step_data_vector.pop_front();
 			}
 			else
 			{