add for inav state.

KNN/KNN.cpp

@@ -9,10 +9,9 @@
 #include "./../CSVparser/CSVparser.h"
 #include "KNN.h"
 
-using namespace std;  
+using namespace std;
+
 
-extern const int colLen;  
-extern const int rowLen; 
 ifstream fin;  
 ofstream fout;  
 
@@ -23,10 +22,12 @@ string Trim(string& str)
  return str;
 }
 
-KNN::KNN(int k)  
+KNN::KNN(int magic_x, int magic_y, int magic_z)  
 {  
-    this->k = k;
-	csv::Parser file = csv::Parser("trainning_data_magic.csv");  
+	int k = 10;
+    this->k = k;  
+
+	csv::Parser file = csv::Parser("d:/magic_map/trainning_data_magic.csv");  
   
     std::cout << file[0][0] << std::endl; // display : 1997  
     std::cout << file[0] << std::endl; // display : 1997 | Ford | E350
@@ -54,9 +55,9 @@ KNN::KNN(int k)
   
     cout<<"please input the test data :"<<endl;  
     /* inuput the test data */  
-    for(int i=0;i<colLen;i++)  
-        cin>>testData[i];  
-      
+    testData[0] = magic_x;
+	testData[1] = magic_y; 
+	testData[2] = magic_z; 
 }  
   
 /* 
@@ -100,7 +101,7 @@ void KNN:: get_all_distance()
 /* 
  * check which label the test data belongs to to classify the test data  
  */  
-void KNN:: get_max_freq_label()  
+tLabel KNN:: get_max_freq_label()  
 {  
     //transform the map_index_dis to vec_index_dis  
     vector<PAIR> vec_index_dis( map_index_dis.begin(),map_index_dis.end() );  
@@ -127,7 +128,8 @@ void KNN:: get_max_freq_label()
         }  
         map_it++;  
     }  
-    cout<<"The test data belongs to the x:"<<label.x<<"y:"<<label.y<<" label"<<endl;  
+    cout<<"The test data belongs to the x:"<<label.x<<"y:"<<label.y<<" label"<<endl; 
+	return label;
 }  
 
 #if 0

KNN/KNN.h

@@ -50,11 +50,11 @@ private:
         double get_distance(tData *d1,tData *d2);  
 public:  
   
-        KNN(int k);  
+        KNN(int magic_x, int magic_y, int magic_z);  
   
         void get_all_distance();  
           
-        void get_max_freq_label();  
+        tLabel get_max_freq_label();  
   
         struct CmpByValue  
         {  

YAServerDlg.cpp

@@ -35,6 +35,8 @@
 #include "./system_basic_info/SystemAnalysis.h"
 #include "./debug_server/debug_server_socket.h"
 #include "./system_basic_info/SystemAnalysis.h"
+#include "./CSVparser/CSVparser.h"
+#include "./KNN/KNN.h"
 
 // 用来跟踪内存使用情况
 #define TRACE_MEMORY_WRITELOG
@@ -2480,7 +2482,7 @@ void CYAServerDlg::parse_package_data(BYTE* DataBuffer, int nLen, DWORD dwConnID
 	case CHAR_READER_SYNC_TIME:
 		{
 			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_5);
-			parse_data_reader_synctime(DataBuffer, nLen, nCurPos, CHAR_READER_SYNC_TIME);
+            parse_data_reader_synctime(DataBuffer, nLen, nCurPos, CHAR_READER_SYNC_TIME);
 			break;
 		}
 	case CHAR_READER_SYNC_TIME_ROOT:
@@ -2735,6 +2737,91 @@ int CYAServerDlg::parse_inav_msg(INAVLocateMsg* msgPtr)
     return card_num;
 }
 
+int g_magic_offset_x = 0;
+int g_magic_offset_y = 0;
+int g_magic_offset_z = 0;
+
+int tmp_magic_max_x = 0;
+int tmp_magic_min_x = 0;
+int tmp_magic_max_y = 0;
+int tmp_magic_min_y = 0;
+int tmp_magic_max_z = 0;
+int tmp_magic_min_z = 0;
+
+int init_times = 100;
+
+int g_magic_is_init = 0;
+
+bool init_magic_offset(INAVLocateData cardData)
+{
+    if(init_times > 0)
+	{
+	   return false;
+	}
+	else
+	{
+	    g_magic_offset_x = (tmp_magic_max_x + tmp_magic_min_x) / 2;
+		g_magic_offset_y = (tmp_magic_max_y + tmp_magic_min_y) / 2;
+		g_magic_offset_z = (tmp_magic_max_z + tmp_magic_min_z) / 2;
+		debug_print_syslog(0, "Magic init successful: magic_offset_x: %d, magic_offset_x: %d, magic_offset_x: %d", 
+			g_magic_offset_x, g_magic_offset_y, g_magic_offset_z);
+	    return true;
+	}
+    if(cardData.MagneticData_X > tmp_magic_max_x)
+	{
+        tmp_magic_max_x = cardData.MagneticData_X;
+	}
+    if(cardData.MagneticData_Y> tmp_magic_max_y)
+    {
+        tmp_magic_max_y = cardData.MagneticData_X;
+    }
+    if(cardData.MagneticData_Z > tmp_magic_max_z)
+    {
+        tmp_magic_max_z = cardData.MagneticData_Z;
+	}
+    if(cardData.MagneticData_X < tmp_magic_min_x)
+	{
+        tmp_magic_min_x = cardData.MagneticData_X;
+    }
+    if(cardData.MagneticData_X < tmp_magic_min_y)
+	{
+        tmp_magic_min_y = cardData.MagneticData_X;
+    }
+    if(cardData.MagneticData_Z < tmp_magic_min_z)
+	{
+        tmp_magic_min_z = cardData.MagneticData_Z;
+	}
+	
+	init_times--;
+	
+}
+
+void write_magic_locate_map(INAVLocateData* msgPtr, std::shared_ptr<Card> card)
+{
+	csv::Parser file = csv::Parser("magic_locate_data.csv");  
+	    vector<string> tmpRow;
+		int magic_x = msgPtr->MagneticData_X;
+    	int magic_y = msgPtr->MagneticData_Y;
+    	int magic_z = msgPtr->MagneticData_Z;
+    	int x = card->x;
+    	int y = card->y;
+		int z = card->z;
+    	char str[20];
+        sprintf_s(str,"%d", magic_x);
+    	tmpRow.push_back(str);
+    	sprintf_s(str,"%d", magic_y);
+    	tmpRow.push_back(str);
+    	sprintf_s(str,"%d", magic_z);
+    	tmpRow.push_back(str);
+    	sprintf_s(str,"%d", x);
+    	tmpRow.push_back(str);
+    	sprintf_s(str,"%d", y);
+    	tmpRow.push_back(str);
+		//sprintf_s(str,"%d", z);
+    	//tmpRow.push_back(str);
+    	file.addRow(0, tmpRow);
+    	file.sync();
+}
 
 void CYAServerDlg::deal_inav_locate_msg(INAVLocateMsg* msgPtr, int card_num)
 {
@@ -2766,9 +2853,41 @@ void CYAServerDlg::deal_inav_locate_msg(INAVLocateMsg* msgPtr, int card_num)
     		card->set_reader_path_tdoa(mp_reader_path_list_tdoa);
         #endif		
     	}
-        card->x = card->x + msgPtr->INAVLocateInfo[i].Displacement_X;
-        card->y= card->y+ msgPtr->INAVLocateInfo[i].Displacement_Y;
-        card->z= card->z+ msgPtr->INAVLocateInfo[i].Displacement_Z;
+#if 0
+		
+		std::shared_ptr<MapInfo> pMap = nullptr;
+	    MapInfoMap::iterator it = mp_map_list.find(card->map_id);
+	    if(it != mp_map_list.end()){
+		    pMap = it->second;
+	    }
+	    else
+	    {
+	        return;
+	    }
+        card->x = card->x + msgPtr->INAVLocateInfo[i].Displacement_X / 1000 / pMap->map_scale;
+        card->y= card->y + msgPtr->INAVLocateInfo[i].Displacement_Y / 1000 / pMap->map_scale;
+        card->z= card->z + msgPtr->INAVLocateInfo[i].Displacement_Z / 1000 / pMap->map_scale;
+
+        //init magic offset
+        if(init_magic_offset(msgPtr->INAVLocateInfo[i]))
+        {
+              g_magic_is_init = 1;
+        }
+		//write magic data
+		if(1 == g_magic_is_init)
+		{
+		    write_magic_locate_map(INAVLocateMsg* msgPtr, std::shared_ptr<Card> card)
+		}
+#else if
+		
+		card->x = card->x + msgPtr->INAVLocateInfo[i].Displacement_X / 1000;
+        card->y= card->y + msgPtr->INAVLocateInfo[i].Displacement_Y / 1000;
+        card->z= card->z + msgPtr->INAVLocateInfo[i].Displacement_Z / 1000;
+#endif
+		INAV_STATUS card_state = calculate_inav_state(&(msgPtr->INAVLocateInfo[i]));
+		//calculate_inav_locate_by_magic(card, &(msgPtr->INAVLocateInfo[i]));
+		//write_inav_locate_magic_data(card, &(msgPtr->INAVLocateInfo[i]));
+		log_card_inav_state(card_state, str_card_id);
         save_card_postion(card);
         card_enter_map(card); // 切换地图
         card_enter_area(card);
@@ -2783,8 +2902,6 @@ void CYAServerDlg::deal_inav_locate_msg(INAVLocateMsg* msgPtr, int card_num)
         if(LT_READER != theApp.locate_type){
         	card->issent = false;
         }
-		INAV_STATUS card_state = calculate_inav_state(&(msgPtr->INAVLocateInfo[i]));
-		log_card_inav_state(card_state, str_card_id);
 	}
 	
     return;
@@ -2798,8 +2915,12 @@ INAV_STATUS CYAServerDlg::calculate_inav_state(INAVLocateData* cardData)
 	double accelerate_z = cardData->AccerateData_Z * 2 * GRAVITY_ACCERATION / INAV_READING_MAX;
 
 	double join_accerate = sqrt(pow(accelerate_x, 2) + pow(accelerate_y, 2) + pow(accelerate_z, 2));
-	
-	if(abs(join_accerate - GRAVITY_ACCERATION) > JOIN_ACCERATE_THRESHOLD)
+
+	debug_print_syslog(0, "displacement_x: %d, displacement_y: %d, displacement_z: %d", 
+			cardData->Displacement_X, cardData->Displacement_Y, cardData->Displacement_Z);
+	double join_diff = abs(join_accerate - GRAVITY_ACCERATION);
+	debug_print_syslog(0, "joint force: %10f", join_diff);
+	if(join_diff > JOIN_ACCERATE_THRESHOLD)
 	{
 	    state.Status = INAV_MOVING;
 	}
@@ -2810,12 +2931,28 @@ INAV_STATUS CYAServerDlg::calculate_inav_state(INAVLocateData* cardData)
 
 	if(INAV_MOVING == state.Status)
 	{
-	    //Y+ is the forward
+	    //X+ is the forward
+	    #if 0
         bool right_forward_condition = (abs(accelerate_x) > MOVING_THRESHOLD) &&(abs(accelerate_y) < MOVING_THRESHOLD) && (accelerate_x < 0);
         bool left_forward_condition = (abs(accelerate_x) > MOVING_THRESHOLD) && (abs(accelerate_y) < MOVING_THRESHOLD) && (accelerate_x > 0);
         bool forward_condition      = (abs(accelerate_y) > MOVING_THRESHOLD) && (abs(accelerate_x) < MOVING_THRESHOLD) && (accelerate_y < 0);
         bool backward_condition     = (abs(accelerate_y) > MOVING_THRESHOLD) && (abs(accelerate_x) < MOVING_THRESHOLD) && (accelerate_y > 0);
-
+        #endif
+		bool forward_condition = (abs(accelerate_x) > MOVING_THRESHOLD) &&(abs(accelerate_y) < MOVING_THRESHOLD) && (accelerate_x > 0);
+        bool backward_condition = (abs(accelerate_x) > MOVING_THRESHOLD) && (abs(accelerate_y) < MOVING_THRESHOLD) && (accelerate_x < 0);
+        bool left_forward_condition      = (abs(accelerate_y) > MOVING_THRESHOLD) && (abs(accelerate_x) < MOVING_THRESHOLD) && (accelerate_y < 0);
+        bool right_forward_condition     = (abs(accelerate_y) > MOVING_THRESHOLD) && (abs(accelerate_x) < MOVING_THRESHOLD) && (accelerate_y > 0);
+		debug_print_syslog(0, "accelerate_x: %f, accelerate_y: %f", 
+			accelerate_x, accelerate_y);
+		debug_print_syslog(0, "right_forward_condition: %d, left_forward_condition: %d, forward_condition: %d, backward_condition: %d", 
+			right_forward_condition, left_forward_condition, forward_condition, backward_condition);
+        bool displace_forward_condition = (abs(cardData->Displacement_X) > DISPLACE_MENT_MOVING_THRESHOLD) &&(abs(cardData->Displacement_Y) < DISPLACE_MENT_MOVING_THRESHOLD) && (cardData->Displacement_X > 0);
+        bool displace_backward_condition = (abs(cardData->Displacement_X) > DISPLACE_MENT_MOVING_THRESHOLD) && (abs(cardData->Displacement_Y) < DISPLACE_MENT_MOVING_THRESHOLD) && (cardData->Displacement_X < 0);
+        bool displace_left_forward_condition      = (abs(cardData->Displacement_Y) > DISPLACE_MENT_MOVING_THRESHOLD) && (abs(cardData->Displacement_X) < DISPLACE_MENT_MOVING_THRESHOLD) && (cardData->Displacement_Y < 0);
+        bool displace_right_forward_condition     = (abs(cardData->Displacement_Y) > DISPLACE_MENT_MOVING_THRESHOLD) && (abs(cardData->Displacement_X) < DISPLACE_MENT_MOVING_THRESHOLD) && (cardData->Displacement_Y > 0);
+
+		debug_print_syslog(0, "displacement right_forward_condition: %d, displacement left_forward_condition: %d, displacement forward_condition: %d, displacement backward_condition: %d", 
+			displace_right_forward_condition, displace_left_forward_condition, displace_forward_condition, displace_backward_condition);
         if(right_forward_condition)
         {
             state.Directory = INAV_RIGHTWARD;
@@ -2856,6 +2993,45 @@ INAV_STATUS CYAServerDlg::calculate_inav_state(INAVLocateData* cardData)
     return state;
 }
 
+void CYAServerDlg::calculate_inav_locate_by_magic(std::shared_ptr<Card> card, INAVLocateData* cardData)
+{
+    int magic_x = cardData->MagneticData_X;
+	int magic_y = cardData->MagneticData_Y;
+	int magic_z = cardData->MagneticData_Z;
+    KNN knn(magic_x, magic_y, magic_z);  
+    knn.get_all_distance();  
+    tLabel tmp_lable = knn.get_max_freq_label();
+	debug_print_syslog(0, "magic__x: %10d, magic_y: %10d, magic_z: %10d", magic_x, magic_y, magic_z);
+	debug_print_syslog(0, "magic_position_x: %10d, magic_position_y: %10d", tmp_lable.x, tmp_lable.y);
+    return;
+}
+
+void CYAServerDlg::write_inav_locate_magic_data(std::shared_ptr<Card> card, INAVLocateData* cardData)
+{
+    csv::Parser file = csv::Parser("d:/magic_map/trainning_data_magic.csv");  
+	vector<string> tmpRow;
+	int magic_x = cardData->MagneticData_X;
+	int magic_y = cardData->MagneticData_Y;
+	int magic_z = cardData->MagneticData_Z;
+	int x = card->x;
+	int y = card->y;
+	char str[10];
+    sprintf_s(str,"%d",magic_x);
+	tmpRow.push_back(str);
+	sprintf_s(str,"%d",magic_y);
+	tmpRow.push_back(str);
+	sprintf_s(str,"%d",magic_z);
+	tmpRow.push_back(str);
+	sprintf_s(str,"%d",x);
+	tmpRow.push_back(str);
+	sprintf_s(str,"%d",y);
+	tmpRow.push_back(str);
+
+	file.addRow(0, tmpRow);
+	file.sync();
+	return;
+}
+
 void CYAServerDlg::log_card_inav_state(INAV_STATUS card_state, string card_id)
 {
 	char card_log_str[2000] = {'\0'};
@@ -5285,6 +5461,10 @@ void CYAServerDlg::card_enter_area(std::shared_ptr<Card> card /* Card* card*/)
 	if(!b_in_area){ 
 		// 找不到区域，使用分站所属区域
 		//从区域列表中查找卡所属分站的区域ID
+		if(card->x == 0 || card->y == 0 || card->z == 0)
+		{
+			return;
+		}
 		AreaMap::iterator it_area_reader = mp_area_list.find(card->p_reader->area_id);
 		if(it_area_reader == mp_area_list.end()){
 			return;

YAServerDlg.h

@@ -323,6 +323,8 @@ public:
     int parse_inav_msg(INAVLocateMsg* msgPtr);
 	void deal_inav_locate_msg(INAVLocateMsg* msgPtr, int card_num);
 	INAV_STATUS calculate_inav_state(INAVLocateData* cardData);
+	void calculate_inav_locate_by_magic(std::shared_ptr<Card> card, INAVLocateData* cardData);
+	void write_inav_locate_magic_data(std::shared_ptr<Card> card, INAVLocateData* cardData);
     void log_card_inav_state(INAV_STATUS card_state, string card_id);
 	void parse_data_lights_state(BYTE* DataBuffer, int& nCurPos, int reader_id, unsigned short wChr);
 	void parse_data_locate_card(BYTE* DataBuffer, int& nCurPos, int reader_id, unsigned short wChr);

uwb_msg_def.h

@@ -204,6 +204,7 @@ typedef struct
 #define INAV_READING_MAX      32768
 
 const double DISPLACEMENT_THRESHOLD = 100;
-const double JOIN_ACCERATE_THRESHOLD = 0.15;
+const double JOIN_ACCERATE_THRESHOLD = 0.3;
 const double MOVING_THRESHOLD = 1.2;
+const int DISPLACE_MENT_MOVING_THRESHOLD = 45;
 #endif