update algo

YAServerDlg.cpp

@@ -209,6 +209,7 @@ DWORD WINAPI _exec_sql(LPVOID lparam)
 
 	if(err > 0){
 		TRACE(_T("sql error \n"));
+
 	}
 	mysql_free_result(pRes);
 	//TRACE(_T("passed\n"));
@@ -1621,8 +1622,12 @@ int CYAServerDlg::init_tdoa_all_readers_coverage(int reader_id)
 			prp->z[1] = 0;	
 			prp->bIsInit = true;
 		}
-		prp->px[ndx-1] = atof(pRow[3])*mapScale;
-		prp->py[ndx-1] = atof(pRow[4])*mapScale;
+		if(prp->px[ndx-1] == 0 && prp->py[ndx-1] == 0){
+			prp->px[ndx-1] = atof(pRow[3])*mapScale;
+			prp->py[ndx-1] = atof(pRow[4])*mapScale;
+		}
+		/*prp->px[ndx-1] = atof(pRow[3])*mapScale;
+		prp->py[ndx-1] = atof(pRow[4])*mapScale;*/
 
 		if(!prp_r->bIsInit){
 			//冗余信息
@@ -1635,8 +1640,14 @@ int CYAServerDlg::init_tdoa_all_readers_coverage(int reader_id)
 
 			prp_r->bIsInit = true;
 		}
-		prp_r->px[ndx-1] = atof(pRow[3])*mapScale;
-		prp_r->py[ndx-1] = atof(pRow[4])*mapScale;
+		if(prp_r->px[ndx-1] == 0 && prp_r->py[ndx-1] == 0){
+			prp_r->px[ndx-1] = atof(pRow[3])*mapScale;
+			prp_r->py[ndx-1] = atof(pRow[4])*mapScale;
+		}
+
+
+		/*prp_r->px[ndx-1] = atof(pRow[3])*mapScale;
+		prp_r->py[ndx-1] = atof(pRow[4])*mapScale;*/
 
 		//prp_r->nRealCalcPoints++;
 	}
@@ -1659,14 +1670,14 @@ void CYAServerDlg::ws_init()
 	using std::placeholders::_1;
 	using std::placeholders::_2;
 	using std::placeholders::_3;
-	using std::placeholders::_4;
+	using std::placeholders::_4;   
 	socket::ptr sock = _io->socket();
-	//default socket opened, also we have "set_open_listener" for monitoring physical connection opened.
-	//sock->on(JSON_CMD_VALUE_USER, sio::socket::event_listener_aux(std::bind(&CYAServerDlg::OnLoginMessage,this,_1,_2,_3,_4)));
+
 	sock->on(JSON_CMD_VALUE_USER, sio::socket::event_listener_aux(std::bind(&CYAServerDlg::OnLogin, this, _1,_2,_3,_4)));
 	sock->on(JSON_CMD_VALUE_CALL, sio::socket::event_listener_aux(std::bind(&CYAServerDlg::OnCallMessage,this,_1,_2,_3,_4)));
 	sock->on(JSON_CMD_VALUE_PUSH, sio::socket::event_listener(std::bind(&CYAServerDlg::OnPushMessage, this)));
-	//sock->on(JSON_CMD_VALUE_USER, sio::socket::event_listener(std::bind(&CYAServerDlg::OnLoginCallBack, this, std::placeholders::_1)));
+
+	//default socket opened, also we have "set_open_listener" for monitoring physical connection opened.
 	_io->set_socket_open_listener(std::bind(&CYAServerDlg::OnConnected, this));
 	//_io->set_open_listener(std::bind(&CYAServerDlg::OnConnected, this));
 	//physical connection closed or drop.
@@ -2241,6 +2252,7 @@ void CYAServerDlg::parse_data_locate_card_vehicle(BYTE* DataBuffer, int& nCurPos
 	}
 
 #ifdef ALGORITHM_TYPE_INS
+	//根据获取的状态更新值来判别
 	if(card->accelerate_state != btAcceleration){
 		//如果加速度状态发生了改变，就重置惯导的权重值
 		card->ins_weight = INS_WEIGHT;
@@ -2250,6 +2262,7 @@ void CYAServerDlg::parse_data_locate_card_vehicle(BYTE* DataBuffer, int& nCurPos
 		card->time_stamp = wCardTickCount;
 		card->ranging_type = btType;
 		card->power_state = btPower;
+		card->accelerate_state_last = card->accelerate_state;
 		card->accelerate_state = btAcceleration;
 		card->flying_time = lTime1;
 		card->sync_num = wSyncNum;
@@ -2275,6 +2288,7 @@ void CYAServerDlg::parse_data_locate_card_vehicle(BYTE* DataBuffer, int& nCurPos
 			card->time_stamp = wCardTickCount;
 			card->ranging_type = btType;
 			card->power_state = btPower;
+			card->accelerate_state_last = card->accelerate_state;
 			card->accelerate_state = btAcceleration;
 			card->flying_time = lTime1;
 			card->sync_num = wSyncNum;
@@ -2284,6 +2298,7 @@ void CYAServerDlg::parse_data_locate_card_vehicle(BYTE* DataBuffer, int& nCurPos
 			card->time_stamp = wCardTickCount;
 			card->ranging_type = btType;
 			card->power_state = btPower;
+			card->accelerate_state_last = card->accelerate_state;
 			card->accelerate_state = btAcceleration;
 			card->flying_time = lTime1;
 			card->sync_num = wSyncNum;
@@ -5646,7 +5661,7 @@ void CYAServerDlg::load_his_card_postion()
 	MYSQL_ROW pRow;
 	int err = 0;
 	char buf[LENGTH_SQL] = {0};
-	sprintf_s(buf, "select l.card_id, l.cur_time, l.x, l.y, l.z, la.map_id, la.area_id, la.enter_time, att.start_time \
+	/*sprintf_s(buf, "select l.card_id, l.cur_time, l.x, l.y, l.z, la.map_id, la.area_id, la.enter_time, att.start_time \
 				   from his_location l, (select card_id, max(cur_time) cur_time from his_location where cur_time >= date_sub(now(), interval 2 hour) group by card_id) lx, \
 				   (select la.* from his_location_area la, (select card_id, max(enter_time) enter_time from his_location_area where leave_time is null group by card_id) lax \
 				   where la.card_id = lax.card_id and la.enter_time = lax.enter_time and la.leave_time is null) la, \
@@ -5654,15 +5669,15 @@ void CYAServerDlg::load_his_card_postion()
 				   where att.card_id = attx.card_id and att.start_time = attx. start_time and att.end_time is null) att \
 				   where l.card_id = lx.card_id and l.cur_time = lx.cur_time \
 				   and l.card_id = la.card_id  and l.area_id = la.area_id and l.cur_time >= la.enter_time \
+				   and l.card_id = att.card_id and l.cur_time >= att.start_time;");*/
+	sprintf_s(buf, "select l.card_id, l.cur_time, l.x, l.y, l.z, la.map_id, la.area_id, la.enter_time, att.start_time \
+				   from his_location l \
+				   ,(select l.card_id, l.cur_time from his_location l where not exists(select 1 from his_location where card_id=l.card_id and cur_time>l.cur_time) and cur_time>= date_sub(now(), interval 2 hour)) lx \
+				   ,(select la.card_id, la.map_id, la.area_id, la.enter_time from his_location_area la where not exists(select 1 from his_location_area where card_id=la.card_id and enter_time > la.enter_time) and la.leave_time is null) la \
+				   ,(select att.card_id, att.start_time from rpt_attendance att where not exists(select 1 from rpt_attendance where card_id=att.card_id and start_time>att.start_time) and att.end_time is null) att \
+				   where l.cur_time >= date_sub(now(), interval 2 hour) and l.card_id = lx.card_id and l.cur_time = lx.cur_time \
+				   and l.card_id = la.card_id  and l.area_id = la.area_id and l.cur_time >= la.enter_time \
 				   and l.card_id = att.card_id and l.cur_time >= att.start_time;");
-	//sprintf_s(buf, "select l.card_id, l.cur_time, l.x, l.y, l.z, la.map_id, la.area_id, la.enter_time, att.start_time \
-	//			   from his_location l \
-	//			   ,(select l.card_id, l.cur_time from his_location l where not exists(select 1 from his_location where card_id=l.card_id and cur_time>l.cur_time) and cur_time>= date_sub(now(), interval 2 hour)) lx \
-	//			   ,(select la.card_id, la.map_id, la.area_id, la.enter_time from his_location_area la where not exists(select 1 from his_location_area where card_id=la.card_id and enter_time > la.enter_time) and la.leave_time is null) la \
-	//			   ,(select att.card_id, att.start_time from rpt_attendance att where not exists(select 1 from rpt_attendance where card_id=att.card_id and start_time>att.start_time) and att.end_time is null) att \
-	//			   where l.cur_time >= date_sub(now(), interval 2 hour) and l.card_id = lx.card_id and l.cur_time = lx.cur_time \
-	//			   and l.card_id = la.card_id  and l.area_id = la.area_id and l.cur_time >= la.enter_time \
-	//			   and l.card_id = att.card_id and l.cur_time >= att.start_time;");
 	//pRes = pConn->Execute(buf, err);
 	pRes = pConn->Execute(buf, err);
 	int nRow = 0;
@@ -5682,7 +5697,6 @@ void CYAServerDlg::load_his_card_postion()
 
 			it->second->area_id = atoi(pRow[5]);
 			it->second->map_id = atoi(pRow[6]);
-			//it->second->map_scale = 
 			enter_time = pRow[7];
 			API_StringToTime(enter_time, it->second->enter_area_time);
 			start_time = pRow[8];
@@ -7090,12 +7104,12 @@ 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_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);
+					strLog.Format(_T("%d,%f,%f,%f,%f,%f,iw: %f,uw: %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->z,card->ins_weight,card->uwb_weight,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);
+						strLog.Format(_T("%d,%f,%f,%f,%f,%f,iw: %f,uw: %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->z,card->ins_weight,card->uwb_weight,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;
 					}

classdef.cpp

@@ -80,7 +80,11 @@ 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;
+	ins_weight = INS_WEIGHT;
+	uwb_weight = UWB_WEIGHT;
+	weight = 0;
+	acce_cur_state = 0;
+	acce_last_state = 0;
 
 	time_stamp_last = 0;
 	reader_id_last  = 0;
@@ -141,232 +145,50 @@ 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){
-		if(this->pos_state_count < nval){
-			this->pos_state_count++;
-		}
-		return false;
-	}
-	//if(this->pos_state_count < nval ){ // 未达到确认次数
-	//	this->pos_state_count++;
-	//	return false;
-	//}
-	this->pos_state_old = this->pos_state;
-	this->pos_state_count = 1;
-	return true;
-	//if(last_area_type_id != cur_area_type_id){
-	//	if(0 ==last_area_type_id || 0==cur_area_type_id){
-	//		return true;
-	//	}
-	//}
-	//return false;
-	//return (this->last_area_type_id != this->cur_area_type_id && 0 == (this->cur_area_type_id & this->last_area_type_id));
-}
-
-void Card::add_dist(_coordinate* dist)
-{
-	EnterCriticalSection(&m_csCard);
-	string s = concat(dist->reader_id, dist->antenna_id);
-	
-	int idx = FindDistMap(dist->t);
-
-	if(-1 == idx){
-		DistQueMapItem dq;
-		dq.cardstamp = dist->t;
-		dq.distmap[s] = dist;
-		_dists.push_back(dq);
-	}else{
-		_dists[idx].distmap[s] = dist;
-	}
-
-	if(_dists.size() >= MAX_DIST_CACHE){ // 超过缓存数量限制
-		// 计算并删除第一个
-		get_coordinate();
-		remove_dist_head();
-	}
-	LeaveCriticalSection(&m_csCard);
-}
-
-time_t Card::get_working_time()
-{
-	//return (card_type == CT_VEHICLE) ? leave_park_time : down_time;
-	return down_time;
-}
-
-int Card::get_effictive_dist_count( int offset /*= 0*/ )
-{
-	if(0 == _dists.size()) return 0;
-	time_stamp_cal = _dists.front().cardstamp;
-	return (_dists.front().distmap.size());
-
-	//int nTimeStamp;
-	//if(time_stamp_max == 0 && offset != 0){
-	//	nTimeStamp = 0xFFFF;
-	//}else{
-	//	nTimeStamp = time_stamp_max - offset;
-	//}
-	//int ret = 0, tmp = -1;
-	//for(int i = 0; i < DIST_COUNT; i++){
-	//	if(NULL == p_dists[i]) break;
-	//	if(p_dists[i]->t == nTimeStamp){
-	//		ret++;
-	//	}
-	//}
-	//return ret;
-}
-
-Card::~Card(void)
-{
-	if(m_pKalmanFilter){
-		delete m_pKalmanFilter;
-		m_pKalmanFilter = NULL;
-	}
-
-	for(int i = 0;i<DIST_COUNT;i++){
-		delete p_dists[i];
-		p_dists[i] = NULL;
-		delete p_dists_locate[i];
-		p_dists_locate[i] = NULL;
-	}
-	
-	if(p_dists){
-		delete[] p_dists;
-		p_dists = NULL;
-	}
-
-	if(p_dists_locate){
-		delete[] p_dists_locate;
-		p_dists_locate = NULL;
-	}
-
-	if(p3_anchors){
-		delete[] p3_anchors;
-		p3_anchors = NULL;
-	}
-
-	if(cal_location3){
-		delete cal_location3;
-	}
-
-	DeleteCriticalSection(&m_csCard);
-}
-
-void Card::get_coordinate_2d( int cnt )
-{
-	//double d_dist[3];
-	//Point2 pt2, pt2_1;
-	//for(int i = 0; i < cnt; i++){
-	//	d_dist[i] = p_dists_locate[i]->d;
-
-	//}
-	//for(int i=0; i< cnt; i++){
-	//	if(p_dists[i]->reader_id == 1){
-	//		p2_anchors[0].x = p_dists[i]->x;
-	//		p2_anchors[0].y = p_dists[i]->y;
-	//		d_dist[0] = p_dists[i]->d;
-	//	}else if(p_dists[i]->reader_id == 2){
-	//		p2_anchors[1].x = p_dists[i]->x;
-	//		p2_anchors[1].y = p_dists[i]->y;
-	//		d_dist[1] = p_dists[i]->d;
-	//	}if(p_dists[i]->reader_id == 3){
-	//		p2_anchors[2].x = p_dists[i]->x;
-	//		p2_anchors[2].y = p_dists[i]->y;
-	//		d_dist[2] = p_dists[i]->d;
-	//	}
-	//}
-	//Mat mat(3, 1, d_dist);
-
-	//if(is_anchor_changed){
-	//	cal_location->setAnchors(p2_anchors);
-	//	pt2 = cal_location->initPos(mat);
-	//} else{
-	//	pt2 = cal_location->estiPos(mat);
-	//}
-	//pt2_1 = cal_location->getError();
-	//x = pt2.x;
-	//y = pt2.y;
-	//z = 0;
-	//x1 = pt2_1.x;
-	//y1 = pt2_1.y;
-	//z1 = 0;
-}
-
-void Card::get_coordinate_3d( int cnt )
-{
-	double d_dist[ANCHOR_COUNT];
-	Point3 pt3, pt3_1;
-	for(int i = 0; i < cnt; i++){
-		d_dist[i] = p_dists_locate[i]->d;
-	}
-
-	Mat mat(ANCHOR_COUNT, 1, d_dist);
-	if(is_anchor_changed){
-		set_anchors(cnt);
-		pt3 = cal_location3->initPos(mat);
-	} else{
-		pt3 = cal_location3->estiPos(mat);
-	}
-	pt3_1 = cal_location3->getError();
-
-	x = pt3.x;
-	y = pt3.y;
-	z = pt3.z;
-	x1 = pt3_1.x;
-	y1 = pt3_1.y;
-	z1 = pt3_1.z;
-}
-
-double Card::get_speed()
-{
-	double speed = 0; 
-	// 计算速度
-	return speed;
-}
-
-void Card::set_anchors( int cnt )
-{
-	for(int i = 0; i < cnt; i++){
-		p3_anchors[i].x = p_dists[i]->x;
-		p3_anchors[i].y = p_dists[i]->y;
-		p3_anchors[i].z = p_dists[i]->z;
-	}
-	cal_location3->setAnchors(p3_anchors, ANCHOR_COUNT);
-	is_anchor_changed = false;
-}
-
 bool operator==(sync_data& a,sync_data&b){
 	return a.sync_num == b.sync_num;
 }
 
 /*
- * 采用TOF或者TDOA算法进行定位计算
+ * 此函数主要判断此坐标是否需要形成json输出给webserver
  *
  * param
- *		cnt		------	_dists数据条数
+ *		acce_state		算法中保存的当前加速度计状态
+ *		acce_state_last	算法中保存的上一次加速度计状态
  *
  * return
- *		无返回值
- *
+ *		无
 */
-void Card::get_coordinate( int cnt )
+void Card::inspect_coordinate(int acce_state,int acce_state_last)
+{
+	//如果是静止或者怠速状态，每次的定位坐标不变化，此时要求采集不再将此数据送往webserver
+	if (acce_state == STATE_ACCE_STATIC)
+	{
+		this->isdealed = false;
+	}
+	else if (acce_state == STATE_ACCE_MOTION)
+	{
+		//如果运动状态时，
+		int nSign = 0;
+		int nState = 0;
+
+	}
+}
+
+void Card::algo_tof(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;
 	bool bfound = false;
-	DistMap::iterator it = _dists.front()->mp_dist.begin();
+	DistMap::iterator it = _dists.front().distmap.begin();
 	if(cnt > 2){
-		for(; it != _dists.front()->mp_dist.end(); ++it){
+		for(; it != _dists.front().distmap.end(); ++it){
 			bfound = false;
 			// 判断是否已经加入到列表
 			for(int j=0; j < i; j++){
@@ -382,7 +204,7 @@ void Card::get_coordinate( int cnt )
 		}
 		nCount = i;
 	}else{
-		for(; it != _dists.front()->mp_dist.end(); ++it,i++){
+		for(; it != _dists.front().distmap.end(); ++it,i++){
 			p_dists_locate[i] = it->second;
 		}
 		nCount = cnt;
@@ -401,8 +223,6 @@ void Card::get_coordinate( int cnt )
 		this->last_z = p_dists_locate[0]->z;
 
 		init_postion = true;
-
-		//InitStation();
 	}	
 
 	int sta_num = 0;
@@ -416,24 +236,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 tmpPos;
-	tmpPos.posx = 0;
-	tmpPos.posy = 0;
-	tmpPos.pos_radius = 0;*/
-
-	if(info_pre.t == 5){
-		int sss = 0;
-		sss = info_pre.t;
-	}
-	//this->p_reader->readerCoveragePath
 	for(int i = 1;i < nCount;i++){
-
 		sta_num = p_dists_locate[i]->reader_id - 1;
 		ant = p_dists_locate[i]->antenna_id;
 		dist = p_dists_locate[i]->d;
 		
-		//p = TOFLocateAlgorithm::Pos(station,sta_num,ant,dist,info_pre);
 		p = LocateAlgorithm::Pos(*(this->pReaderPathMap),sta_num,ant,dist,info_pre);
 
 		if(p == NULL)
@@ -483,8 +290,10 @@ void Card::get_coordinate( int cnt )
 	this->v = ((int)(this->last_locate.v*1000))/1000.0;
 	this->z = 0;
 	this->a = 0;
+}
 
-#elif defined ALGORITHM_TYPE_TDOA
+void Card::algo_tdoa(int cnt)
+{
 	if(cnt < 2){
 		return;
 	}
@@ -503,16 +312,11 @@ void Card::get_coordinate( int cnt )
 			if(dst >= 5){
 				bRet = true;
 				break;
-				//continue;
 			}
 		}
 		p_dists_locate[k] = it->second;
 	}
 
-	this->x = 0;
-	this->y = 0;
-	this->z = 0;
-
 	if(bRet){
 		this->x = this->last_locate.x;
 		this->y = this->last_locate.y;
@@ -529,27 +333,30 @@ void Card::get_coordinate( int cnt )
 	bRet = false;
 
 	int maxSyncTimes = 0;
-	int acce_state = 2;
+	int acce_state = 0;
+	int acce_state_last = 0;
 
 	for(int i=0;i<k;i++){
 		if(i==0){
 			maxSyncTimes = p_dists_locate[i]->st;
 			acce_state = p_dists_locate[i]->acce_state;
+			acce_state_last = p_dists_locate[i]->acce_state_last;
 		}
 		else{
 			if(maxSyncTimes < p_dists_locate[i]->st){
 				maxSyncTimes = p_dists_locate[i]->st;
 				acce_state = p_dists_locate[i]->acce_state;
+				acce_state_last = p_dists_locate[i]->acce_state_last;
 			}
 		}
 		ReceiveDataMap::iterator prdm_it = pRdm.find(p_dists_locate[i]->reader_id);
 		if(prdm_it == pRdm.end()){
-
 			if(p_dists_locate[i]->tt == LLONG_MAX ){
+				//如果同步时间戳存在异常值，则不走算法定位，直接返回上一次结果值
 				bRet = true;
 				break;
 			}
-
+			//保存信息用于定位
 			ReceiveData prd;
 			prd.reader_id = p_dists_locate[i]->reader_id;
 			prd.antenna_id = p_dists_locate[i]->antenna_id;
@@ -562,9 +369,19 @@ void Card::get_coordinate( int cnt )
 		}		
 	}
 
-	acce_state = 2;
+	//存在异常，直接返回上一次结果值
+	if(bRet){
+		pRdm.clear();
+		this->x = this->last_locate.x;
+		this->y = this->last_locate.y;
+		this->z = INVALID_COORDINATE;
+		return;
+	}
+
+	int i = 0, nCount = 0;
+	POS *p = NULL;
 	bool bOutput = false;
-	int nSize = 0;
+	//int nSize = 0;
 
 	if(pTdoaReaderPathMap->size() > 0 && pRdm.size() > 1){
 		this->m_nCalcSyncNum = maxSyncTimes;
@@ -671,9 +488,9 @@ void Card::get_coordinate( int cnt )
 					//其他情况，则x2 - x1 > 0 为正
 					if(cx == sd.x){
 						if(cy > sd.y){
-							nSign = 1;
-						}else{
 							nSign = -1;
+						}else{
+							nSign = 1;
 						}
 					}else{
 						if(cx > sd.x){
@@ -711,6 +528,13 @@ void Card::get_coordinate( int cnt )
 					this->last_vx = cvx;
 					this->last_vy = cvy;	
 					cv = cv*3.6;
+
+					//速度的限制
+					if(cv > 30){
+						this->x = this->last_locate.x;
+						this->y = this->last_locate.y;
+						return;
+					}
 					
 					//删除第一个元素到tmp（含）之间的所有元素
 					bool bStartDel = false;
@@ -757,6 +581,12 @@ void Card::get_coordinate( int cnt )
 
 #ifdef ALGORITHM_TYPE_INS
 		bRet = false;
+		if(acce_state!=acce_state_last){
+			//状态发生了变化
+			acce_state = 4;
+		}else{
+			acce_state = 5;
+		}
 		switch(acce_state){
 		case 0:	//熄火
 		case 1:	//怠速
@@ -773,6 +603,26 @@ void Card::get_coordinate( int cnt )
 			//如果根据加速度状态判断为后退，则取-1
 			ins_state = -1;
 			break;
+		case 4:	//状态发生了变化
+			bRet = true;
+			ins_state = this->accelerate_state_last;
+			double cweight = 0;
+			cweight = this->ins_weight * ins_state + this->uwb_weight*uwb_state;
+			if(cweight>=INS_WEIGHT*-1&&cweight<=INS_WEIGHT){
+				this->ins_weight = abs(cweight);
+			}else{
+				this->ins_weight = INS_WEIGHT;
+			}
+			if(k*uwb_weight < 0){
+				//状态改变，这一次的状态和上一次的状态不同
+				bUseKalman = false;
+			}else{
+				bUseKalman = true;
+			}
+			break;
+		case 5:	//状态未发生了变化
+			//如果UWB状态发生了变化，则此次定位有问题	
+			break;
 		}
 		if(!bRet){
 			double cweight = 0;
@@ -966,7 +816,7 @@ void Card::get_coordinate( int cnt )
 				}else{
 					this->x = this->last_locate.x;
 					this->y = this->last_locate.y;
-					this->z = this->last_locate.z;
+					this->z = -5;//this->last_locate.z
 					this->v = this->last_locate.v;
 				}
 				
@@ -1018,10 +868,224 @@ void Card::get_coordinate( int cnt )
 	}
 
 	pRdm.clear();
+}
 
-#else
+bool Card::is_pos_state_changed( int nval )  // 考勤
+{
+	if(this->pos_state == this->pos_state_old){
+		if(this->pos_state_count < nval){
+			this->pos_state_count++;
+		}
+		return false;
+	}
+	//if(this->pos_state_count < nval ){ // 未达到确认次数
+	//	this->pos_state_count++;
+	//	return false;
+	//}
+	this->pos_state_old = this->pos_state;
+	this->pos_state_count = 1;
+	return true;
+	//if(last_area_type_id != cur_area_type_id){
+	//	if(0 ==last_area_type_id || 0==cur_area_type_id){
+	//		return true;
+	//	}
+	//}
+	//return false;
+	//return (this->last_area_type_id != this->cur_area_type_id && 0 == (this->cur_area_type_id & this->last_area_type_id));
+}
+
+void Card::add_dist(_coordinate* dist)
+{
+	EnterCriticalSection(&m_csCard);
+	string s = concat(dist->reader_id, dist->antenna_id);
+	
+	int idx = FindDistMap(dist->t);
+
+	if(-1 == idx){
+		DistQueMapItem dq;
+		dq.cardstamp = dist->t;
+		dq.distmap[s] = dist;
+		_dists.push_back(dq);
+	}else{
+		_dists[idx].distmap[s] = dist;
+	}
+
+	if(_dists.size() >= MAX_DIST_CACHE){ // 超过缓存数量限制
+		// 计算并删除第一个
+		get_coordinate();
+		remove_dist_head();
+	}
+	LeaveCriticalSection(&m_csCard);
+}
+
+time_t Card::get_working_time()
+{
+	//return (card_type == CT_VEHICLE) ? leave_park_time : down_time;
+	return down_time;
+}
+
+int Card::get_effictive_dist_count( int offset /*= 0*/ )
+{
+	if(0 == _dists.size()) return 0;
+	time_stamp_cal = _dists.front().cardstamp;
+	return (_dists.front().distmap.size());
+
+	//int nTimeStamp;
+	//if(time_stamp_max == 0 && offset != 0){
+	//	nTimeStamp = 0xFFFF;
+	//}else{
+	//	nTimeStamp = time_stamp_max - offset;
+	//}
+	//int ret = 0, tmp = -1;
+	//for(int i = 0; i < DIST_COUNT; i++){
+	//	if(NULL == p_dists[i]) break;
+	//	if(p_dists[i]->t == nTimeStamp){
+	//		ret++;
+	//	}
+	//}
+	//return ret;
+}
+
+Card::~Card(void)
+{
+	if(m_pKalmanFilter){
+		delete m_pKalmanFilter;
+		m_pKalmanFilter = NULL;
+	}
+
+	for(int i = 0;i<DIST_COUNT;i++){
+		delete p_dists[i];
+		p_dists[i] = NULL;
+		delete p_dists_locate[i];
+		p_dists_locate[i] = NULL;
+	}
+	
+	if(p_dists){
+		delete[] p_dists;
+		p_dists = NULL;
+	}
 
+	if(p_dists_locate){
+		delete[] p_dists_locate;
+		p_dists_locate = NULL;
+	}
+
+	if(p3_anchors){
+		delete[] p3_anchors;
+		p3_anchors = NULL;
+	}
+
+	if(cal_location3){
+		delete cal_location3;
+	}
+
+	DeleteCriticalSection(&m_csCard);
+}
+
+void Card::get_coordinate_2d( int cnt )
+{
+	//double d_dist[3];
+	//Point2 pt2, pt2_1;
+	//for(int i = 0; i < cnt; i++){
+	//	d_dist[i] = p_dists_locate[i]->d;
+
+	//}
+	//for(int i=0; i< cnt; i++){
+	//	if(p_dists[i]->reader_id == 1){
+	//		p2_anchors[0].x = p_dists[i]->x;
+	//		p2_anchors[0].y = p_dists[i]->y;
+	//		d_dist[0] = p_dists[i]->d;
+	//	}else if(p_dists[i]->reader_id == 2){
+	//		p2_anchors[1].x = p_dists[i]->x;
+	//		p2_anchors[1].y = p_dists[i]->y;
+	//		d_dist[1] = p_dists[i]->d;
+	//	}if(p_dists[i]->reader_id == 3){
+	//		p2_anchors[2].x = p_dists[i]->x;
+	//		p2_anchors[2].y = p_dists[i]->y;
+	//		d_dist[2] = p_dists[i]->d;
+	//	}
+	//}
+	//Mat mat(3, 1, d_dist);
+
+	//if(is_anchor_changed){
+	//	cal_location->setAnchors(p2_anchors);
+	//	pt2 = cal_location->initPos(mat);
+	//} else{
+	//	pt2 = cal_location->estiPos(mat);
+	//}
+	//pt2_1 = cal_location->getError();
+	//x = pt2.x;
+	//y = pt2.y;
+	//z = 0;
+	//x1 = pt2_1.x;
+	//y1 = pt2_1.y;
+	//z1 = 0;
+}
+
+void Card::get_coordinate_3d( int cnt )
+{
+	double d_dist[ANCHOR_COUNT];
+	Point3 pt3, pt3_1;
+	for(int i = 0; i < cnt; i++){
+		d_dist[i] = p_dists_locate[i]->d;
+	}
+
+	Mat mat(ANCHOR_COUNT, 1, d_dist);
+	if(is_anchor_changed){
+		set_anchors(cnt);
+		pt3 = cal_location3->initPos(mat);
+	} else{
+		pt3 = cal_location3->estiPos(mat);
+	}
+	pt3_1 = cal_location3->getError();
+
+	x = pt3.x;
+	y = pt3.y;
+	z = pt3.z;
+	x1 = pt3_1.x;
+	y1 = pt3_1.y;
+	z1 = pt3_1.z;
+}
+
+double Card::get_speed()
+{
+	double speed = 0; 
+	// 计算速度
+	return speed;
+}
+
+void Card::set_anchors( int cnt )
+{
+	for(int i = 0; i < cnt; i++){
+		p3_anchors[i].x = p_dists[i]->x;
+		p3_anchors[i].y = p_dists[i]->y;
+		p3_anchors[i].z = p_dists[i]->z;
+	}
+	cal_location3->setAnchors(p3_anchors, ANCHOR_COUNT);
+	is_anchor_changed = false;
+}
+
+/*
+ * 采用TOF或者TDOA算法进行定位计算
+ *
+ * param
+ *		cnt		------	_dists数据条数
+ *
+ * return
+ *		无返回值
+ *
+*/
+void Card::get_coordinate( int cnt )
+{
+#ifdef ALGORITHM_TOF
+	algo_tof(cnt);
+#elif defined ALGORITHM_TYPE_TDOA
+	algo_tdoa(cnt);
+#else
+	
 #endif
+	inspect_coordinate(this->acce_cur_state,this->acce_last_state);
+
 	if(_isnan(this->x) || _isnan(this->y) || _isnan(this->z)){
 		this->x = this->last_x;
 		this->y = this->last_y;

classdef.h

@@ -104,7 +104,7 @@ enum ALARM_TYPE_ID
 };
 
 enum STATUS_DEVICE{
-	STATUS_DEVICE_NORMAL = 1,
+	STATUS_DEVICE_NORMAL = 0,
 	STATUS_DEVICE_ERROR,
 };
 
@@ -218,6 +218,7 @@ struct _coordinate{
 		special = 0;
 		acceleration = 0;
 		acce_state = 0;	
+		acce_state_last = 0;
 	}
 	int t; // 定位时间戳
 	int reader_id;
@@ -235,6 +236,7 @@ struct _coordinate{
 	int st;   //同步序号
 	double acceleration;	//加速度
 	int acce_state;	//加速度状态
+	int acce_state_last;	//加速度计上一次状态
 	int special; //分站是否属于特殊分站，0属于特殊分站，1普通分站
 
 	_coordinate& operator=(_coordinate &tmp){
@@ -253,6 +255,7 @@ struct _coordinate{
 		special = tmp.special;
 		acceleration = tmp.acceleration;
 		acce_state = tmp.acce_state;
+		acce_state_last = tmp.acce_state_last;
 		return *this;
 	}
 };
@@ -749,6 +752,9 @@ public:
 	int get_effictive_dist_count(int offset = 0);
 	void set_reader_path(ReaderPathMap *rpm);
 	void set_reader_path(TDOAReaderPathMap* trpm);
+	void inspect_coordinate(int acce_state,int acce_state_last);
+	void algo_tof(int cnt);
+	void algo_tdoa(int cnt);
 public:
 	// 滤波算法相关
 	//CalLocation* cal_location;
@@ -765,6 +771,10 @@ public:
 	double ins_weight;
 	double uwb_weight;
 	double weight;
+	
+	int acc_change_state;	//加速度计状态改变，0为未改变，1为改变
+	int acce_cur_state;		//在算法中保存当前加速度状态，保存这两个值的原因是在多线程情况下accelerate_state和accelerate_state_last状态可能变化
+	int acce_last_state;	//在算法中保存上一次加速度状态
 
 	bool is_anchor_changed;
 	void get_coordinate();
@@ -785,7 +795,7 @@ public: //
 	double distance; // 距离
 	int64_t flying_time; // 飞行时间
 	int power_state; // 电量
-	unsigned short sync_num;
+	int sync_num;
 
 	int time_stamp_last; // 定位时间戳
 	int context_type_last; // 报文类型 tof，tdoa

locate_algorithm.cpp

@@ -906,6 +906,12 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 			continue;
 		}
 
+		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
+		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;
@@ -932,11 +938,11 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 			continue;
 		}
 
-		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
-		TDOAReaderPathMap::iterator rdm_it = trpm.find(f1.reader_id);
-		if(rdm_it == trpm.end()){
-			continue;
-		}
+		////如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
+		//TDOAReaderPathMap::iterator rdm_it = trpm.find(f1.reader_id);
+		//if(rdm_it == trpm.end()){
+		//	continue;
+		//}
 
 		//获取地图集路径
 		ReaderPath * pRP = trpm.find(f1.reader_id)->second->find(f2.reader_id)->second;