no ins

Filter/KalmanFilter.cpp

@@ -57,6 +57,11 @@ void CKalmanFilter::Initial(double t)
 	m_pCar->H.setIdentity(OBSERVATION_NUMS, OBSERVATION_NUMS);
 	m_pCar->z.setZero(OBSERVATION_NUMS, 1);
 	m_pCar->R.setIdentity(OBSERVATION_NUMS, OBSERVATION_NUMS);
+	//ÒÔÏÂ4¸ö²ÎÊýÓÃÓڹߵ¼
+	/*m_pCar->Q(1,0) = 100;
+	m_pCar->Q(3,2) = 100;
+	m_pCar->R(1,1) = 0.1;
+	m_pCar->R(3,3) = 0.1;*/
 	m_pCar->t = 0;
 }
 

YAServer.h

@@ -12,7 +12,6 @@
 #include <string>
 using namespace std;
 
-
 // CYAServerApp:
 // 有关此类的实现，请参阅 YAServer.cpp
 //

YAServerDlg.cpp

@@ -128,6 +128,7 @@ BEGIN_MESSAGE_MAP(CYAServerDlg, CDialogEx)
 	ON_MESSAGE(WM_CARD_ADD_DIST, &CYAServerDlg::OnCardAddDist)
 	ON_MESSAGE(WM_REMOVE_SOCKET, &CYAServerDlg::OnRemoveSocket)
 	ON_EN_UPDATE(IDC_EDIT_READERID, &CYAServerDlg::OnEnUpdateEditReaderid)
+	ON_COMMAND(ID_ABOUT, &CYAServerDlg::OnAbout)
 END_MESSAGE_MAP()
 
 // CYAServerDlg 消息处理程序
@@ -504,7 +505,7 @@ HCURSOR CYAServerDlg::OnQueryDragIcon()
 */
 void CYAServerDlg::OnBnStart()
 {
-	Test();
+	//Test();
 	if(!is_websocket_login){
 		ws_login();
 	}
@@ -899,11 +900,11 @@ int CYAServerDlg::init_vehicle(string vehicle_id /* = "" */, bool is_card /*= fa
 }
 
 /*
-* 初始化所有车卡信息
+* 初始化所有自组网卡信息
 *
 * param
-*		vehicle_id ------	车卡id
-*      is_card    ------	是否是卡
+*		adhoc_id   ------	自组网卡id
+*       is_card    ------	是否是卡
 *
 * return
 *		成功返回0，失败返回1
@@ -920,14 +921,59 @@ int CYAServerDlg::init_adhoc(string adhoc_id /*= ""*/,bool is_card /*= false*/)
 	MYSQL_ROW pRow;
 	MYSQL_RES *pRes = NULL;
 	int err = 0;
-	char strsql[LENGTH_SQL] = {0};
-
-	if(adhoc_id == ""){
-		sprintf_s(strsql,"select adhoc_id,name,card_id from dat_adhoc where state==0");
+	char buf[LENGTH_SQL] = {0};
+	if(adhoc_id.length() == 0){
+		sprintf_s(buf,"select adhoc_id,v.card_id,v.dept_id, \
+					     d.name as dname \
+						 from dat_adhoc v \
+						 LEFT JOIN dat_card c ON v.card_id = c.card_id \
+						 LEFT JOIN dat_dept d ON v.dept_id = d.dept_id \
+						 where c.card_type_id = 3 and v.state = 0 ;");
+	}else if(is_card){
+		sprintf_s(buf,"select adhoc_id,v.card_id,v.dept_id, \
+						 d.name as dname \
+						 from dat_adhoc v \
+						 LEFT JOIN dat_card c ON v.card_id = c.card_id \
+						 LEFT JOIN dat_dept d ON v.dept_id = d.dept_id \
+						 where c.card_type_id = 3 and v.state = 0 and v.card_id ='%s';",adhoc_id.c_str());
 	}else{
-		sprintf_s(strsql,"select adhoc_id,name,card_id from dat_adhoc where state==0 and adhoc_id =='%s'",adhoc_id);
+		sprintf_s(buf,"select adhoc_id,v.card_id,v.dept_id, \
+					     d.name as dname \
+						 from dat_adhoc v \
+						 LEFT JOIN dat_card c ON v.card_id = c.card_id \
+						 LEFT JOIN dat_dept d ON v.dept_id = d.dept_id \
+						 where c.card_type_id = 3 and v.state = 0 and v.adhoc_id ='%s';",adhoc_id.c_str());
 	}
 
+	pRes = pConn->Execute(buf, err);
+
+	if(pRes == NULL){
+		return 1;
+	}
+
+	Card* pCard = NULL;
+	int nRow = 0;
+	while(pRow = mysql_fetch_row(pRes)){
+		CardMap::iterator it = mp_card_list_all.find(pRow[1]);
+		if(it != mp_card_list_all.end()){
+			pCard = it->second;
+		}else{
+			pCard = new Card(pRow[1], CT_ADHOC, theApp.z_offset);
+			mp_card_list_all.insert(make_pair(pCard->card_id, pCard));
+		}
+		pCard->id = (pRow[0] == NULL) ? "" : pRow[0];
+		pCard->card_id = (pRow[1] == NULL) ? "" : pRow[1];
+		pCard->dept_id = (pRow[2] == NULL) ? 0:atoi(pRow[2]);
+		pCard->department = (pRow[3] == NULL) ? "" : pRow[3];
+		nRow++;
+	}
+	mysql_free_result(pRes);
+
+	CString strlog;
+	strlog.Format(_T("%s%d%s"), _T(LOG_INIT_ADHOC), nRow, _T(LOG_RECORD_COUNT));
+	show_log(strlog);
+
+
 	return 0;
 }
 
@@ -1160,15 +1206,20 @@ int CYAServerDlg::init_dev_adhoc( int reader_id /*= 0*/ )
 	int err = 0;
 	char strsql[LENGTH_SQL] = {0};	
 	if(reader_id == 0){//dat_dev_adhoc
-		sprintf_s(strsql, "select ad.adhoc_id, ad.reader_id, ad.idx, ad.x, ad.y, ad.z from dat_adhoc ad, dat_reader r \
+		sprintf_s(strsql, "select ad.id, ad.reader_id, ad.level, ad.x, ad.y, ad.z from dat_dev_adhoc ad, dat_reader r \
 						  where ad.reader_id = r.reader_id;");
 	}else{
-		sprintf_s(strsql, "select ad.adhoc_id, ad.reader_id, ad.idx, ad.x, ad.y, ad.z from dat_adhoc ad, dat_reader r \
+		sprintf_s(strsql, "select ad.id, ad.reader_id, ad.level, ad.x, ad.y, ad.z from dat_dev_adhoc ad, dat_reader r \
 						  where ad.reader_id = r.reader_id and ad.reader_id = %d; ", reader_id);
 	}
 	pRes = pConn->Execute(strsql,err);
+
+	if(pRes == NULL){
+		return 1;
+	}
 	int nRow = 0;
 	int idx = 0;
+
 	while(pRow = mysql_fetch_row(pRes)){
 		ReaderMap::iterator it = mp_reader_list.find(atoi(pRow[1]));
 		if(it != mp_reader_list.end()){
@@ -1186,7 +1237,7 @@ int CYAServerDlg::init_dev_adhoc( int reader_id /*= 0*/ )
 	mysql_free_result(pRes);
 
 	CString strlog;
-	strlog.Format(_T("%s%d%s"), _T(LOG_INIT_ADHOC), nRow, _T(LOG_RECORD_COUNT));
+	strlog.Format(_T("%s%d%s"), _T(LOG_INIT_ADHOC_LOCATE), nRow, _T(LOG_RECORD_COUNT));
 	show_log(strlog);
 	return 0;
 }
@@ -1605,7 +1656,6 @@ void CYAServerDlg::ws_init()
 	is_websocket_login = false;
 
 	_io->set_reconnect_attempts(3);
-
 	using std::placeholders::_1;
 	using std::placeholders::_2;
 	using std::placeholders::_3;
@@ -2190,6 +2240,12 @@ void CYAServerDlg::parse_data_locate_card_vehicle(BYTE* DataBuffer, int& nCurPos
 		nCurPos += sizeof(WORD);
 	}
 
+#ifdef ALGORITHM_TYPE_INS
+	if(card->accelerate_state != btAcceleration){
+		//如果加速度状态发生了改变，就重置惯导的权重值
+		card->ins_weight = INS_WEIGHT;
+	}
+#endif	
 	if(LT_COORDINATE == theApp.locate_type){
 		card->time_stamp = wCardTickCount;
 		card->ranging_type = btType;
@@ -2271,14 +2327,17 @@ void CYAServerDlg::parse_data_adhoc( BYTE* DataBuffer, int& nCurPos, int reader_
 			CardMap::iterator it_card = mp_card_list_all.find(str_card_id);
 			if(it_card != mp_card_list_all.end()){
 				card = it_card->second;
-
 			}else{
 				card = new Card(str_card_id, CT_ADHOC, theApp.z_offset);
 				mp_card_list_all.insert(make_pair(str_card_id, card));
 			}
 			CardMap::iterator it_adhoc_card = mp_card_list_down_adhoc.find(str_card_id);
 			if(it_adhoc_card==mp_card_list_down_adhoc.end()&&it_card!=mp_card_list_all.end()){
-				mp_card_list_down_adhoc.insert(make_pair(str_card_id, it_card->second));
+				if(card == NULL){
+					mp_card_list_down_adhoc.insert(make_pair(str_card_id, it_card->second));
+				}else{
+					mp_card_list_down_adhoc.insert(make_pair(str_card_id, card));
+				}
 			}
 			//设置当前分站， 进入分站时间、进入区域时间，当前区域，当前地图
 			card->set_reader(it->second);
@@ -2294,7 +2353,6 @@ void CYAServerDlg::parse_data_adhoc( BYTE* DataBuffer, int& nCurPos, int reader_
 			if(btCmd & 0x01){ // 呼救
 				card->status_help = STATUS_ERROR;
 			}else if(btCmd & 0x02){ // 呼叫应答，即呼叫成功
-				TRACE("ANS!");
 				if(STATUS_NORMAL == card->status_call){
 					card->state_biz = STATUS_NORMAL;
 				}
@@ -2308,58 +2366,6 @@ void CYAServerDlg::parse_data_adhoc( BYTE* DataBuffer, int& nCurPos, int reader_
 			//}
 			deal_card_msg(card);
 		}
-
-		//BYTE btDiret = (btCmd >> 6);
-		//if(btDiret == 0x02){ // 只处理上传数据，0x00、0x03 保留，0x01下发数据，0x02上传数据
-		//	btCmd &= 0x3F;  // 高两位为命令传递方向，上位机无用
-		//	BYTE btId = 0;
-		//	memcpy(&btId, &DataBuffer[nCurPos], sizeof(BYTE));
-		//	nCurPos += sizeof(BYTE);
-		//	BYTE btTickcount = 0;
-		//	memcpy(&btTickcount, &DataBuffer[nCurPos], sizeof(BYTE));
-		//	nCurPos += sizeof(BYTE);
-		//	BYTE btIdx = 0;
-		//	memcpy(&btIdx, &DataBuffer[nCurPos], sizeof(BYTE));
-		//	nCurPos += sizeof(BYTE);
-
-		//	if(btId != 0){
-		//		string str_card_id = CFunctions::getstrwithzero(CT_ADHOC, 3) + CFunctions::getstrwithzero(btId, 10);
-		//		Card* card = NULL;
-		//		CardMap::iterator it_card = mp_card_list_all.find(str_card_id);
-		//		if(it_card != mp_card_list_all.end()){
-		//			card = it_card->second;
-		//		}else{
-		//			card = new Card(str_card_id, CT_ADHOC, theApp.z_offset);
-		//			mp_card_list_all.insert(make_pair(str_card_id, card));
-		//		}
-		//		//设置当前分站， 进入分站时间、进入区域时间，当前区域，当前地图
-		//		card->set_reader(it->second);
-		//		if(it->second->adhoc[btIdx]){
-		//			card->x = it->second->adhoc[btIdx]->x;
-		//			card->y = it->second->adhoc[btIdx]->y;
-		//			card->z = it->second->adhoc[btIdx]->z;
-		//		}
-
-		//		if(btCmd & 0x01){ // 呼救
-		//			card->state_biz = STATUS_HELP;
-		//		}else if(btCmd & 0x02){ // 呼叫应答，即呼叫成功
-		//			TRACE("ANS!");
-		//			if(STATUS_NORMAL == card->status_call){
-
-		//			}
-		//		}else {
-		//			card->status_call = STATUS_NORMAL;
-		//			card->status_help = STATUS_NORMAL;
-		//		}
-		//		card->issent = false;
-		//		//if(card->status_help != btCmd){
-		//		//	card->status_help = btCmd;
-		//		//}
-		//		deal_card_msg(card);
-		//	}
-		//}else{
-		//	nCurPos += 3 * sizeof(BYTE);
-		//}
 	}
 }
 
@@ -2786,6 +2792,7 @@ std::string CYAServerDlg::get_json_help()
 {
 	Json::Value root;
 	Json::Value alarms;
+	Json::Value adhoc;
 	char _time[STR_LEN_TIME];
 	struct tm local_time;
 
@@ -2805,10 +2812,11 @@ std::string CYAServerDlg::get_json_help()
 		}
 		if(!alarm.isNull()){
 			//alarms.append(alarm);
-			alarms=alarm;
-
+			//alarms=alarm;
+			alarms.append(alarm);
 			string sss = "";
 			sss = alarms.toStyledString();
+			//adhoc.append(alarms);
 		}
 	}
 	if(!alarms.isNull()){
@@ -3440,10 +3448,12 @@ void CYAServerDlg::card_enter_park( Card* card )
 {
 	if(card->is_pos_state_pack_changed(TEST_CONFIRM_COUNT)){ // 位置没有变化
 		if(AAT_ENTER == card->pos_state_park){ //
-			card->up_time = card->deal_time;	
+			//card->up_time = card->deal_time;	
+			card->up_time = CFunctions::systime_to_timet(card->deal_time);
 			store_data_card(card, RPT_ATTEND_UP);
 		}else if(AAT_LEAVE == card->pos_state_park){ // 入井
-			card->down_time = card->deal_time;
+			//card->down_time = card->deal_time;
+			card->down_time = CFunctions::systime_to_timet(card->deal_time);
 			store_data_card(card, RPT_ATTEND_DOWN);
 		}
 	}
@@ -3507,7 +3517,8 @@ void CYAServerDlg::card_enter_area( Card* card)
 					card_leave_area(card, it_area_cur->second);
 				}
 				card->area_id = it_area->second->area_id;
-				card->enter_area_time = card->deal_time;
+				//card->enter_area_time = card->deal_time;
+				card->enter_area_time = CFunctions::systime_to_timet(card->deal_time);
 				card->pos_state = (0 == it_area->second->area_type_id) ? DT_UP :DT_DOWN;
 				card->pos_state_park = it_area->second->is_att;
 
@@ -3547,7 +3558,8 @@ void CYAServerDlg::card_enter_area( Card* card)
 			card_leave_area(card, it_area_cur->second);
 		}
 		card->area_id = it_area_reader->second->area_id;
-		card->enter_area_time = card->deal_time;
+		//card->enter_area_time = card->deal_time;
+		card->enter_area_time = CFunctions::systime_to_timet(card->deal_time);
 		card->pos_state = (0 == it_area_reader->second->area_type_id) ? DT_UP :DT_DOWN;
 		card->pos_state_park = it_area_reader->second->is_att;
 		if(CT_PERSON == card->card_type){
@@ -3836,11 +3848,17 @@ void CYAServerDlg::store_data_card( Card* card, int tag )
 	char sql[LENGTH_SQL] = {0};
 	char _time[STR_LEN_TIME], _time_ex[STR_LEN_TIME];
 	struct tm local_time, local_time_ex;
-	localtime_s(&local_time, &card->deal_time);
-	strftime(_time, 30, "%Y-%m-%d %H:%M:%S", &local_time);
+	//localtime_s(&local_time, &card->deal_time);
+	//strftime(_time, 30, "%Y-%m-%d %H:%M:%S", &local_time);
 
 	bool b_exec = false;
 
+	sprintf_s(_time,
+		STR_LEN_TIME,
+		"%u-%u-%u %u:%u:%u.%u",
+		card->deal_time.wYear,card->deal_time.wMonth,card->deal_time.wDay,
+		card->deal_time.wHour + 8,card->deal_time.wMinute,card->deal_time.wSecond,card->deal_time.wMilliseconds);
+
 	switch(tag){
 	case TEST_LOCATE_COMPARE:
 		{
@@ -3924,6 +3942,7 @@ void CYAServerDlg::store_data_card( Card* card, int tag )
 	case RPT_ATTEND_UP:
 		{
 			if(card->map_id != 0 && card->area_id != 0){
+				TRACE(_T(" 6 "));
 				b_exec = true;
 				localtime_s(&local_time_ex, &card->down_time);
 				strftime(_time_ex, 30, "%Y-%m-%d %H:%M:%S", &local_time_ex);
@@ -4593,12 +4612,6 @@ void CYAServerDlg::parse_json_data_call_card(message::ptr const& data /*const ch
 
 	map<string,message::ptr> data_map = data->get_map()[JSON_ROOT_KEY_DATA]->get_map();
 
-	/*if(m_log_recv_package){
-		string s_log = "";
-		s_log = data->get_string();
-		writeErrorLog(_T("JSON_R"),CFunctions::c2wc(s_log.c_str()),false);
-	}*/
-
 	call_id = data_map[JSON_KEY_CALL_CARD_CALL_ID]->get_string();
 	call_time_out = data_map[JSON_KEY_CALL_CARD_CALL_TIME_OUT]->get_int();
 	call_type_id = data_map[JSON_KEY_CALL_CARD_CALL_TYPE]->get_int();
@@ -4722,6 +4735,8 @@ void CYAServerDlg::parse_json_data_call_card(message::ptr const& data /*const ch
 			if(it_call_reader != mp_call_info.end()){
 				call_reader = it_call_reader->second;	
 				call_reader->is_start_call = true;
+				call_reader->is_call_all = false;
+				call_reader->cards_count = 1;
 				if(call_reader->call_type > call_type_id){
 					call_reader->call_type = call_type_id;
 				}
@@ -4993,6 +5008,8 @@ void CYAServerDlg::deal_call_edit_card(string id, EDIT_TYPE_ID edit_type_id)
 				init_vehicle(id, true);
 			}else if(CT_PERSON == card_type_id) {
 				init_staffer(id, true);
+			}else if(CT_ADHOC == card_type_id){
+				init_adhoc(id, true);
 			}
 			break;
 		}
@@ -5002,6 +5019,8 @@ void CYAServerDlg::deal_call_edit_card(string id, EDIT_TYPE_ID edit_type_id)
 				init_vehicle(id, true);
 			}else if(CT_PERSON == card_type_id) {
 				init_staffer(id, true);
+			}else if(CT_ADHOC == card_type_id){
+				init_adhoc(id, true);
 			}
 			break;
 		}
@@ -5087,6 +5106,41 @@ void CYAServerDlg::deal_call_edit_vehicle(string id, EDIT_TYPE_ID edit_type_id)
 	show_log(strlog);
 }
 
+void CYAServerDlg::deal_call_edit_adhoc(string id, EDIT_TYPE_ID edit_type_id)
+{
+	CString strlog;
+	switch (edit_type_id)
+	{
+	case ET_INSERT:
+		{
+			init_adhoc(id);
+			break;
+		}
+	case ET_UPDATE:
+		{
+			init_adhoc(id);
+			break;
+		}
+	case ET_DELETE:
+		{
+			string cardid = "";
+			CardMap::iterator it_card = mp_card_list_down_adhoc.begin();
+			for(; it_card != mp_card_list_down_adhoc.end(); ++it_card){
+				if(it_card->second->id == id){
+					cardid = it_card->second->card_id;
+					break;
+				}
+			}
+			remove_card(cardid);
+			break;
+		}
+	default:
+		break;
+	}
+	strlog.Format(_T("%s%s"), _T(LOG_EDIT_ADHOC), _T(LOG_SUCCESS));
+	show_log(strlog);
+}
+
 void CYAServerDlg::remove_card( string card_id )
 {
 	if(card_id.length() == 0)
@@ -5526,6 +5580,7 @@ void CYAServerDlg::init_base_data()
 	init_card();
 	init_staffer();
 	init_vehicle();
+	init_adhoc();
 }
 
 void CYAServerDlg::load_his_data()
@@ -5773,7 +5828,8 @@ int CYAServerDlg::init_card( string card_id /*= ""*/ )
 
 void CYAServerDlg::card_up_mine( Card* card )
 {
-	card->up_time = card->deal_time;
+	//card->up_time = card->deal_time;
+	card->up_time = CFunctions::systime_to_timet(card->deal_time);
 	store_data_card(card, RPT_ATTEND_UP);
 	if(CT_PERSON == card->card_type){
 		CardMap::iterator it_card = mp_card_list_down_person.find(card->card_id);
@@ -5822,7 +5878,8 @@ void CYAServerDlg::card_up_mine( Card* card )
 
 void CYAServerDlg::card_down_mine( Card* card )
 {
-	card->down_time = card->deal_time;
+	//card->down_time = card->deal_time;
+	card->down_time = CFunctions::systime_to_timet(card->deal_time);
 	if(CT_PERSON == card->card_type){
 		CardMap::iterator it_card = mp_card_list_down_person.find(card->card_id);
 		if(it_card == mp_card_list_down_person.end()){
@@ -6115,7 +6172,8 @@ void CYAServerDlg::deal_card_state( Card* card )
 		store_data_card(card, ALARM_CARD_LOW_POWER_END);
 	}
 	if(card->power_state_last == 0 && card->power_state != 0){
-		card->low_power_time = card->deal_time;
+		//card->low_power_time = card->deal_time;CFunctions::systime_to_timet(card->deal_time)
+		card->low_power_time = CFunctions::systime_to_timet(card->deal_time);
 		store_data_card(card, ALARM_CARD_LOW_POWER_START);
 	}
 	card->power_state_last = card->power_state;
@@ -6552,52 +6610,52 @@ void CYAServerDlg::Test()
 
 	//34
 	ReceiveData* prd2 = new ReceiveData();	
-	prd2->reader_id = 218;
+	prd2->reader_id = 217;
 	prd2->antenna_id = 1;
-	prd2->rec_time_stamp = 60380465780;//283613853712;//
-	prd2->x = 164.0*dMapScale;
+	prd2->rec_time_stamp = 777596169634;//283613853712;//
+	prd2->x = 210.1*dMapScale;
 	prd2->y = 449.5*dMapScale;
 	prd2->z = 0;
-	prd2->special = 0;
+	prd2->special = 1;
 	pRdm.insert(ReceiveDataMap::value_type(prd2->reader_id,*prd2));	
 
 	//35
 	ReceiveData* prd3 = new ReceiveData();	
-	prd3->reader_id = 217;
+	prd3->reader_id = 218;
 	prd3->antenna_id = 1;
-	prd3->rec_time_stamp = 60380488541;//283613846609;
-	prd3->x = 210.1*dMapScale;
+	prd3->rec_time_stamp = 777596203395;//283613846609;
+	prd3->x = 164.0*dMapScale;
 	prd3->y = 449.5*dMapScale;
 	prd3->z = 0;
 	prd2->special = 0;
 	pRdm.insert(ReceiveDataMap::value_type(prd3->reader_id,*prd3));
 
 	//36
-	//ReceiveData* prd1 = new ReceiveData();	
-	//prd1->reader_id = 214;
-	//prd1->antenna_id = 1;
-	//prd1->rec_time_stamp = 464710764016;//283613846791;
-	//prd1->x = 328.7*dMapScale;
-	//prd1->y = 449.5*dMapScale;
-	//prd1->z = 0;
-	//prd1->special = 0;
-	//pRdm.insert(ReceiveDataMap::value_type(prd1->reader_id,*prd1));
-
-	//ReceiveData* prd4 = new ReceiveData();	
-	//prd4->reader_id = 213;
-	//prd4->antenna_id = 1;
-	//prd4->rec_time_stamp = 464710783767;//283613846609;
-	//prd4->x = 356.3*dMapScale;
-	//prd4->y = 449.5*dMapScale;
-	//prd4->z = 0;
-	//pRdm.insert(ReceiveDataMap::value_type(prd4->reader_id,*prd4));
+	ReceiveData* prd1 = new ReceiveData();	
+	prd1->reader_id = 216;
+	prd1->antenna_id = 1;
+	prd1->rec_time_stamp = 777596203705;//283613846791;
+	prd1->x = 242.8*dMapScale;
+	prd1->y = 449.5*dMapScale;
+	prd1->z = 0;
+	prd1->special = 0;
+	pRdm.insert(ReceiveDataMap::value_type(prd1->reader_id,*prd1));
+
+	ReceiveData* prd4 = new ReceiveData();	
+	prd4->reader_id = 215;
+	prd4->antenna_id = 1;
+	prd4->rec_time_stamp = 777596242277;//283613846609;
+	prd4->x = 296.4*dMapScale;
+	prd4->y = 449.5*dMapScale;
+	prd4->z = 0;
+	pRdm.insert(ReceiveDataMap::value_type(prd4->reader_id,*prd4));
 
 	//ReceiveData* prd5 = new ReceiveData();	
-	//prd5->reader_id = 205;
+	//prd5->reader_id = 214;
 	//prd5->antenna_id = 1;
-	//prd5->rec_time_stamp = 630968206079;//283613846609;
-	//prd5->x = 804.000000*dMapScale;
-	//prd5->y = 285.200000*dMapScale;
+	//prd5->rec_time_stamp = 277993299781;//283613846609;
+	//prd5->x = 328.7*dMapScale;
+	//prd5->y = 449.5*dMapScale;
 	//prd5->z = 0;
 	//pRdm.insert(ReceiveDataMap::value_type(prd5->reader_id,*prd5));
 
@@ -6633,13 +6691,124 @@ void CYAServerDlg::release_memory()
 		delete[] theApp.m_path_monitor;
 		theApp.m_path_monitor = NULL;
 	}
+
+	int nSize = 0;
+
 	//mp_area_list
+	release_area_memory();
 	//mp_map_list
+	release_map_memory();
 	//mp_card_list_all
+	release_card_memory();
 	//mp_reader_list
+	release_reader_memory();
+	//mp_dept_list
 	release_dept_memory();
 
-	delete m_qsmSQL;
+	if(m_qsmSQL){
+		delete m_qsmSQL;
+		m_qsmSQL = NULL;
+	}
+}
+
+void CYAServerDlg::release_area_memory()
+{
+	if(mp_area_list.size() > 0){
+		AreaMap::iterator it = mp_area_list.begin();
+		for(;it!=mp_area_list.end();){
+			Area* tmp = (Area*)(it->second);
+			mp_area_list.erase(it);
+			if(tmp){
+
+				/*if(tmp->polygon){
+					delete[] tmp->polygon;
+					tmp->polygon = NULL;
+				}*/
+
+				delete tmp;
+				tmp = NULL;
+			}
+			it = mp_area_list.begin();
+		}
+	}
+}
+
+void CYAServerDlg::release_map_memory()
+{
+	if(mp_map_list.size() > 0 ){
+		MapInfoMap::iterator it = mp_map_list.begin();
+		for(it;it!=mp_map_list.end();){
+			MapInfo* tmp = (MapInfo*)(it->second);
+			mp_map_list.erase(it);
+			if(tmp){
+				delete tmp;
+				tmp = NULL;
+			}
+			it = mp_map_list.begin();
+		}
+	}
+}
+
+void CYAServerDlg::release_card_memory()
+{
+	if(mp_card_list_all.size()>0){
+		CardMap::iterator it = mp_card_list_all.begin();
+		for(;it!=mp_card_list_all.end();){
+			Card* tmp = (Card*)(it->second);
+			mp_card_list_all.erase(it);
+			if(tmp){
+				delete tmp;
+				tmp = NULL;
+			}
+			it = mp_card_list_all.begin();
+		}
+	}
+}
+
+void CYAServerDlg::release_reader_memory()
+{
+	if(mp_reader_list.size() > 0 ){
+		ReaderMap::iterator it = mp_reader_list.begin();
+		for(;it!=mp_reader_list.end();){
+			Reader* tmp = (Reader*)(it->second);
+
+			mp_reader_list.erase(it);
+			if(tmp){
+				delete tmp;
+				tmp = NULL;
+			}
+
+			it = mp_reader_list.begin();
+		}
+	}
+
+	if(mp_reader_path_list_tdoa.size() > 0 ){
+		TDOAReaderPathMap::iterator it = mp_reader_path_list_tdoa.begin();
+		for(;it != mp_reader_path_list_tdoa.end();){
+			ReaderPathMap* tmpMap = (ReaderPathMap*)(it->second);
+
+			for(ReaderPathMap::iterator it_r = tmpMap->begin();it_r != tmpMap->end();){
+				ReaderPath* tmpPath = (ReaderPath* )(it_r->second);
+				tmpMap->erase(it_r);
+
+				if(tmpPath){
+					delete tmpPath;
+					tmpPath = NULL;
+				}
+
+				it_r = tmpMap->begin();
+			}
+
+			mp_reader_path_list_tdoa.erase(it);
+
+			if(tmpMap){
+				delete tmpMap;
+				tmpMap = NULL;
+			}
+
+			it = mp_reader_path_list_tdoa.begin();
+		}
+	}
 }
 
 /*
@@ -6858,6 +7027,7 @@ void CYAServerDlg::deal_card_msg( Card* card, bool is_hist /*= false*/ )
 			dist->d_offset = 0;
 			dist->special = card->p_reader->m_nIsSpecial;
 			dist->st = card->sync_num;
+			dist->acce_state = card->accelerate_state;
 
 			unsigned short root_sync_num = 0;
 			bool bUseAlgo = false;
@@ -6892,6 +7062,7 @@ void CYAServerDlg::deal_card_msg( Card* card, bool is_hist /*= false*/ )
 				}
 
 				EnterCriticalSection(&m_csAddDist);
+				card->is_deal_by_algo = false;
 				card->add_dist(dist);
 				LeaveCriticalSection(&m_csAddDist);
 			}
@@ -6919,8 +7090,15 @@ void CYAServerDlg::deal_card_msg( Card* card, bool is_hist /*= false*/ )
 					strLog.Empty();
 
 					strLog = _T("");
-					strLog.Format(_T("%d,%f,%f,%f,%f,f: %f,s: %f,ct: %d, ov: %f,cv: %f,av: %f, lsn: %d,cardid: %s"),card->m_unCalcSyncNum,card->origin_locate.x,card->origin_locate.y,card->x, card->y,card->m_dFirstDiff,card->m_dSecondDiff,card->time_stamp_cal,card->origin_locate.v,card->v,card->last_locate.acceleration,card->m_unSyncNumInList,p_card_id);
+					strLog.Format(_T("%d,%f,%f,%f,%f,f: %f,s: %f,ct: %d, ov: %f,cv: %f,av: %f, lsn: %d,cardid: %s"),card->m_nCalcSyncNum,card->origin_locate.x,card->origin_locate.y,card->x, card->y,card->m_dFirstDiff,card->m_dSecondDiff,card->time_stamp_cal,card->origin_locate.v,card->v,card->last_locate.acceleration,card->m_nSyncNumInList,p_card_id);
 					writeErrorLog(_T("KALMAN_S"),strLog,false);
+				}else{
+					if(card->is_deal_by_algo){
+						strLog = _T("");
+						strLog.Format(_T("%d,%f,%f,%f,%f,f: %f,s: %f,ct: %d, ov: %f,cv: %f,av: %f, lsn: %d,cardid: %s"),card->m_nCalcSyncNum,card->origin_locate.x,card->origin_locate.y,card->x, card->y,card->m_dFirstDiff,card->m_dSecondDiff,card->time_stamp_cal,card->origin_locate.v,card->v,card->last_locate.acceleration,card->m_nSyncNumInList,p_card_id);
+						writeErrorLog(_T("KALMAN_S"),strLog,false);
+						card->is_deal_by_algo = false;
+					}
 				}
 				if(p_card_id){
 					free(p_card_id);
@@ -6932,12 +7110,15 @@ void CYAServerDlg::deal_card_msg( Card* card, bool is_hist /*= false*/ )
 	if(LT_READER == theApp.locate_type){
 		//TRACE(_T("time memcpy 1\n"));
 		time_t cur_time = time(NULL);
-		if(difftime(cur_time, card->deal_time) <30){
+		time_t c_time = CFunctions::systime_to_timet(card->deal_time);
+		//if(difftime(cur_time, card->deal_time) <30){
+		if(difftime(cur_time, c_time) <30){
 			return ;
 		}
 	}
 	//TRACE(_T("time memcpy 2\n"));
-	card->deal_time = time(NULL);
+	//card->deal_time = time(NULL);
+	GetSystemTime(&card->deal_time);
 	dw = ::GetTickCount();
 	save_card_postion(card);
 	card_enter_map(card); // 切换地图
@@ -7609,4 +7790,11 @@ void CYAServerDlg::parse_json_data_call_card_cancel(message::ptr const& data )
 	LeaveCriticalSection(&m_csCallInfo);
 	strlog.Format(_T("%s%s"), _T(LOG_CANCEL_CALL_CARD), _T(LOG_SUCCESS));
 	show_log(strlog);
-}
+}
+
+void CYAServerDlg::OnAbout()
+{
+	// TODO: 在此添加命令处理程序代码
+	CAboutDlg dlg;
+	dlg.DoModal();
+}

YAServerDlg.h

@@ -378,6 +378,7 @@ public:
 	void deal_call_edit_card(string id, EDIT_TYPE_ID edit_type_id);
 	void deal_call_edit_staff(string id, EDIT_TYPE_ID edit_type_id);
 	void deal_call_edit_vehicle(string id, EDIT_TYPE_ID edit_type_id);
+	void deal_call_edit_adhoc(string id, EDIT_TYPE_ID edit_type_id);
 	void remove_card(string card_id);
 
 	void send_call_info();
@@ -386,6 +387,10 @@ public:
 	void clear_call_info();
 	int get_card_id_len(int card_type);
 	void release_memory();
+	void release_area_memory();
+	void release_map_memory();
+	void release_card_memory();
+	void release_reader_memory();
 	void release_dept_memory();
 	void Test();
 public:
@@ -410,4 +415,5 @@ protected:
 public:
 	UINT m_reader_id_hist;
 	afx_msg void OnEnUpdateEditReaderid();
+	afx_msg void OnAbout();
 };

classdef.cpp

@@ -15,8 +15,9 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 	coor_offset_x = offset_x;
 	coor_offset_y = offset_y;
 	this->z_offset = z_offset;
-
-	down_time = up_time = enter_area_time = enter_reader_time = deal_time = rec_time = time(NULL);
+	
+	GetSystemTime(&deal_time);
+	down_time = up_time = enter_area_time = enter_reader_time = rec_time = time(NULL);
 	time_over_time = time_area_over_time = time_area_forbidden = time_over_speed = time_low_power = time(NULL);
 
 	x =	y = z = last_x = last_y = last_z = stored_x = stored_y = stored_z = 0;
@@ -57,20 +58,17 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 		p_dists[i] = NULL;
 		p_dists_locate[i] = NULL;
 	}
-	//cal_location = new CalLocation(KALMAN_OFFSET_COOR, KALMAN_OFFSET_MOVE, KALMAN_OFFSET_RANGING, KALMAN_INTERVAL);
-	//p2_anchors = new Point2[ANCHOR_COUNT];
-	//cal_location3 = new CalLocation3(KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_COOR_TRI,
-	//	KALMAN_OFFSET_MOVE_TRI, KALMAN_OFFSET_MOVE_TRI, 0, KALMAN_OFFSET_RANGING_TRI, KALMAN_INTERVAL_TRI);
 	cal_location3 = new CalLocation3(KALMAN_OFFSET_COOR_TRI, KALMAN_OFFSET_MOVE_TRI, KALMAN_OFFSET_RANGING_TRI, KALMAN_INTERVAL_TRI);
 	p3_anchors = new Point3[ANCHOR_COUNT];
-	is_init_kalman = false;
+	is_deal_by_algo = false;
 	pReaderPathMap = NULL;
 	pTdoaReaderPathMap = NULL;
 	last_s_locate_reader[0] = last_s_locate_reader[1] = -1;
 	m_syncNumList.clear();
 	m_dFirstDiff = m_dSecondDiff = 0.0;
 
-	m_unCalcSyncNum = 0;
+	m_nCalcSyncNum = 0;
+	m_nSyncNumInList = 0;
 	last_locate.tt = 0;
 	last_locate.a = last_locate.antenna_id = last_locate.reader_id = last_locate.t = 0;
 	last_locate.x = last_locate.y = last_locate.z = last_locate.d = last_locate.d_offset = last_locate.v = 0.0;
@@ -82,6 +80,7 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 	origin_locate.x = origin_locate.y = origin_locate.z = origin_locate.v = 0.0;
 	last_vx = last_vy = 0.0;
 	v = 0.0;
+	ins_weight = uwb_weight = weight = 0;
 
 	time_stamp_last = 0;
 	reader_id_last  = 0;
@@ -529,15 +528,18 @@ void Card::get_coordinate( int cnt )
 
 	bRet = false;
 
-	unsigned long long maxSycnTimes = 0;
+	int maxSyncTimes = 0;
+	int acce_state = 2;
 
 	for(int i=0;i<k;i++){
 		if(i==0){
-			maxSycnTimes = p_dists_locate[i]->st;
+			maxSyncTimes = p_dists_locate[i]->st;
+			acce_state = p_dists_locate[i]->acce_state;
 		}
 		else{
-			if(maxSycnTimes < p_dists_locate[i]->st){
-				maxSycnTimes = p_dists_locate[i]->st;
+			if(maxSyncTimes < p_dists_locate[i]->st){
+				maxSyncTimes = p_dists_locate[i]->st;
+				acce_state = p_dists_locate[i]->acce_state;
 			}
 		}
 		ReceiveDataMap::iterator prdm_it = pRdm.find(p_dists_locate[i]->reader_id);
@@ -560,11 +562,12 @@ void Card::get_coordinate( int cnt )
 		}		
 	}
 
+	acce_state = 2;
 	bool bOutput = false;
 	int nSize = 0;
 
 	if(pTdoaReaderPathMap->size() > 0 && pRdm.size() > 1){
-		this->m_unCalcSyncNum = maxSycnTimes;
+		this->m_nCalcSyncNum = maxSyncTimes;
 		p = LocateAlgorithm::Pos(&pRdm,*pTdoaReaderPathMap);
 
 		this->origin_locate.x = p->posx / (this->map_scale*1.0);
@@ -594,20 +597,31 @@ void Card::get_coordinate( int cnt )
 
 		sync_data sdNew;
 		sync_data sd;
-
+		
 		cx = p->posx / (this->map_scale*1.0);
 		cy = p->posy / (this->map_scale*1.0);
 		cz = p->posz / (this->map_scale*1.0);
+		
+#ifdef ALGORITHM_TYPE_INS
+		int ins_state = 0;
+		int uwb_state = 0;
+		bool bUseKalman = false;
+		bool bOriginLocateState = false;
+#endif
 
 		if(p->posx != INVALID_COORDINATE && p->posy != INVALID_COORDINATE){
+#ifdef ALGORITHM_TYPE_INS
+			bOriginLocateState = true;
+#endif
 			//定位成功
 			if(this->m_nLastLocateT == 0){
-				sd.sync_num = maxSycnTimes;
-				sd.x = cx;
-				sd.y = cy;
-				sd.vx = 0;
-				sd.vy = 0;
-				m_syncNumList.push_back(sd);
+				sdNew.sync_num = maxSyncTimes;
+				sdNew.x = cx;
+				sdNew.y = cy;
+				sdNew.vx = 0;
+				sdNew.vy = 0;
+				sdNew.update = false;
+				m_syncNumList.push_back(sdNew);
 			}
 			else{
 				//现在的关于同步序号的处理是这样的：
@@ -619,91 +633,38 @@ void Card::get_coordinate( int cnt )
 				this->m_dFirstDiff = p->dFirstDiff;
 				this->m_dSecondDiff = p->dSecondDiff;
 
-				//deltaT = (maxSycnTimes - this->m_nLastLocateT)*interval_time;
-				//处理相同同步序号里有两个卡的不同时间戳的情况
-				//if(fabs(deltaT)<1E-5){
-				//	deltaT = 0.2;
-				//}
-				////使用加速度抛掉位置相差过大的数据
-				//if(deltaT > 0){
-				//	cvx = (cx - this->last_locate.x)/deltaT;
-				//	cvy = (cy - this->last_locate.y)/deltaT;
-				//	cvx *= this->map_scale;
-				//	cvy *= this->map_scale;
-
-				//	//速度正负的判断：已x轴，y轴正向运动为正
-				//	//如果x相等，则y2 - y1 > 0为正
-				//	//其他情况，则x2 - x1 > 0 为正
-				//	if(cx == this->last_locate.x){
-				//		if(cy >= this->last_locate.y){
-				//			nSign = 1;
-				//		}else{
-				//			nSign = -1;
-				//		}
-				//	}else{
-				//		if(cx > this->last_locate.x){
-				//			nSign = 1;
-				//		}else{
-				//			nSign = -1;
-				//		}
-				//	}
-
-				//	cv = sqrt(pow(cvx,2) + pow(cvy,2));
-				//	cv = cv*nSign;
-
-				//	double avx = (cvx - this->last_vx) / deltaT;
-				//	double avy = (cvy - this->last_vy) / deltaT;
-				//	double av = sqrt(pow(avx,2) + pow(avy,2));
-				//	//车卡的加速度
-				//	switch(this->card_type){
-				//	case CT_PERSON:
-				//		if(av > 3){
-				//			this->x = this->last_locate.x;
-				//			this->y = this->last_locate.y;
-				//			return;
-				//		}
-				//		break;
-				//	case CT_VEHICLE:
-				//		if(av > 5){
-				//			//保留上次结果
-				//			this->x = this->last_locate.x;
-				//			this->y = this->last_locate.y;
-				//			return;
-				//		}
-				//		break;
-				//	}
-				//}
-
 				//从队列尾部开始查找，找到第一个同步序号与当前计算卡的同步序号相差5个以上的数据
-				list<sync_data>::reverse_iterator it = m_syncNumList.rbegin();			
+				//list<sync_data>::reverse_iterator it = m_syncNumList.rbegin();	
+				list<sync_data>::reverse_iterator it;
 				bool bOverflow = false;
-				for(it;it!=m_syncNumList.rend();++it){
-					if(maxSycnTimes - it->sync_num >= 5){
+				for(it = m_syncNumList.rbegin();it!=m_syncNumList.rend();it++){
+					if(maxSyncTimes - it->sync_num >= 5){
 						sd = *it;
 						break;
 					}
 					else{
-						if(maxSycnTimes - it->sync_num < 0 ){
+						if(maxSyncTimes - it->sync_num < 0 && maxSyncTimes < 100){
 							//如果最新同步号小于列表中的同步号则
-							if(maxSycnTimes + 65536 - it->sync_num >=5 ){
+							if(maxSyncTimes + 65536 - it->sync_num >=5 ){
 								bOverflow = true;
 								sd = *it;
 							}
+						}else{
+							continue;
 						}
 					}
 				}
 				//根据溢出条件来计算deltaT
 				if(bOverflow){
-					deltaT = (maxSycnTimes + 65536 - sd.sync_num)*interval_time;
+					deltaT = (maxSyncTimes + 65536 - sd.sync_num)*interval_time;
 				}else{
-					deltaT = (maxSycnTimes - sd.sync_num)*interval_time;
+					deltaT = (maxSyncTimes - sd.sync_num)*interval_time;
 				}
 				//使用间隔来修正速度
 				if(deltaT - 1.0 >= 0){
-					cvx = (cx - sd.x)/deltaT;
-					cvy = (cy - sd.y)/deltaT;
-					cvx *= this->map_scale;
-					cvy *= this->map_scale;
+					//转为m/s
+					cvx = (cx - sd.x)*this->map_scale/deltaT;
+					cvy = (cy - sd.y)*this->map_scale/deltaT;
 
 					//速度正负的判断：以x轴，y轴正向运动为正
 					//如果x相等，则y2 - y1 > 0为正
@@ -749,7 +710,6 @@ void Card::get_coordinate( int cnt )
 					this->last_locate.acceleration = av;
 					this->last_vx = cvx;
 					this->last_vy = cvy;	
-
 					cv = cv*3.6;
 					
 					//删除第一个元素到tmp（含）之间的所有元素
@@ -768,151 +728,264 @@ void Card::get_coordinate( int cnt )
 
 					//更新值为当前值并插入队列
 					//sync_data sdNew;
-					sdNew.sync_num = maxSycnTimes;
-					this->m_unSyncNumInList = sd.sync_num;
+					sdNew.sync_num = maxSyncTimes;
+					this->m_nSyncNumInList = sd.sync_num;
 					//sdNew.x = cx;
 					//sdNew.y = cy;
 					sdNew.vx = cvx;
 					sdNew.vy = cvy;
 					//m_syncNumList.push_back(sdNew);	
 					sdNew.update = true;
+#ifdef ALGORITHM_TYPE_INS
+					uwb_state = nSign == 1?1:-1;		//uwb形式判断不出静止或者怠速
+#endif
 				}else{
 					//cv = this->last_locate.v;
 					cv = this->origin_locate.v;
+#ifdef ALGORITHM_TYPE_INS
+					uwb_state = this->accelerate_state_last;
+#endif		
 				}
 			}	
+		}	
+#ifdef ALGORITHM_TYPE_INS
+		else{
+			bOriginLocateState = false;
+			uwb_state = this->accelerate_state_last;
 		}
+#endif
 
-		if(m_nFilterType == FILTER_KALMAN){
-			double kalman_detal_t = (maxSycnTimes - this->last_locate.tt)*interval_time;
-
-			//通过卡尔曼滤波处理
-			if(p->posx == INVALID_COORDINATE && p->posy == INVALID_COORDINATE){
-				this->m_pKalmanFilter->m_bFlag = false;	
-				if(this->m_pKalmanFilter->m_nCounts < 3 || this->m_pKalmanFilter->m_pCar->P(0,0) > 2 || kalman_detal_t > 3){
-					//P(0,0):连续时间(大于2s)都定位失败
-					//deltaT>3:距离上次成功定位时间间隔为3s
-					this->x = this->last_locate.x;
-					this->y = this->last_locate.y;
-					this->z = -3;
-					return;
-				}
-				
-				if(this->m_pKalmanFilter->m_nCounts >= 3){
-					//只有三次以上才允许使用kalman滤波以下的函数
-					//this->m_pKalmanFilter->Predict(deltaT);
-					this->m_pKalmanFilter->Predict(kalman_detal_t);
-					this->x = this->m_pKalmanFilter->m_pCar->x(0,0);
-					this->y = this->m_pKalmanFilter->m_pCar->x(2,0);
-					this->z = -4;
-					sdNew.update = true;
-				}		
+#ifdef ALGORITHM_TYPE_INS
+		bRet = false;
+		switch(acce_state){
+		case 0:	//熄火
+		case 1:	//怠速
+			//如果是熄火或者怠速状态，则将x,y方向的速度置为0，并送到卡尔曼滤波中
+			cvx = 0;
+			cvy = 0;
+			bRet = true;
+			break;
+		case 2:	//前进
+			//如果根据加速度状态判断为前进，则取1
+			ins_state = 1;
+			break;
+		case 3:	//倒退
+			//如果根据加速度状态判断为后退，则取-1
+			ins_state = -1;
+			break;
+		}
+		if(!bRet){
+			double cweight = 0;
+			cweight = this->ins_weight * ins_state + this->uwb_weight*uwb_state;
+			//如果计算出的权重在合适范围内，就逐渐降低惯导的权限
+			//如果计算出的权重超过范围，则重置惯导的权重为90%
+			if(cweight>=INS_WEIGHT*-1&&cweight<=INS_WEIGHT){
+				this->ins_weight = abs(cweight);
 			}else{
-				this->m_pKalmanFilter->m_bFlag = true;
-				this->m_pKalmanFilter->m_nCounts++;
+				this->ins_weight = INS_WEIGHT;
+			}
 
-				this->m_pKalmanFilter->m_pCar->z(0,0) = cx;
-				this->m_pKalmanFilter->m_pCar->z(1,0) = cvx;
-				this->m_pKalmanFilter->m_pCar->z(2,0) = cy;
-				this->m_pKalmanFilter->m_pCar->z(3,0) = cvy;
+			this->weight = cweight;
 
-				if(this->m_pKalmanFilter->m_nCounts == 1){
-					//第一次直接赋值
-					this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
-				}
-				if(this->m_pKalmanFilter->m_nCounts == 2){
-					//两次处理
-					this->m_pKalmanFilter->m_pCar->z(1, 0) = (this->m_pKalmanFilter->m_pCar->z(0, 0) - this->m_pKalmanFilter->m_pCar->x(0, 0))/deltaT;
-					this->m_pKalmanFilter->m_pCar->z(3, 0) = (this->m_pKalmanFilter->m_pCar->z(2, 0) - this->m_pKalmanFilter->m_pCar->x(2, 0))/deltaT;
-					this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
+			if(bOriginLocateState){
+				if(cweight*uwb_state <= 0){
+					//惯导和uwb定位方向不一致，定位失败
+					//增加一次判断，与上一次的进行比对，如果和上一次的一致，则送进卡尔曼滤波
+					if(cweight*this->accelerate_state_last > 0){
+						bUseKalman = true;
+					}else{
+						bUseKalman = false;
+					}
+				}else{
+					//惯导和uwb定位方向一致，定位成功
+					//送进卡尔曼滤波
+					bUseKalman = true;
 				}
-				if(this->m_pKalmanFilter->m_nCounts >= 3){
-					//只有三次以上才允许使用kalman滤波以下的函数
-					//this->m_pKalmanFilter->Predict_Correct(deltaT);
-					this->m_pKalmanFilter->Predict_Correct(kalman_detal_t);
-					sdNew.update = true;
-					
-					if(deltaT!=0){
-						/*this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - this->last_locate.x)/deltaT;
-						this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - this->last_locate.y)/deltaT;*/
-						this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - sd.x)/deltaT;
-						this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - sd.y)/deltaT;
-					}	
+			}else{
+				if(k*uwb_state > 0){
+					//方向一致，但原始定位失败了
+					//送进卡尔曼滤波
+					bUseKalman = true;
+				}else{
+					//方向不一致，而且定位也失败了
+					//不送了，直接取上一次的结果
+					bUseKalman = false;
 				}
 			}
+		}
+		else{
+			//不送到卡尔曼滤波
+			//bUseKalman = true;
+			bUseKalman = false;
+		}
+#endif
+		if(m_nFilterType == FILTER_KALMAN){
+#ifdef ALGORITHM_TYPE_INS
+			if(bUseKalman){
+#endif
+				double kalman_detal_t = (maxSyncTimes - this->last_locate.st)*interval_time;
+
+				//通过卡尔曼滤波处理
+				if(p->posx == INVALID_COORDINATE && p->posy == INVALID_COORDINATE){
+					this->m_pKalmanFilter->m_bFlag = false;	
+					if(this->m_pKalmanFilter->m_nCounts < 3 || this->m_pKalmanFilter->m_pCar->P(0,0) > 2 || kalman_detal_t > 3){
+						//P(0,0):连续时间(大于2s)都定位失败
+						//deltaT>3:距离上次成功定位时间间隔为3s
+						this->x = this->last_locate.x;
+						this->y = this->last_locate.y;
+						this->z = -3;
+						return;
+					}
 
-			if(p->nFirstReader == -1 && p->nSecondReader == -1){
-				p->nFirstReader = last_s_locate_reader[0];
-				p->nSecondReader = last_s_locate_reader[1];
-			}
-			//增加地图集的判定，判断定位结果是否在地图集上
-			//如果不在地图集上，需要再次定位
-			//需要带出定位结果的分站信息，
-			//利用地图集中分站信息再次定位
-			POS kalman_p;
-			kalman_p.nFirstReader = p->nFirstReader;
-			kalman_p.nSecondReader = p->nSecondReader;
-			kalman_p.posx = this->m_pKalmanFilter->m_pCar->x(0,0) * this->map_scale;
-			kalman_p.posy = this->m_pKalmanFilter->m_pCar->x(2,0) * this->map_scale;
-			//kalman_p.posz = this->m_pKalmanFilter->m_pCar->x(2,0) * this->map_scale;
-			kalman_p.pos_radius = p->pos_radius;
-
-			if(!LocateAlgorithm::IsOnMap(&kalman_p,*pTdoaReaderPathMap)){
-				//再一次定位到地图集上
-				POS* cp = LocateAlgorithm::Pos(&kalman_p,*pTdoaReaderPathMap);
-				
-				if(cp != NULL){}
+					if(this->m_pKalmanFilter->m_nCounts >= 3){
+						//只有三次以上才允许使用kalman滤波以下的函数
+						//this->m_pKalmanFilter->Predict(deltaT);
+						this->m_pKalmanFilter->Predict(kalman_detal_t);
+						//this->x = this->m_pKalmanFilter->m_pCar->x(0,0) / this->map_scale;
+						//this->y = this->m_pKalmanFilter->m_pCar->x(2,0) / this->map_scale;
+						this->z = -4;
+						sdNew.update = true;
+					}		
+				}else{
+					this->m_pKalmanFilter->m_bFlag = true;
+					this->m_pKalmanFilter->m_nCounts++;
 
-				this->m_pKalmanFilter->m_pCar->x(0,0) = cp->posx/this->map_scale;
-				this->m_pKalmanFilter->m_pCar->x(2,0) = cp->posy/this->map_scale;
+					this->m_pKalmanFilter->m_pCar->z(0,0) = cx * this->map_scale;
+					this->m_pKalmanFilter->m_pCar->z(1,0) = cvx;
+					this->m_pKalmanFilter->m_pCar->z(2,0) = cy * this->map_scale;
+					this->m_pKalmanFilter->m_pCar->z(3,0) = cvy;
 
-				if(cp){
-					delete cp;
-					cp = NULL;
+					if(this->m_pKalmanFilter->m_nCounts == 1){
+						//第一次直接赋值
+						this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
+					}
+					if(this->m_pKalmanFilter->m_nCounts == 2){
+						//两次处理
+						this->m_pKalmanFilter->m_pCar->z(1, 0) = (this->m_pKalmanFilter->m_pCar->z(0, 0) - this->m_pKalmanFilter->m_pCar->x(0, 0))/deltaT;
+						this->m_pKalmanFilter->m_pCar->z(3, 0) = (this->m_pKalmanFilter->m_pCar->z(2, 0) - this->m_pKalmanFilter->m_pCar->x(2, 0))/deltaT;
+						this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
+					}
+					if(this->m_pKalmanFilter->m_nCounts >= 3){
+						//只有三次以上才允许使用kalman滤波以下的函数
+						//this->m_pKalmanFilter->Predict_Correct(deltaT);
+						this->m_pKalmanFilter->Predict_Correct(kalman_detal_t);
+						sdNew.update = true;
+
+						if(deltaT!=0){
+							/*this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - this->last_locate.x)/deltaT;
+							this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - this->last_locate.y)/deltaT;*/
+							this->m_pKalmanFilter->m_pCar->x(1,0) = (this->m_pKalmanFilter->m_pCar->x(0,0) - sd.x*this->map_scale)/deltaT;
+							this->m_pKalmanFilter->m_pCar->x(3,0) = (this->m_pKalmanFilter->m_pCar->x(2,0) - sd.y*this->map_scale)/deltaT;
+						}	
+					}
 				}
-			}		
-
-			this->last_locate.x = this->x = this->m_pKalmanFilter->m_pCar->x(0,0);
-			this->last_locate.y = this->y = this->m_pKalmanFilter->m_pCar->x(2,0);
-			this->last_locate.z = this->z;
-			this->last_locate.tt = maxSycnTimes;
-
-			last_s_locate_reader[0] = p->nFirstReader;
-			last_s_locate_reader[1] = p->nSecondReader;
-			
-			this->m_pKalmanFilter->m_pCar->t = this->m_nLastLocateT = maxSycnTimes;
-			this->v = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,0),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,0),2))*this->map_scale;
-			this->v*=3.6;	//转为km/h
-
-			cvx = this->m_pKalmanFilter->m_pCar->x(1,0);
-			cvy = this->m_pKalmanFilter->m_pCar->x(3,0);
-
-			nSign = 1;
-			if(this->m_pKalmanFilter->m_pCar->x(1,0) == 0){
-				if(this->m_pKalmanFilter->m_pCar->x(3,0)>0){
-					nSign = 1;
+
+				if(p->nFirstReader == -1 && p->nSecondReader == -1){
+					p->nFirstReader = last_s_locate_reader[0];
+					p->nSecondReader = last_s_locate_reader[1];
 				}
-			}else{
-				if(this->m_pKalmanFilter->m_pCar->x(1,0) > 0){
-					nSign = 1;
+				//增加地图集的判定，判断定位结果是否在地图集上
+				//如果不在地图集上，需要再次定位
+				//需要带出定位结果的分站信息，
+				//利用地图集中分站信息再次定位
+				POS kalman_p;
+				kalman_p.nFirstReader = p->nFirstReader;
+				kalman_p.nSecondReader = p->nSecondReader;
+				kalman_p.posx = this->m_pKalmanFilter->m_pCar->x(0,0);//* this->map_scale
+				kalman_p.posy = this->m_pKalmanFilter->m_pCar->x(2,0);//* this->map_scale
+				kalman_p.pos_radius = p->pos_radius;
+
+				if(!LocateAlgorithm::IsOnMap(&kalman_p,*pTdoaReaderPathMap)){
+					//再一次定位到地图集上
+					POS* cp = LocateAlgorithm::Pos(&kalman_p,*pTdoaReaderPathMap);
+
+					if(cp != NULL){
+						this->m_pKalmanFilter->m_pCar->x(0,0) = cp->posx;
+						this->m_pKalmanFilter->m_pCar->x(2,0) = cp->posy;
+					}
+
+					if(cp){
+						delete cp;
+						cp = NULL;
+					}
+				}		
+
+				this->last_locate.x = this->x = this->m_pKalmanFilter->m_pCar->x(0,0) / this->map_scale;
+				this->last_locate.y = this->y = this->m_pKalmanFilter->m_pCar->x(2,0) / this->map_scale;
+				this->last_locate.z = this->z;
+				this->last_locate.st = maxSyncTimes;
+
+				last_s_locate_reader[0] = p->nFirstReader;
+				last_s_locate_reader[1] = p->nSecondReader;
+
+				this->m_pKalmanFilter->m_pCar->t = this->m_nLastLocateT = maxSyncTimes;
+				this->v = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,0),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,0),2)); //*this->map_scale
+				this->v*=3.6;	//转为km/h
+
+				cvx = this->m_pKalmanFilter->m_pCar->x(1,0);
+				cvy = this->m_pKalmanFilter->m_pCar->x(3,0);
+
+				nSign = 1;
+				if(this->m_pKalmanFilter->m_pCar->x(1,0) == 0){
+					if(this->m_pKalmanFilter->m_pCar->x(3,0)>0){
+						nSign = 1;
+					}
 				}else{
-					nSign = -1;
+					if(this->m_pKalmanFilter->m_pCar->x(1,0) > 0){
+						nSign = 1;
+					}else{
+						nSign = -1;
+					}
 				}
-			}
-			this->v*=nSign;
+				this->v*=nSign;
 
+				this->last_locate.v = this->v;
+				this->origin_locate.v = cv;
 
-			this->last_locate.v = this->v;
-			this->origin_locate.v = cv;
+				if(sdNew.update){
+					sdNew.sync_num = maxSyncTimes;
+					sdNew.x = this->x;
+					sdNew.y = this->y;
+					sdNew.vx = cvx;
+					sdNew.vy = cvy;
+					m_syncNumList.push_back(sdNew);
+				}
+#ifdef ALGORITHM_TYPE_INS
+			}else{
+				//这组数据的处理方法是：
+				//如果第一次成功定位，但状态是静止，就取第一次成功定位值(这是为了处理当第一次成功定位，但状态是静止的，此时取上一次定位值为零的问题)，
+				//后续如果不管定位成功还是失败，只要状态是静止的，就输出上一次成功定位值，并更新同步序号。
+				//取上次结果
+				if(this->last_locate.x == 0 && this->last_locate.y == 0){
+					if(this->map_scale > 0){
+						this->last_locate.x = this->x = p->posx / (this->map_scale*1.0);
+						this->last_locate.y = this->y = p->posy / (this->map_scale*1.0);
+						this->last_locate.z = this->z = p->posz / (this->map_scale*1.0);
+						this->last_locate.v = this->v = cv;
+					}
+				}else{
+					this->x = this->last_locate.x;
+					this->y = this->last_locate.y;
+					this->z = this->last_locate.z;
+					this->v = this->last_locate.v;
+				}
+				
+				this->m_nLastLocateT = this->m_nCalcSyncNum = this->last_locate.st = this->sync_num = maxSyncTimes;
+	
+				if(sdNew.update){
+					sdNew.sync_num = maxSyncTimes;
+					sdNew.x = this->x;
+					sdNew.y = this->y;
+					sdNew.vx = cvx;
+					sdNew.vy = cvy;
+					m_syncNumList.push_back(sdNew);	
 
-			if(sdNew.update){
-				sdNew.sync_num = maxSycnTimes;
-				sdNew.x = this->x;
-				sdNew.y = this->y;
-				sdNew.vx = cvx;
-				sdNew.vy = cvy;
-				m_syncNumList.push_back(sdNew);
+					this->m_nSyncNumInList = maxSyncTimes;
+				}
+
+				this->is_deal_by_algo = true;
 			}
+#endif
 		}else{
 			//TRACE(_T("no kalman . \n"));
 			//最新通过算法算出的结果
@@ -933,7 +1006,7 @@ void Card::get_coordinate( int cnt )
 				this->last_locate.y = this->y;
 				this->last_locate.z = this->z;
 				this->last_locate.v = this->v;
-				this->m_nLastLocateT = this->last_locate.tt = maxSycnTimes;
+				this->m_nLastLocateT = this->last_locate.st = maxSyncTimes;
 			}
 			this->a = 0;
 		}
@@ -963,11 +1036,9 @@ void Card::get_coordinate()
 
 std::string Card::get_state_text()
 {
-	//if(this->state == 0){
-	//	return "正常";
-	//}
 	string ret = "";
 	state = 0;
+	state_biz = 0;
 	if(status_help == STATUS_ERROR){
 		state += STATUS_HELP;
 		state_biz += STATUS_HELP;
@@ -1067,9 +1138,11 @@ void Card::EnableFilter(int nType)
 	switch(nType){
 	case FILTER_KALMAN:
 		//分配卡尔曼滤波类型变量并初始化参数
-		m_pKalmanFilter = new CKalmanFilter();
-		m_pKalmanFilter->Initial(0.2);
-		m_pKalmanFilter->m_bFlag = false;
+		if(m_pKalmanFilter == NULL){
+			m_pKalmanFilter = new CKalmanFilter();
+			m_pKalmanFilter->Initial(0.2);
+			m_pKalmanFilter->m_bFlag = false;
+		}
 		break;
 	default:
 		break;
@@ -1104,6 +1177,11 @@ Reader::Reader(void)
 	for(int i = 0;i < ANTENNA_COUNT;i++){
 		ant[i] = NULL;
 	}
+
+	for(int i = 0;i < ADHOC_COUNT;i++){
+		adhoc[i] = NULL;
+	}
+
 	bIsInitCoverage = false;
 
 	reader_interval_time = 0.0;
@@ -1113,6 +1191,26 @@ Reader::Reader(void)
 
 Reader::~Reader(void)
 {
+	for(int i = 0;i < ANTENNA_COUNT;i++){
+		Antenna* tmp = NULL;
+		tmp = ant[i];
+		if(tmp){
+			delete tmp;
+			tmp = NULL;
+			ant[i] = NULL;
+		}
+	}
+
+	for(int i = 0; i < ADHOC_COUNT;i++){
+		Adhoc * tmp = NULL;
+		tmp = adhoc[i];
+		if(tmp){
+			delete tmp;
+			tmp = NULL;
+			adhoc[i] = NULL;
+		}
+	
+	}
 }
 
 std::string Reader::get_state_text()

classdef.h

@@ -16,7 +16,7 @@
 #define KALMAN_INTERVAL_TRI 1
 
 #define ANTENNA_COUNT 2
-#define ADHOC_COUNT 4
+#define ADHOC_COUNT 10
 #define READER_TIMEOUT 20
 #define OVER_TIME 480 * 60
 
@@ -27,6 +27,7 @@
 //算法类型
 //#define ALGORITHM_TYPE_TOF
 #define ALGORITHM_TYPE_TDOA
+//#define ALGORITHM_TYPE_INS		//使用惯导判断
 
 //滤波功能开启
 //#define FILTER_KALMAN
@@ -46,11 +47,12 @@
 #define MIN(x,y) (x < y ? x : y)
 #define MAX(x,y) (x > y ? x : y)
 
-//#define MAP_SCALE 0.392
-//#define MAP_SCALE 0.77  
-//#define MAP_SCALE 7 
 using namespace std;
 
+const int MAX_CALC_POINTS = 30;
+const double INS_WEIGHT = 9.0;	//惯导权重
+const double UWB_WEIGHT = 1.0;  //UWB权重
+
 enum TIMER_ID
 {
 	TMR_SEND_COUNTING = 1,
@@ -215,6 +217,7 @@ struct _coordinate{
 		d_offset = 0.0;
 		special = 0;
 		acceleration = 0;
+		acce_state = 0;	
 	}
 	int t; // 定位时间戳
 	int reader_id;
@@ -231,6 +234,7 @@ struct _coordinate{
 
 	int st;   //同步序号
 	double acceleration;	//加速度
+	int acce_state;	//加速度状态
 	int special; //分站是否属于特殊分站，0属于特殊分站，1普通分站
 
 	_coordinate& operator=(_coordinate &tmp){
@@ -248,6 +252,7 @@ struct _coordinate{
 		d_offset = tmp.d_offset;
 		special = tmp.special;
 		acceleration = tmp.acceleration;
+		acce_state = tmp.acce_state;
 		return *this;
 	}
 };
@@ -265,7 +270,7 @@ struct sync_data{
 	double vx;
 	double vy;
 
-	unsigned long long sync_num;	//本次同步号
+	int sync_num;	//本次同步号
 
 	bool update;
 
@@ -311,8 +316,6 @@ struct _call_info_reader
 };
 typedef map<int, _call_info_reader*> CallInfoReaderMap;
 
-const int MAX_CALC_POINTS = 30;
-
 // 分站接收时间定义
 struct ReceiveData{
 	unsigned int reader_id;		// 分站号
@@ -356,6 +359,18 @@ struct POS{
 	double dFirstDiff;
 	double dSecondDiff;
 
+	POS(){
+		nFirstReader = INVALID_COORDINATE;
+		nSecondReader = INVALID_COORDINATE;
+		posx = 0;
+		posy = 0;
+		posz = 0;
+		dFirstDiff = 0;
+		dSecondDiff = 0;
+		pos_radius = 0;
+	
+	}
+
 	POS& operator=(POS& tmp){
 		nFirstReader = tmp.nFirstReader;
 		nSecondReader = tmp.nSecondReader;
@@ -377,8 +392,6 @@ struct RESULT{
 	int nCount;
 };
 
-//2016-09-12 end
-
 class Mine;
 class Area;
 class Reader;
@@ -394,20 +407,15 @@ typedef map<int, MapInfo*> MapInfoMap;
 typedef map<int, Dept*> DeptMap;
 typedef map<int, string> AlarmTypeMap;
 typedef map<int,ReaderPath*> ReaderPathMap;
-//typedef map<int,ReaderPathMap> TDOAReaderPathMap;
 typedef unordered_map<int,ReaderPathMap*> TDOAReaderPathMap;
 typedef unordered_map<string, _coordinate*> DistMap;
 
-//typedef unordered_map<DWORD, DistMap> DistQueMap;
-
 struct DistQueMapItem
 {
 	WORD cardstamp;
 	DistMap distmap;
 };
 typedef deque<DistQueMapItem> DistQueMap;
-//typedef unordered_map<DWORD, DistMap> DistQueMap;
-//typedef list<DWORD, DistMap> DistQueMap;
 
 // 地图
 class MapInfo{
@@ -509,6 +517,7 @@ public:
 	CardMap area_card_list_person;
 	CardMap area_card_list_vehicle;
 };
+
 // 自组网预置坐标
 class Adhoc
 {
@@ -610,7 +619,8 @@ public:
 	string group; // 班组 
 	string worktype; // 工种、车辆类型
 
-	time_t deal_time; // 最后处理卡逻辑时间
+	//time_t deal_time; // 最后处理卡逻辑时间
+	SYSTEMTIME deal_time;
 	time_t rec_time; // 最后接收时间
 	time_t down_time; // 入井时间
 	time_t up_time; // 升井时间
@@ -702,8 +712,8 @@ public:
 
 	int m_nFilterType;
 	int m_nLastLocateT;
-	unsigned short m_unCalcSyncNum;	//卡在算法中的同步号，此同步号和卡的实时同步号并不一定一致
-	unsigned short m_unSyncNumInList;	//从队列中取出的大于5的同步号
+	int m_nCalcSyncNum;	//卡在算法中的同步号，此同步号和卡的实时同步号并不一定一致
+	int m_nSyncNumInList;	//从队列中取出的大于5的同步号
 
 	//int last_area_type_id;
 	//int cur_area_type_id;
@@ -713,7 +723,7 @@ public:
 	bool isdealed; // 已经处理
 	bool isreceive; // 接收到
 	bool is_pos_changed;
-	bool is_init_kalman ;
+	bool is_deal_by_algo;
 	bool is_hist;
 	bool is_need_cal;
 	bool m_bUseFilter;
@@ -752,6 +762,10 @@ public:
 
 	int last_s_locate_reader[2];	//上一次定位成功的分站信息
 
+	double ins_weight;
+	double uwb_weight;
+	double weight;
+
 	bool is_anchor_changed;
 	void get_coordinate();
 	void get_coordinate(int cnt);

constdef.h

@@ -17,7 +17,7 @@
 #define JSON_CMD_VALUE_SET_LIMIT_VALUE "set_limit_value"
 #define JSON_CMD_VALUE_CALL_CARD "call_card"
 #define JSON_CMD_VALUE_CLEAR_CARD "clear_card"
-#define JSON_CMD_VALUE_DEAL_HELP "deal_help"
+#define JSON_CMD_VALUE_DEAL_HELP "helpme_done"   //deal_help
 #define JSON_CMD_VALUE_EDIT_MAP "edit_map"
 #define JSON_CMD_VALUE_EDIT_AREA "edit_area"
 #define JSON_CMD_VALUE_EDIT_STATION "edit_station"

locate_algorithm.cpp

@@ -836,12 +836,14 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 {
 	POS *pos = NULL;
 	pos = new POS;
-	pos->posx = 0;
+	/*pos->posx = 0;
 	pos->posy = 0;
 	pos->posz = 0;
 	pos->pos_radius = 0;
 	pos->dFirstDiff = INVALID_COORDINATE;
 	pos->dSecondDiff = INVALID_COORDINATE;
+	pos->nFirstReader = INVALID_COORDINATE;
+	pos->nSecondReader = INVALID_COORDINATE;*/
 
 	double a = 0;
 	double ref_dist = 0.0;   //两分站之间无路径的距离差
@@ -864,6 +866,10 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 	//ReceiveData* tmp_reader = new ReceiveData[Totals];
 	ReceiveData tmp_reader[Totals];
 
+	//存储和第一条分站存在路径的分站信息
+	ReceiveData tmp_dist_reader[Totals];
+	int nDistReaders = 0;
+
 	ReceiveDataMap::iterator first = pRdm->begin();
 	//for(ReceiveDataMap::iterator first = pRdm->begin();first!=pRdm->end();++first){///11
 		//ReceiveDataMap::iterator second = pRdm->begin();
@@ -899,17 +905,26 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 		if(f1.reader_id == f2.reader_id){
 			continue;
 		}
+
+		ReaderPathMap::iterator rpm_it = trpm.find(f1.reader_id)->second->find(f2.reader_id);
+		if(rpm_it == trpm.find(f1.reader_id)->second->end()){
+			continue;
+		}else{
+			//如果和第一条分站存在地图集
+			tmp_dist_reader[nDistReaders].reader_id = f2.reader_id;
+			tmp_dist_reader[nDistReaders].x = f2.x;
+			tmp_dist_reader[nDistReaders].y = f2.y;
+			tmp_dist_reader[nDistReaders].z = f2.z;
+			tmp_dist_reader[nDistReaders].rec_time_stamp = f2.rec_time_stamp;
+			nDistReaders++;
+		}
+
 		//根据距离的正负，后续判断计算位置取舍时使用
 		int nSign = 1;
 		long long diffTime = f1.rec_time_stamp - f2.rec_time_stamp;
-		/*if(diffTime < 0){
-		diffTime = diffTime*(-1);
-		nSign = -1;
-		}*/
 
 		//计算位置
 		double dist = CFunctions::getDistance(diffTime,CFunctions::TDOA);
-		/*dist = dist * nSign;*/
 
 		double readers_dist = sqrt(pow(f1.x - f2.x,2) + pow(f1.y - f2.y,2));
 
@@ -923,26 +938,6 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 			continue;
 		}
 
-		ReaderPathMap::iterator rpm_it = trpm.find(f1.reader_id)->second->find(f2.reader_id);
-		if(rpm_it == trpm.find(f1.reader_id)->second->end()){
-		//	//如果两个分站之间无地图集
-		//	diffTime = f1.rec_time_stamp - f2.rec_time_stamp;
-		//	//计算位置
-		//	ref_dist = CFunctions::getDistance(diffTime,CFunctions::TDOA);
-
-		//	tmp_reader[0].reader_id = f1.reader_id;
-		//	tmp_reader[0].x = f1.x;
-		//	tmp_reader[0].y = f1.y;
-		//	tmp_reader[0].z = f1.z;
-
-		//	tmp_reader[1].reader_id = f2.reader_id;
-		//	tmp_reader[1].x = f2.x;
-		//	tmp_reader[1].y = f2.y;
-		//	tmp_reader[1].z = f2.z;
-
-			continue;
-		}
-
 		//获取地图集路径
 		ReaderPath * pRP = trpm.find(f1.reader_id)->second->find(f2.reader_id)->second;
 		ReaderPath pRP2 = *trpm.find(f1.reader_id)->second->find(f2.reader_id)->second;
@@ -1076,60 +1071,81 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 			}
 		}
 
-		if(pRdm->size()>=3){
-			ReceiveDataMap::iterator it = pRdm->begin();
+		if(pRdm->size()>=3&&bValid){
+			/*ReceiveDataMap::iterator it = pRdm->begin();
 			ReceiveDataMap::iterator f_it = pRdm->end();
 			int k = 0;
 			int readersNo[2] = {0};
 			readersNo[0] = nFirstReader[0];
-			readersNo[1] = nSecondReader[0];
+			readersNo[1] = nSecondReader[0];*/
 
 			//找出没有参与定位的任意一个分站信息
-			ReceiveData rd;
-			bool isExist = false;
-			for(it;it!=pRdm->end();++it){
-				for(int i=0;i<2;i++){
-					if(it->first == readersNo[i]){
-						f_it = pRdm->end();
-						isExist = false;
-						break;
-					}else{
-						isExist = true;
-					}
-				}
-				if(isExist){
-					//如果存在
-					rd.reader_id = it->first;
-					rd.rec_time_stamp = it->second.rec_time_stamp;
-					rd.antenna_id = it->second.antenna_id;
-					rd.special = it->second.special;
-					rd.x = it->second.x;
-					rd.y = it->second.y;
-					rd.z = it->second.z;
-				}
-			}
+			//ReceiveData rd;
+			//bool isExist = false;
+			//for(it;it!=pRdm->end();++it){
+			//	for(int i=0;i<2;i++){
+			//		if(it->first == readersNo[i]){
+			//			f_it = pRdm->end();
+			//			isExist = false;
+			//			break;
+			//		}else{
+			//			isExist = true;
+			//		}
+			//	}
+			//	if(isExist){
+			//		//如果存在
+			//		rd.reader_id = it->first;
+			//		rd.rec_time_stamp = it->second.rec_time_stamp;
+			//		rd.antenna_id = it->second.antenna_id;
+			//		rd.special = it->second.special;
+			//		rd.x = it->second.x;
+			//		rd.y = it->second.y;
+			//		rd.z = it->second.z;
+			//	}
+			//}
 
 			double dist[2] = {0};
 			//计算定位结果到参与定位结果之一的分站和未参与定位的分站之间的距离差
 			//比较此距离差和这两个
-			if(rd.reader_id!=0){
-				ReceiveDataMap::iterator it = pRdm->find(nFirstReader[0]);
+			//if(rd.reader_id!=0){
+			//	ReceiveDataMap::iterator it = pRdm->find(nFirstReader[0]);
+
+			//	dist[0] = sqrt(pow(res[0].x - it->second.x,2) + pow(res[0].y - it->second.y,2));
+			//	dist[1] = sqrt(pow(res[0].x - rd.x,2) + pow(res[0].y - rd.y,2));
+
+			//	double dif1 = fabs(dist[0] - dist[1]);
 
-				dist[0] = sqrt(pow(res[0].x - it->second.x,2) + pow(res[0].y - it->second.y,2));
-				dist[1] = sqrt(pow(res[0].x - rd.x,2) + pow(res[0].y - rd.y,2));
+			//	double dt = it->second.rec_time_stamp - rd.rec_time_stamp;
+			//	double dif2 = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));
+
+			//	double dif = fabs(dif1 - dif2);
+
+			//	if(dif>10){//4
+			//		bValid = false;
+			//	}	
+			//}
+			//判断下定位坐标距离定位分站的距离
+
+
+			//2016/12/06 更改选取分站方式:
+			//选取和第一个分站有地图集的两个分站来参与判别，
+			//例如：分站顺序是2，1,3，其中（1,2）和（2,3）之间有地图集，那么就选取1,3来参与判别
+			//如果分站顺序是1,2，3，其中（1，2）有地图集，(1,3)之间无地图集，那么此时就不使用以下方法进行判别
+			if(nDistReaders == 2){
+				dist[0] = sqrt(pow(res[0].x - tmp_dist_reader[0].x,2) + pow(res[0].y - tmp_dist_reader[0].y,2));
+				dist[1] = sqrt(pow(res[0].x - tmp_dist_reader[1].x,2) + pow(res[0].y - tmp_dist_reader[1].y,2));
 
 				double dif1 = fabs(dist[0] - dist[1]);
 
-				double dt = it->second.rec_time_stamp - rd.rec_time_stamp;
+				double dt = tmp_dist_reader[0].rec_time_stamp - tmp_dist_reader[1].rec_time_stamp;
 				double dif2 = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));
 
 				double dif = fabs(dif1 - dif2);
 
-				if(dif>4){
+				if(dif>10){//4
 					bValid = false;
 				}	
 			}
-			//判断下定位坐标距离定位分站的距离
 		}
 
 		if(bValid){
@@ -1184,8 +1200,10 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 	//pos->posz = res.z;
 	pos->posz = 0;
 
-	pos->nFirstReader = nFirstReader[r_idx];
-	pos->nSecondReader = nSecondReader[r_idx];
+	if(pos->posx != INVALID_COORDINATE && pos->posy != INVALID_COORDINATE){
+		pos->nFirstReader = nFirstReader[r_idx];
+		pos->nSecondReader = nSecondReader[r_idx];
+	}
 
 	return pos;
 }

log_def.h

@@ -13,13 +13,14 @@
 #define LOG_INIT_CARD "初始化标识卡信息"
 #define LOG_INIT_STAFF "初始化人员信息"
 #define LOG_INIT_VEHICLE "初始化车辆信息"
+#define LOG_INIT_ADHOC "初始化自组网信息"
 #define LOG_INIT_MAP_PATH "初始化地图路径信息"
 #define LOG_INIT_READER_PATH "初始化分站路径信息"
 #define LOG_INIT_MAP "初始化地图信息"
 #define LOG_INIT_AREA "初始化区域信息"
 #define LOG_INIT_READER "初始化分站信息"
 #define LOG_INIT_ANTENNA "初始化分站天线信息"
-#define LOG_INIT_ADHOC "初始化自组网设备预置坐标信息"
+#define LOG_INIT_ADHOC_LOCATE "初始化自组网设备预置坐标信息"
 #define LOG_LOAD_HIS_AREA "载入区域历史数据"
 #define LOG_LOAD_HIS_POSTION "载入位置历史数据"
 #define LOG_LOAD_HIS_STATE "载入状态历史数据"
@@ -55,6 +56,7 @@
 #define LOG_EDIT_CARD "编辑标识卡"
 #define LOG_EDIT_STAFFER "编辑员工"
 #define LOG_EDIT_VEHICLE "编辑车辆"
+#define LOG_EDIT_ADHOC "编辑自组网卡"
 #define LOG_REQUEST_ALL_POSTION "请求所有位置信息"
 
 #define LOG_INVALID_PARSE "非法数据解析"