tdoa

YAServerDlg.cpp

@@ -56,6 +56,7 @@
 //int aa = 0;
 
 DWORD g_tickcount;
+DWORD g_diffTickCount = 0;
 
 class CAboutDlg : public CDialogEx
 {
@@ -547,6 +548,8 @@ HCURSOR CYAServerDlg::OnQueryDragIcon()
 
 void CYAServerDlg::OnBnStart()
 {
+	Test();
+
 	if(!is_websocket_login){
 		ws_login();
 	}
@@ -1103,6 +1106,7 @@ int CYAServerDlg::init_dev_adhoc( int reader_id /*= 0*/ )
 }
 
 /*
+ * 函数名：init_all_readers_coverage
  * 从数据库的dat_reader_path表中读取指定map_id和reader_id的地图范围集，
  * 将此分站的地图范围集保存在分站类中的readerCoverage中；
  * 如果指定了相同的地图范围集的reader_id,则直接将指定的reader_id的地图范围集赋值给此reader_id的地图范围集
@@ -1110,10 +1114,10 @@ int CYAServerDlg::init_dev_adhoc( int reader_id /*= 0*/ )
  * 
  *
  * @param
- *		reader		分站类对象
+ *		reader_id		分站id号
  *
  * @return
- *		-1，表示在分站map中未找到此分站信息；0，表示正常执行
+ *		-1，表示在分站map中未找到此分站信息或者未正常执行；0，表示正常执行
  *
 */
 int CYAServerDlg::init_all_readers_coverage(int reader_id)
@@ -1121,6 +1125,7 @@ int CYAServerDlg::init_all_readers_coverage(int reader_id)
 	CMysqlConn* pConn = NULL;
 	CDBConnGuard pDbGuard(pConn);
 
+	//如果无可用数据库连接，不执行后续操作
 	if(pConn == NULL){
 		return -1;
 	}
@@ -1136,7 +1141,9 @@ int CYAServerDlg::init_all_readers_coverage(int reader_id)
 	MYSQL_ROW pRow;
 	int err = 0;
 	int nRow = 0;
+	//查询数据库
 	pRes = pConn->Execute(sql,err);
+
 	//从数据库获得信息赋给分站对象的地图集
 	while(pRow = mysql_fetch_row(pRes)){
 		int nReaderId = atoi(pRow[1]);
@@ -1144,6 +1151,8 @@ int CYAServerDlg::init_all_readers_coverage(int reader_id)
 
 		int nSameConfigReader = atoi(pRow[0]);
 		int nIdx = atoi(pRow[2]);
+		//判断地图集信息是否可利用其它分站的覆盖范围，如果可用，则使用其它分站的覆盖范围
+		//-1表示不可用，否则表示可复用分站id号为nSameConfigReader的地图集覆盖范围
 		if(nSameConfigReader == -1){
 			_point p;
 			p.x = atof(pRow[3]);
@@ -1160,11 +1169,12 @@ int CYAServerDlg::init_all_readers_coverage(int reader_id)
 	}
 	mysql_free_result(pRes);
 
+	//输出操作数据库的日志信息
 	CString strlog;
 	strlog.Format(_T("%s%d%s"), _T(LOG_INIT_VEHICLE), nRow, _T(LOG_RECORD_COUNT));
 	show_log(strlog);
 
-	//根据获得地图集将其赋给station
+	//根据获得地图集将其赋给mp_reader_path_list
 	ReaderMap::iterator it = mp_reader_list.begin();
 	for(it;it!=mp_reader_list.end();it++){
 		if(it->second->bIsInitCoverage){
@@ -1193,7 +1203,8 @@ int CYAServerDlg::init_all_readers_coverage(int reader_id)
 				pReaderPath->pz[j - 1] = it->second->readerCoveragePath.find(j)->second.z*dMapScale;
 			}
 	
-			//初始化
+			//以下对应与全局变量的数组形式的另一种实现
+			//初始化 
 			/*station[nReaderId].nRealCalcPoints = 0;
 			for(int i=0;i<2;i++){
 				station[nReaderId].x[i] = 0;
@@ -1225,6 +1236,135 @@ int CYAServerDlg::init_all_readers_coverage(int reader_id)
 	return 0;
 }
 
+/*
+ * 函数名：init_tdoa_all_readers_coverage
+ * 实现初始化TDOA方式的地图集描述，并存储冗余数据
+ * 冗余数据的主要作用在于：例如存在分站<B1,B2>之间的地图集描述，那么也存在<B2,B1>的对应关系
+ *
+ * param 
+ *		reader_id	------	分站id号
+ *
+ * return
+ *		如果执行失败返回-1
+ *		正确执行，返回0
+ *
+*/
+int CYAServerDlg::init_tdoa_all_readers_coverage(int reader_id)
+{
+	CMysqlConn* pConn = NULL;
+	CDBConnGuard pDbGuard(pConn);
+
+	//无可用数据库连接
+	if(pConn == NULL){
+		return -1;
+	}
+
+	char sql[LENGTH_SQL] = {0};
+	if(reader_id == 0){
+		sprintf_s(sql, "select p.start_reader_id,p.end_reader_id,p.idx,p.x,p.y,p.z,p.is_bezier from dat_reader_path_tdoa p ,dat_reader r where p.start_reader_id = r.reader_id;");
+	}else{
+		sprintf_s(sql, "select start_reader_id,end_reader_id,idx,x,y,z,is_bezier from dat_reader_path_tdoa where reader_id = %d; ", reader_id);
+	}
+
+	MYSQL_RES* pRes = NULL;
+	MYSQL_ROW pRow;
+	int err = 0;
+	int nRow = 0;
+
+	//查询数据库
+	pRes = pConn->Execute(sql,err);
+
+	double mapScale = 0.0;
+	while(pRow = mysql_fetch_row(pRes)){
+		int start_reader_id = atoi(pRow[0]);
+		int end_reader_id   = atoi(pRow[1]);
+		int ndx = atoi(pRow[2]);
+
+		//后缀_r都为冗余信息
+		ReaderPath* prp = NULL;
+		ReaderPath* prp_r = NULL;
+
+		TDOAReaderPathMap::iterator it = mp_reader_path_list_tdoa.find(start_reader_id);	
+
+		if(it == mp_reader_path_list_tdoa.end()){
+			ReaderPathMap* prpm = new ReaderPathMap();
+			ReaderPath* prp1 = new ReaderPath();
+			prpm->insert(make_pair(end_reader_id,prp1));
+			mp_reader_path_list_tdoa.insert(make_pair(start_reader_id,*prpm));	
+
+			//冗余数据
+			ReaderPathMap* prpm_r = new ReaderPathMap();
+			ReaderPath* prp1_r = new ReaderPath();
+			prpm_r->insert(make_pair(start_reader_id,prp1_r));
+			mp_reader_path_list_tdoa.insert(make_pair(end_reader_id,*prpm_r));		
+		}else{
+			ReaderPathMap::iterator it2;
+			it2 = it->second.find(end_reader_id);
+			if(it2 == it->second.end()){
+				ReaderPath* prp1 = new ReaderPath();
+				it->second.insert(make_pair(end_reader_id,prp1));
+
+				//冗余数据
+				ReaderPathMap* prpm_r = new ReaderPathMap();
+				ReaderPath* prp1_r = new ReaderPath();
+				prpm_r->insert(make_pair(start_reader_id,prp1_r));
+				mp_reader_path_list_tdoa.insert(make_pair(end_reader_id,*prpm_r));
+			}
+		}
+
+		ReaderPathMap::iterator it_rpm = mp_reader_path_list_tdoa.find(start_reader_id)->second.find(end_reader_id);
+		if(it_rpm!=mp_reader_path_list_tdoa.find(start_reader_id)->second.end()){
+			prp = it_rpm->second;
+		}else{
+			prp = new ReaderPath();
+		}
+
+		//冗余信息
+		ReaderPathMap::iterator it_rpm_r = mp_reader_path_list_tdoa.find(end_reader_id)->second.find(start_reader_id);
+		if(it_rpm_r!=mp_reader_path_list_tdoa.find(end_reader_id)->second.end()){
+			prp_r = it_rpm_r->second;
+		}else{
+			prp_r = new ReaderPath();
+		}
+		
+		//获得地图比例
+		int map_id = mp_reader_list.find(start_reader_id)->second->map_id;
+		MapInfoMap::iterator mit = mp_map_list.find(map_id);
+		if(mit == mp_map_list.end()){
+			//没有相关地图信息，要输出错误日志
+			AfxMessageBox(_T("分站所在地图不存在！"));
+			break;
+		}
+		mapScale = mp_map_list.find(map_id)->second->map_scale;
+
+		//获得分站坐标，其中索引0表示起始分站坐标，索引1表示结束分站坐标
+		prp->x[0] = mp_reader_list.find(start_reader_id)->second->reader_x*mapScale;
+		prp->y[0] = mp_reader_list.find(start_reader_id)->second->reader_y*mapScale;
+		prp->x[1] = mp_reader_list.find(end_reader_id)->second->reader_x*mapScale;
+		prp->y[1] = mp_reader_list.find(end_reader_id)->second->reader_y*mapScale;
+		prp->px[ndx-1] = atof(pRow[3])*mapScale;
+		prp->py[ndx-1] = atof(pRow[4])*mapScale;
+		prp->nRealCalcPoints++;
+
+		//冗余信息
+		prp_r->x[0] = mp_reader_list.find(end_reader_id)->second->reader_x*mapScale;
+		prp_r->y[0] = mp_reader_list.find(end_reader_id)->second->reader_y*mapScale;
+		prp_r->x[1] = mp_reader_list.find(start_reader_id)->second->reader_x*mapScale;
+		prp_r->y[1] = mp_reader_list.find(start_reader_id)->second->reader_y*mapScale;
+		prp_r->px[ndx-1] = atof(pRow[3])*mapScale;
+		prp_r->py[ndx-1] = atof(pRow[4])*mapScale;
+		prp_r->nRealCalcPoints++;
+	}
+	mysql_free_result(pRes);
+
+	//输出操作结果信息
+	CString strlog;
+	strlog.Format(_T("%s%d%s"), _T(LOG_INIT_VEHICLE), nRow, _T(LOG_RECORD_COUNT));
+	show_log(strlog);
+
+	return 0;
+}
+
 //int CYAServerDlg::init_adhoc( string adhoc_id /*= ""*/ )
 //{
 //	CMysqlConn* pConn = NULL;
@@ -1655,7 +1795,11 @@ void CYAServerDlg::parse_data_locate_card(BYTE* DataBuffer, int& nCurPos, int re
 		}
 		// 设置当前分站， 进入分站时间、进入区域时间，当前区域，当前地图
 		card->set_reader(it->second);
+#ifdef ALGORITHM_TYPE_TOF
 		card->set_reader_path(&mp_reader_path_list);
+#elif defined ALGORITHM_TYPE_TDOA
+		card->set_reader_path(&mp_reader_path_list_tdoa);
+#endif
 		card->reader_tickcount = it->second->tick_count;
 		card->is_hist = false;
 
@@ -2316,6 +2460,7 @@ std::string CYAServerDlg::get_json_postion()
 	if(! pos_maps.isNull()){
 		root[JSON_ROOT_KEY_CMD] = JSON_CMD_VALUE_POS_MAP;
 		root[JSON_ROOT_KEY_DATA] = pos_maps;
+		root[JSON_ROOT_KEY_DIFF_TICK_COUNT] = (int)g_diffTickCount;
 		ret = root.toFastString();
 	}
 	return ret;
@@ -2485,8 +2630,10 @@ void CYAServerDlg::send_json_data(string cmd, string data, bool is_login /* = fa
 	//strlog.Format(_T("%s: %s"), _T(LOG_SEND_JSON_DATA), CFunctions::c2wc(cmd.c_str()));
 	//show_log(strlog);
 	dw = GetTickCount();
+	g_diffTickCount = dw - g_tickcount;
 	TRACE(_T("%d\r\n"), dw - g_tickcount );
 	g_tickcount = dw;
+	
 }
 
 // 处理分站状态
@@ -2942,6 +3089,15 @@ void CYAServerDlg::package_data()
 	if(strJson == "") return ;
 	send_json_data(JSON_CMD_VALUE_PUSH, strJson);
 	dw_last_send_time = GetTickCount();
+	//ifstream
+
+	//json字符串输出到文件中
+	/*ofstream ofs;
+    ofs.open("result.js",ios::app);
+	Json::StyledWriter writer;
+	string res = writer.write(strJson);
+	ofs << res;
+    ofs.close();*/
 }
 
 void CYAServerDlg::package_data_test()
@@ -4605,6 +4761,7 @@ void CYAServerDlg::init_base_data()
 	init_reader();
 	init_antenna();
 	init_all_readers_coverage();
+	init_tdoa_all_readers_coverage();
 	init_dev_adhoc();
 }
 
@@ -5365,3 +5522,105 @@ void CYAServerDlg::parse_data_locate_reader_his( BYTE * DataBuffer, int& nCurPos
 		parse_data_locate_card_his(DataBuffer, nCurPos, dwReaderID, wTickCount, strTime);
 	}
 }
+
+void CYAServerDlg::Test()
+{
+	ReaderPath* prp = new ReaderPath();
+	/*prp->x[0] = -1;
+	prp->x[1] = 1;
+	prp->y[0] = 0;
+	prp->y[1] = 0;
+
+	prp->px[0] = -1;
+	prp->px[1] = 1;
+	prp->py[0] = 0;
+	prp->py[1] = 0;
+
+	//double dist = 0.5;
+	double dist = -0.5;*/
+
+	//2
+	/*prp->x[0] = 10;
+	prp->x[1] = 10;
+	prp->y[0] = 2;
+	prp->y[1] = 13.7;
+
+	prp->px[0] = 10;
+	prp->px[1] = 10;
+	prp->py[0] = 2;
+	prp->py[1] = 13.7;*/
+
+	//3
+	prp->x[0] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->x[0];
+	prp->x[1] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->x[1];
+	prp->y[0] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->y[0];
+	prp->y[1] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->y[1];
+
+	prp->px[0] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->px[0];
+	prp->px[1] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->px[1];
+	prp->py[0] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->py[0];
+	prp->py[1] = mp_reader_path_list_tdoa.find(27)->second.find(28)->second->py[1];
+
+	//double dist = 0.5;
+	double dist = -0.5;
+
+	RESULT* r = NULL;
+	r = LocateAlgorithm::GetPos(prp,dist,0);
+
+	//因为双曲线与分站之间第i条线段或者第j条线段分别有两焦点
+	//或者分站之间就一条直线，有两焦点
+	double xcross[2] = {0};
+	double ycross[2] = {0};
+
+	for(int j = 0; j < 2;j++){
+		xcross[j] = r->x[j];
+		ycross[j] = r->y[j];
+	}
+
+	double deltad[2] = {0};
+	for(int j = 0; j < 2;j ++){
+		double d[2] = {0};
+		double dx1 = r->x[j] - prp->x[0];
+		double dy1 = r->y[j] - prp->y[0];
+		d[0] = sqrt(pow(dx1,2) + pow(dy1,2));
+
+		double dx2 = r->x[j] - prp->x[1];
+		double dy2 = r->y[j] - prp->y[1];
+		d[1] = sqrt(pow(dx2,2) + pow(dy2,2));
+
+		deltad[j] = d[0] - d[1];
+	}
+
+	//解的索引
+	int idx = 0;
+	if(dist > 0){
+		for(int j = 0;j < 2;j++){
+			if(deltad[j] > 0){
+				idx = j;
+				break;
+			}
+		}	
+	}else{
+		for(int j = 0;j < 2;j++){
+			if(deltad[j] < 0){
+				idx = j;
+				break;
+			}
+		}	
+	}
+
+	bool bs = false;
+
+	_coordinate res;
+	if(!bs){
+		res.x = xcross[idx];
+		res.y = ycross[idx];
+	}else{
+		//例如当分站为B1B3,B2B3的情况下
+		double tempx = xcross[idx];
+		double tempy = ycross[idx];
+
+		res.x = (tempx + res.x)/2.0;
+		res.y = (tempy + res.y)/2.0;
+	}
+}

YAServerDlg.h

@@ -146,6 +146,7 @@ public:
 	DeptMap mp_dept_list; // 所有部门
 	AlarmTypeMap mp_alarm_type_list;
 	ReaderPathMap mp_reader_path_list;	//当前地图的所有分站的覆盖范围
+	TDOAReaderPathMap mp_reader_path_list_tdoa;
 
 	CardMap mp_card_list_down_vehicle; // 井下车辆
 	CardMap mp_card_list_down_person; // 井下人员
@@ -195,6 +196,7 @@ public:
 	int init_antenna(int reader_id = 0); // 天线
 	int init_dev_adhoc(int reader_id = 0); // 自组网坐标
 	int init_all_readers_coverage(int reader_id = 0);			//从数据库读取地图集覆盖范围,初始化所有分站的地图集
+	int init_tdoa_all_readers_coverage(int reader_id = 0);			//从数据库读取地图集覆盖范围,初始化所有分站的地图集
 	// 载入历史数据
 	void load_his_data();
 	void load_his_down(); // 入井状态
@@ -292,7 +294,6 @@ public:
 	// 通用方法
 	int API_TimeToString(string &strDateStr,const time_t &timeData);
 	int API_StringToTime(const string &strDateStr,time_t &timeData);
-
 	void formatByteArray(CString &strBuf, BYTE * buf, int nLen);
 	void writeErrorLog(const CString strFile, const CString strErr ,bool bTime = true);
 	BOOL openLogFile(const CString strFile);
@@ -323,6 +324,7 @@ public:
 	void send_sync_reader();
 	void clear_call_info();
 	int get_card_id_len(int card_type);
+	void Test();
 public:
 	std::unique_ptr<client> _io;
 	CListBox m_list_log;

classdef.cpp

@@ -54,6 +54,7 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 	p3_anchors = new Point3[ANCHOR_COUNT];
 	is_init_kalman = false;
 	pReaderPathMap = NULL;
+	pTdoaReaderPathMap = NULL;
 }
 
 Card::Card( void )
@@ -99,11 +100,24 @@ void Card::set_reader( Reader* preader ) //
 	}
 }
 
-void Card::set_reader_path(ReaderPathMap *rpm)
+/*
+ * 设置地图集覆盖范围，适用TOF
+ *
+*/
+void Card::set_reader_path(ReaderPathMap* rpm)
 {
 	this->pReaderPathMap = rpm;
 }
 
+/*
+ * 设置地图集覆盖范围，适用TDOA
+ *
+*/
+void Card::set_reader_path(TDOAReaderPathMap* trpm)
+{
+	this->pTdoaReaderPathMap = trpm;
+}
+
 bool Card::is_pos_state_changed( int nval )  // 考勤
 {
 	if(this->pos_state == this->pos_state_old){ 
@@ -397,11 +411,30 @@ void Card::set_anchors( int cnt )
 	this->t = coor_dest->t;
 	//}
 }*/
+
+/*
+ * 采用TOF或者TDOA算法进行定位计算
+ *
+ * param
+ *		cnt		------	_dists数据条数
+ *
+ * return
+ *		无返回值
+ *
+*/
 void Card::get_coordinate( int cnt )
 {
+	int i = 0, nCount = 0;
+	POS *p = NULL;
+	POS tmpPos;
+	tmpPos.posx = 0;
+	tmpPos.posy = 0;
+	tmpPos.pos_radius = 0;
+
+#ifdef ALGORITHM_TOF
 //	if(cnt < 2) return; // 只有一个测量值，忽略不计算
 	int difT = 1;
-	int i = 0, nCount = 0;
+	//int i = 0, nCount = 0;
 	bool bfound = false;
 	DistMap::iterator it = _dists.front()->mp_dist.begin();
 	if(cnt > 2){
@@ -455,11 +488,11 @@ void Card::get_coordinate( int cnt )
 	info_pre.ant = p_dists_locate[0]->antenna_id;
 	info_pre.dist = p_dists_locate[0]->d;
 
-	POS *p = NULL;
+	/*POS *p = NULL;
 	POS tmpPos;
 	tmpPos.posx = 0;
 	tmpPos.posy = 0;
-	tmpPos.pos_radius = 0;
+	tmpPos.pos_radius = 0;*/
 
 	if(info_pre.t == 5){
 		int sss = 0;
@@ -473,7 +506,7 @@ void Card::get_coordinate( int cnt )
 		dist = p_dists_locate[i]->d;
 		
 		//p = TOFLocateAlgorithm::Pos(station,sta_num,ant,dist,info_pre);
-		p = TOFLocateAlgorithm::Pos(*(this->pReaderPathMap),sta_num,ant,dist,info_pre);
+		p = LocateAlgorithm::Pos(*(this->pReaderPathMap),sta_num,ant,dist,info_pre);
 
 		if(p == NULL)
 		{
@@ -531,6 +564,77 @@ void Card::get_coordinate( int cnt )
 	this->v = ((int)(this->last_locate.v*1000))/1000.0;
 	this->z = 0;
 	this->a = 0;
+
+	/*ReaderPath* prp = new ReaderPath();
+	prp->x[0] = -1;
+	prp->x[1] = 1;
+	prp->y[0] = 0;
+	prp->y[1] = 0;
+
+	prp->px[0] = -1;
+	prp->px[1] = 1;
+	prp->py[0] = 0;
+	prp->py[1] = 0;
+
+	dist = 0.5;
+
+	LocateAlgorithm::GetPos(prp,dist,0);*/
+
+#elif defined ALGORITHM_TYPE_TDOA
+
+	//模拟数据
+	//_dists.front()->mp_dist.begin()->second->reader_id = 27;
+	//end
+	//TDOA
+	DistMap::iterator it = _dists.front()->mp_dist.begin();
+	ReceiveDataMap pRdm;
+	pRdm.clear();
+	for(it;it!=_dists.front()->mp_dist.end();it++){
+		//ReceiveDataMap::iterator prdm_it = pRdm.find(nCount);
+		ReceiveDataMap::iterator prdm_it = pRdm.find(it->second->reader_id);
+		if(prdm_it == pRdm.end()){
+			ReceiveData* prd = new ReceiveData();	
+			prd->reader_id = it->second->reader_id;
+			prd->antenna_id = it->second->antenna_id;
+			prd->rec_time_stamp = it->second->t;
+			//pRdm[nCount]  = *prd;
+			pRdm.insert(ReceiveDataMap::value_type(prd->reader_id,*prd));
+			//nCount++;
+		}		
+	}
+
+
+	pRdm.clear();
+
+	ReceiveData* prd1 = new ReceiveData();	
+	prd1->reader_id = 27;
+	prd1->antenna_id = 1;
+	prd1->rec_time_stamp = 1000;
+	//pRdm[nCount]  = *prd;
+	pRdm.insert(ReceiveDataMap::value_type(prd1->reader_id,*prd1));
+
+	ReceiveData* prd2 = new ReceiveData();	
+	prd2->reader_id = 28;
+	prd2->antenna_id = 1;
+	prd2->rec_time_stamp = 1900;
+	//pRdm[nCount]  = *prd;
+	pRdm.insert(ReceiveDataMap::value_type(prd2->reader_id,*prd2));
+
+	if(pTdoaReaderPathMap->size() > 0){
+		p = LocateAlgorithm::Pos(&pRdm,*pTdoaReaderPathMap);
+	}
+	this->map_scale = 0.02;
+
+	this->x = p->posx / (this->map_scale*1.0);
+	this->y = p->posy / (this->map_scale*1.0);
+	this->v = 0;
+	this->z = 0;
+	this->a = 0;
+		
+#else
+
+#endif
+
 }
 
 //void Card::InitStation()

classdef.h

@@ -39,6 +39,10 @@
 //#define MAP_SCALE 0.77  
 //#define MAP_SCALE 7 
 
+//算法类型
+//#define ALGORITHM_TYPE_TOF
+#define ALGORITHM_TYPE_TDOA
+
 using namespace std;
 
 enum TIMER_ID
@@ -221,27 +225,19 @@ typedef map<int, _call_info_reader*> CallInfoReaderMap;
 
 const int MAX_CALC_POINTS = 30;
 
-class ReaderPath{
-public:
-	ReaderPath(){
-		nRealCalcPoints = 0;
-		x[0] = x[1] = y[0] = y[1] = 0;
-		for(int i=0;i<MAX_CALC_POINTS;i++){
-			px[i] = py[i] = pz[i] = 0;
-		}
-	}
-public:
-	//int nType;	//0---表示px,py使用2个参数，1表示px,py使用3个参数
-	int nRealCalcPoints;
-	double x[2];
-	double y[2];
-	double z[2];
-	double px[MAX_CALC_POINTS];
-	double py[MAX_CALC_POINTS];
-	double pz[MAX_CALC_POINTS];
+// 分站接收时间定义
+struct ReceiveData{
+	unsigned int reader_id;		// 分站号
+	unsigned short antenna_id;  // 天线号
+	long long rec_time_stamp;	// 分站接收时间，一个5字节的无符号数
+	ReceiveData(){
+		reader_id = -1;
+		antenna_id = -1;
+		rec_time_stamp = 0;
+	};
 };
 
-typedef map<int,ReaderPath*> ReaderPathMap;
+typedef unordered_map<int,ReceiveData> ReceiveDataMap;
 
 //地图数据 2016-09-12 start
 //const int STATION_MAX = 40;
@@ -268,6 +264,7 @@ struct INFO_PRE{
 struct POS{
 	double posx;
 	double posy;
+	double posz;
 	double pos_radius;
 };
 
@@ -285,6 +282,7 @@ class Reader;
 class Card;
 class MapInfo;
 class Dept;
+class ReaderPath;
 
 typedef map<string, Card*> CardMap;
 typedef map<int, Area*> AreaMap;
@@ -293,6 +291,9 @@ typedef map<int, MapInfo*> MapInfoMap;
 typedef map<int, Dept*> DeptMap;
 typedef map<int, string> AlarmTypeMap;
 typedef map<string, _coordinate*> DistMap;
+typedef map<int,ReaderPath*> ReaderPathMap;
+//typedef map<int,ReaderPathMap> TDOAReaderPathMap;
+typedef unordered_map<int,ReaderPathMap> TDOAReaderPathMap;
 
 struct DistQueItem
 {
@@ -543,6 +544,7 @@ public:
 	int reader_id; // 当前分站
 	Reader* p_reader;
 	ReaderPathMap* pReaderPathMap;
+	TDOAReaderPathMap* pTdoaReaderPathMap;
 	_coordinate** p_dists;
 	_coordinate** p_dists_locate;
 	_coordinate last_locate;
@@ -588,6 +590,7 @@ public:
 	double get_speed();
 	int get_effictive_dist_count(int offset = 0);
 	void set_reader_path(ReaderPathMap *rpm);
+	void set_reader_path(TDOAReaderPathMap* trpm);
 public:
 	// 滤波算法相关
 	//CalLocation* cal_location;
@@ -643,4 +646,28 @@ public:
 	CardMap dept_card_list_vehicle;
 };
 
+/*
+ * 地图集路径描述
+*/
+class ReaderPath{
+public:
+	ReaderPath(){
+		nRealCalcPoints = 0;
+		x[0] = x[1] = y[0] = y[1] = 0;
+		for(int i=0;i<MAX_CALC_POINTS;i++){
+			px[i] = py[i] = pz[i] = 0;
+		}
+	}
+public:
+	//对于TOF而言，x,y表示1,2天线对应的坐标
+	//对于TDOA而言，x,y表示焦点F1,F2（等同于分站i,i+1）的坐标
+	int nRealCalcPoints;
+	double x[2];
+	double y[2];
+	double z[2];
+	double px[MAX_CALC_POINTS];
+	double py[MAX_CALC_POINTS];
+	double pz[MAX_CALC_POINTS];
+};
+
 #endif //YASERVER_CLASSDEF_H_

constdef.h

@@ -12,6 +12,7 @@
 #define PARAM_NAME_VERSION "version"
 #define JSON_ROOT_KEY_CMD "cmd"
 #define JSON_ROOT_KEY_DATA "data"
+#define JSON_ROOT_KEY_DIFF_TICK_COUNT "difftick"
 // ²É¼¯¶Ë½ÓÊÕ½Ó¿Ú
 #define JSON_CMD_VALUE_SET_LIMIT_VALUE "set_limit_value"
 #define JSON_CMD_VALUE_CALL_CARD_START "call_card_start"

locate_algorithm.cpp

@@ -3,6 +3,7 @@
 #include <math.h>
 #include <algorithm>
 #include "../common/matrix/_Matrix.h"
+#include "../common/Functions/Functions.h"
 using namespace std;
 
 // ant 天线， tar定位目标， d 测距
@@ -662,7 +663,7 @@ RESULT* TOFLocateAlgorithm::GetPos(STATION *s,int sta_num,int ant,double dist,in
 	return result;
 }*/
 
-POS* TOFLocateAlgorithm::Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre)
+POS* LocateAlgorithm::Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre)
 {
 	POS* p = new POS;
 	p->posx = 0;
@@ -733,7 +734,7 @@ POS* TOFLocateAlgorithm::Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,I
 	return p;
 }
 
-RESULT* TOFLocateAlgorithm::GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i)
+RESULT* LocateAlgorithm::GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i)
 {
 	double x1 = 0.0;
 	double y1 = 0.0;
@@ -819,4 +820,260 @@ RESULT* TOFLocateAlgorithm::GetPos(ReaderPathMap rpm,int sta_num,int ant,double
 	return result;
 }
 
-//STATION station[STATION_MAX];
+/*
+ * TDOA算法实现
+ * 函数名：Pos
+ *		遍历求解
+ * param
+ *		pRdm	------		标签到达分站的时刻数组
+ *	    trpm    ------		地图集
+ *
+ * return
+ *	    返回最终结果坐标
+ *
+*/
+POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
+{
+	//如果只有一条记录，则直接返回，无法求解
+	if(pRdm->size() <= 1){
+		return NULL;
+	}
+
+	POS *pos = NULL;
+	pos = new POS;
+
+	double a = 0;
+	_coordinate res;
+	bool bFirst = false;
+	int nIdx = 0;
+	for(ReceiveDataMap::iterator first = pRdm->begin();first!=pRdm->end();first++){
+		ReceiveDataMap::iterator second = pRdm->begin();
+		std::advance(second,1);
+
+		if(second == pRdm->end()){
+			continue;
+		}
+
+		ReceiveData f1;
+		f1.antenna_id = first->second.antenna_id;
+		f1.reader_id = first->second.reader_id;
+		f1.rec_time_stamp = first->second.rec_time_stamp;
+
+		for(;second != pRdm->end();second++){
+			ReceiveData f2;
+			f2.antenna_id = second->second.antenna_id;
+			f2.reader_id = second->second.reader_id;
+			f2.rec_time_stamp = second->second.rec_time_stamp;
+
+			double diffTime = f1.rec_time_stamp - f2.rec_time_stamp;
+			double dist = CFunctions::getDistance(diffTime);
+
+			//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
+			TDOAReaderPathMap::iterator rdm_it = trpm.find(f1.reader_id);
+			if(rdm_it == trpm.end()){
+				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;
+			}
+
+			ReaderPath * pRP = trpm.find(f1.reader_id)->second.find(f2.reader_id)->second;
+
+			int k = 0;
+
+			int seg_num = pRP->nRealCalcPoints - 1;
+
+			if(seg_num == 0){
+				continue;
+			}
+
+			for(int i = 0;i < seg_num;i++){
+				RESULT * r = NULL;
+				r = GetPos(pRP,dist,i);
+				if(r == NULL){
+					continue;
+				}
+
+				//因为双曲线与分站之间第i条线段或者第j条线段分别有两焦点
+				//或者分站之间就一条直线，有两焦点
+				double xcross[2] = {0};
+				double ycross[2] = {0};
+
+				for(int j = 0; j < 2;j++){
+					xcross[j] = r->x[j];
+					ycross[j] = r->y[j];
+				}
+
+				double deltad[2] = {0};
+				for(int j = 0; j < 2;j ++){
+					double d[2] = {0};
+					double dx1 = r->x[j] - pRP->x[0];
+					double dy1 = r->y[j] - pRP->y[0];
+					d[0] = sqrt(pow(dx1,2) + pow(dy1,2));
+
+					double dx2 = r->x[j] - pRP->x[1];
+					double dy2 = r->y[j] - pRP->y[1];
+					d[1] = sqrt(pow(dx2,2) + pow(dy2,2));
+
+					deltad[j] = d[0] - d[1];
+				}
+
+				//解的索引
+				int idx = 0;
+				if(dist > 0){
+					for(int j = 0;j < 2;j++){
+						if(deltad[j] > 0){
+							idx = j;
+							break;
+						}
+					}	
+				}else{
+					for(int j = 0;j < 2;j++){
+						if(deltad[j] < 0){
+							idx = j;
+							break;
+						}
+					}	
+				}
+
+				if(!bFirst){
+					res.x = xcross[idx];
+					res.y = ycross[idx];
+					bFirst = true;
+				}else{
+					//例如当分站为B1B3,B2B3的情况下
+					//将两解做误差判别，如果两次误差在0.3之内，则做平均
+					double tempx = xcross[idx];
+					double tempy = ycross[idx];
+
+					double diffD = sqrt(pow(res.x - tempx,2) + pow(res.y - tempy,2));
+
+					if(diffD - 0.3 < 0){
+						res.x = (tempx + res.x)/2.0;
+						res.y = (tempy + res.y)/2.0;
+					}
+
+					//res.x = (tempx + res.x)/2.0;
+					//res.y = (tempy + res.y)/2.0;
+				}
+
+				if(r){
+					delete r;
+					r = NULL;
+				}
+			}
+		}
+	}
+
+	pos->posx = res.x;
+	pos->posy = res.y;
+
+	return pos;
+}
+
+/*
+ * TDOA算法求解实现
+ * 函数名：GetPos
+ *		实现直线和双曲线求交点
+ * param
+ *		pRP    ------ 分站路径
+ *      dist   ------ 距离差
+ *      i	   ------ 第i条线段
+ *
+ * return
+ *      满足条件的解，最多两个
+ *
+*/
+RESULT* LocateAlgorithm::GetPos(ReaderPath * pRP,double dist,int i)
+{
+	//解的个数
+	int count = 0;
+
+	//解的坐标
+	double x[2] = {0};
+	double y[2] = {0};
+
+	//双曲线的两个焦点分别是
+	double x1 = pRP->x[0];
+	double y1 = pRP->y[0];
+
+	double x2 = pRP->x[1];
+	double y2 = pRP->y[1];
+
+	//ReaderPath * pRP = trpm.find(start.reader_id)->second.find(end.reader_id)->second;
+
+	if(pRP->px[i] - pRP->px[i+1] == 0){
+		//x相等，双曲线在Y轴上
+		x[0] = x[1] = pRP->px[i];
+		y[0] = (y1 + y2 + dist)/2.0;
+		y[1] = (y1 + y2 - dist)/2.0;
+	}
+	else{
+		//双曲线和直线相交
+		//直线常数求解
+		double k = (pRP->py[i+1] - pRP->py[i])/(pRP->px[i+1] - pRP->px[i]);
+		double kb = pRP->py[i] - k*pRP->px[i];
+
+		//求解的常数中间量
+		double d1 = 2*(x2 - x1);//m
+		double d2 = 2*(y1 + y2);//n
+		double d3 = pow(y1,2) + pow(x1,2)- pow(dist,2) - pow(y2,2)  - pow(x2,2);//p
+		//方程ax^2 + bx + c = 0
+		double a = pow(d1 - d2*k,2) - 4*pow(dist,2)*(pow(k,2) + 1);
+		double b = 2*((d3 - d2*kb)*(d1 - d2*k) - 4*pow(dist,2)*(k*(kb - y2) - x2));
+		double c = pow(d3 - d2*kb,2) - 4*pow(dist,2)*(pow(x2,2) + pow(kb-y2,2));
+
+		double delta = pow(b,2) - 4*a*c;
+		if(delta > 0){
+			count = 2;
+		}
+		else if(delta == 0){
+			count = 1;
+		}
+		else{
+			count = 0;
+		}
+
+		if(count == 0){
+			return NULL;
+		}
+
+		x[0] = -(b + sqrt(delta))/(2*a);
+		x[1] = (-b + sqrt(delta) )/(2*a);
+		y[0] = k*x[0] + kb;
+		y[1] = k*x[1] + kb;	
+	}
+
+	//判断两解是否在线段范围内
+	if(count > 0){
+		for(int t = 0; t < count ;t++){
+			//两分站之间第i条线段的两端点
+			if(x[t] < min(pRP->px[i],pRP->px[i+1])||
+			   x[t] > max(pRP->px[i],pRP->px[i+1])||
+			   y[t] < min(pRP->py[i],pRP->py[i+1])||
+			   y[t] > max(pRP->py[i],pRP->py[i+1])
+				)
+			{
+				x[t] = 0;
+				y[t] = 0;
+			}
+		}
+	}
+
+	RESULT* result = new RESULT;
+	memset(result->x,0,3*sizeof(double));
+	memset(result->y,0,3*sizeof(double));
+	//memset(result->z,0,3*sizeof(double));
+	result->x[0] = x[0];
+	result->x[1] = x[1];
+	result->y[0] = y[0];
+	result->y[1] = y[1];
+	//result->z[0] = 0;
+	//result->z[1] = 0;
+
+	return result;
+}
+
+//STATION station[STATION_MAX];ReaderPathMap rpm

locate_algorithm.h

@@ -31,15 +31,19 @@ bool is_all_inline(_coordinate** coor_list, int cnt );
 //extern STATION station[STATION_MAX];
 
 /*
- * TOF定位算法
+ * 定位算法
  *
 */
-class TOFLocateAlgorithm{
+class LocateAlgorithm{
 public:
+	//TOF算法函数
 	//static POS * Pos(STATION *s,int sta_num,int ant,double dist,INFO_PRE info_pre);
 	//static RESULT* GetPos(STATION *s,int sta_num,int ant,double dist,int i);
 	static POS* Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre);
 	static RESULT* GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i);
+	//TDOA算法函数
+	static POS* Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm);
+	static RESULT* GetPos(ReaderPath * pRP,double dist,int i);
 };
 
 #endif