add speed and acce

YAServerDlg.cpp

@@ -13109,7 +13109,9 @@ int CYAServerDlg::ModifyCardInfo()
 		::GetLocalTime(&curTime);
 
 		//加500ms
-		it->second->ChangeSystemTime(lastTime);
+		//it->second->ChangeSystemTime(lastTime);
+		//加1s
+		it->second->ChangeSystemTimeBySecond(lastTime);
 		int ret = CFunctions::CompareSystemTime(curTime,lastTime);
 		if (ret <= 0)
 		{
@@ -13118,7 +13120,7 @@ int CYAServerDlg::ModifyCardInfo()
 
 		::GetLocalTime(&it->second->last_cell->deal_time);
 
-		int nStep = it->second->last_cell->nStep;
+		int nStep = it->second->last_cell->nStep*2/3;
 		if (nStep < 1)
 		{
 			nStep = 1;
@@ -13142,6 +13144,8 @@ int CYAServerDlg::ModifyCardInfo()
 		{
 			continue; //cellid如果为0,那么此卡放弃push进格子列表，continue下一个card
 		}
+		it->second->origin_locate.x = it->second->origin_locate.y = 0;
+
 		std::mutex mu;
 		mu.lock();
 		cell->interval_time = it->second->last_cell->interval_time;
@@ -13150,8 +13154,8 @@ int CYAServerDlg::ModifyCardInfo()
 		{
 			*(it->second->last_cell) = *cell;
 		}
-		it->second->x = it->second->last_cell->getX();
-		it->second->y = it->second->last_cell->getY();
+		it->second->x = it->second->last_cell->getX()/it->second->map_scale;
+		it->second->y = it->second->last_cell->getY()/it->second->map_scale;
 		it->second->locate_cells.push_back(it->second->last_cell);
 		mu.unlock();
 

classdef.cpp

@@ -171,6 +171,8 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 
 	drive_face_location_write_time = 0;
 	idle_pos_list.resize(0);
+
+	fit_b = fit_k = fit_r = cell_a = 0.0;
 }
 
 Card::Card( void )
@@ -1936,7 +1938,7 @@ void Card::get_coordinate( int cnt )
 		std::string flag1 = is_fit_pos?"Y":"N";
 		int nSource1 = 1;		
 		char chLog1[300] = {0};
-		sprintf_s(chLog1,"cardid: %s, sync_num: %d, ct: %d, ox: %f, oy: %f, x: %f,y: %f, readerName: %s, cellId : %d, originId: %d, oReaderName: %s, ov: %f, cv: %f, fit: %s, source: %d",
+		sprintf_s(chLog1,"cardid: %s, sync_num: %d, ct: %d, ox: %f, oy: %f, x: %f,y: %f, readerName: %s, cellId : %d, originId: %d, oReaderName: %s, ov: %f, cv: %f, fit: %s, source: %d, fit_k : %f, fit_b: %f, fit_r: %f",
 			card_id.c_str(),
 			m_nCalcSyncNum,
 			time_stamp_cal,
@@ -1951,7 +1953,11 @@ void Card::get_coordinate( int cnt )
 			origin_locate.v,
 			get_speed(),
 			flag1.c_str(),
-			nSource1);
+			nSource1,
+			fit_k,
+			fit_b,
+			fit_r
+			);
 
 		std::string strLog1 = "";
 		strLog1 = chLog1;
@@ -3821,7 +3827,6 @@ int Card::PersonLocation(int cnt)
 //
 int Card::DiscreteLocate(int cnt)
 {
-	//方案二:20170801
 	ULONGLONG nowTickCounts = ::GetTickCount();
 	//如果last_cell存在，那说明不是第一次定位，如果不存在，那么继续往下走。
 	/*if (last_cell)
@@ -3844,6 +3849,7 @@ int Card::DiscreteLocate(int cnt)
 	std::shared_ptr<Cell> cell = nullptr;
 	std::shared_ptr<Cell> tmp_cell = std::make_shared<Cell>();
 	is_fit_pos = false;
+	std::shared_ptr<POS> pos = nullptr;
 
 	//1.数据有效性判断
 	ret = CheckDistData(cnt);
@@ -3862,7 +3868,8 @@ int Card::DiscreteLocate(int cnt)
 				return 1;
 			}
 
-			std::shared_ptr<POS> pos = std::make_shared<POS>();
+			//std::shared_ptr<POS> pos = std::make_shared<POS>();
+			pos = std::make_shared<POS>();
 			//3.算法定位	
 			std::vector<std::shared_ptr<POS>> udm_pos;
 			udm_pos.resize(0);
@@ -3876,6 +3883,7 @@ int Card::DiscreteLocate(int cnt)
 			{
 				return 1;
 			}
+
 			bool bRet = false;
 			bRet = LocateAlgorithm::IsOnMap(pos,pTdoaReaderPathMap);
 			if (!bRet)
@@ -3941,6 +3949,7 @@ int Card::DiscreteLocate(int cnt)
 							{
 								bOverThre = false;
 								isDirectOutput = true;
+								pos->update = true;
 							} else if ((degree - 0.7) > ZERO_PRECISION && (0.9 - degree) > ZERO_PRECISION)
 							{
 								//向前大跳
@@ -3949,7 +3958,6 @@ int Card::DiscreteLocate(int cnt)
 							}else if ((degree - 0.5) > ZERO_PRECISION &&(0.7 - degree) > ZERO_PRECISION)
 							{
 								bOverThre = false;
-								//lvCells.pop_back();
 							}
 							else if(0.5 - degree > ZERO_PRECISION)
 							{
@@ -3970,12 +3978,11 @@ int Card::DiscreteLocate(int cnt)
 
 									//根据上一次数据计算格子数，并根据上一次方向进行偏移
 									double y = 0;
-									//std::list<std::shared_ptr<Cell>>::iterator it = his_cell.begin();
 									std::list<std::shared_ptr<Cell>>::iterator it = lvCells.begin();
 
 									y = fit_k*(this->time_stamp_cal - (*it)->card_stamp_time + 1) + fit_b;
 
-									int nCellId  =0 ;
+									int nCellId  = 0 ;
 									std::string strReaderName = "";
 									//判断两次格子的id是否在相同分站内，不在走跨分站逻辑
 									if ((*it)->strOriginReaderName != cell->strOriginReaderName)
@@ -4080,7 +4087,27 @@ int Card::DiscreteLocate(int cnt)
 
 	if (cell)
 	{
-		SaveCardAlgoData(cell);
+		//做加速度处理
+		double dAcce = 0;
+		bool bRet = false;
+		if (abs(fit_k - last_k) < ZERO_PRECISION)
+		{
+			bRet = true;
+		}else{
+			dAcce = abs((fit_k - last_k)/(delta_ct))*1.2;
+			if (dAcce > 10)
+			{
+				bRet = false;
+			}else{
+				bRet = true;
+			}
+		}
+
+		if (bRet)
+		{
+			SaveCardAlgoData(cell);
+		}
+		//SaveCardAlgoData(cell);
 	}	
 
 	return ret;
@@ -4663,6 +4690,47 @@ int Card::ChangeSystemTime(SYSTEMTIME& st)
 	return 0;
 }
 
+int Card::ChangeSystemTimeBySecond(SYSTEMTIME& st)
+{
+	//只考虑到小时
+	int nMs = st.wMilliseconds;
+	int nSecond = st.wSecond;
+	int nMinute = st.wMinute;
+	int nHour = st.wHour;
+
+	//毫秒数保持不变
+	nMs = st.wMilliseconds;
+
+	//如果秒大于60，向分进一格
+	if (st.wSecond + 1 >= 60)
+	{
+		nSecond = st.wSecond + 1 - 60;
+		//如果分大于60，向时进一格
+		if (st.wMinute + 1 >= 60)
+		{
+			nMinute = st.wMinute + 1 - 60;
+			//如果时大于24，向天进一格
+			if (st.wHour + 1 >= 24)
+			{
+				nHour = st.wHour + 1 - 24;
+			}else{
+				nHour = st.wHour + 1;
+			}
+		}else{
+			nMinute = st.wMinute + 1;
+		}				
+	}else{
+		nSecond = st.wSecond + 1;
+	}			
+
+	st.wMilliseconds = nMs;
+	st.wSecond = nSecond;
+	st.wMinute = nMinute;
+	st.wHour = nHour;
+
+	return 0;
+}
+
 //void Card::handle_data_thread(LPVOID pContext)
 //{
 //	CYAServerDlg* pDlg = (CYAServerDlg*)pContext;
@@ -5202,19 +5270,20 @@ int Card::SaveCardAlgoData(std::shared_ptr<Cell>& cell)
 	//	return 1;
 	//mu.unlock();
 
-	if (!is_algo_first_location)
-	{
-		//确保第一次不会进来算速度
-		if (this->x != INVALID_COORDINATE && this->y != INVALID_COORDINATE)
-		{
-			//pos为本次定位坐标，(x,y)为上一次定位坐标
-			double distance = sqrt(pow(pos->cx - this->x,2) + pow(pos->cy - this->y,2))*map_scale;
-			this->mileage = distance/1000.0;
-			int nCounts = ceil(distance/CELL_WIDTH);
-			this->v = nCounts*CELL_WIDTH/(SEND_CELL_INTERVAL/1000.0);
-			this->v*=3.6;
-		}
-	}
+	//if (!is_algo_first_location)
+	//{
+	//	//确保第一次不会进来算速度
+	//	if (this->x != INVALID_COORDINATE && this->y != INVALID_COORDINATE)
+	//	{
+	//		//pos为本次定位坐标，(x,y)为上一次定位坐标
+	//		double distance = sqrt(pow(pos->cx - this->x,2) + pow(pos->cy - this->y,2))*map_scale;
+	//		this->mileage = distance/1000.0;
+	//		int nCounts = ceil(distance/CELL_WIDTH);
+	//		this->v = nCounts*CELL_WIDTH/(SEND_CELL_INTERVAL/1000.0);
+	//		this->v*=3.6;
+	//	}
+	//}
+	this->v = fit_k*0.6*3.6;
 
 	this->x = pos->cx;
 	this->y = pos->cy;
@@ -5278,6 +5347,10 @@ int Card::SaveCardAlgoData(std::shared_ptr<Cell>& cell)
 		//相等的话就保持原来的方向
 	}
 
+	if (pos)
+	{
+	}
+
 	return 0;
 }
 
@@ -5338,15 +5411,10 @@ int Card::CheckSolution(std::shared_ptr<POS>& p)
 		//抛不掉，就用队列里的同步数据（从后往前找），找到第一个与当前同步序号相差大于5的同步数据来进行第二次计算速度以及加速度，
 		//如果加速度大于5，就不要此次的定位数据，
 		//如果通过加速度判断就将队列中从头开始到此同步数据的所有元素都丢弃，并插入新的此次同步数据
-
 		double interval_time = 0.2;
 		double deltaT = 0;
 		sync_data sd;
 
-		/*sd.x = last_locate.x;
-		sd.y = last_locate.y;
-		sd.vx = abs(last_vx);
-		sd.vy = abs(last_vy);*/
 		sd.locate = this->locate;
 		if (locate)
 		{
@@ -5357,9 +5425,9 @@ int Card::CheckSolution(std::shared_ptr<POS>& p)
 			//p->diff_reader_sync_num = locate->DeltaTSecond();
 			//deltaT = locate->DeltaTSecond();
 			
-			char msg[100] = {0};
+			/*char msg[100] = {0};
 			sprintf_s(msg,"card_id: %s, ct: %d, dt: %.4f, distance: %.4f",card_id.c_str(),time_stamp_cal,dTestDelatT,dTestDistance);
-			Log::write_log(FILE_TYPE::SYS_S,msg,true);
+			Log::write_log(FILE_TYPE::SYS_S,msg,true);*/
 		}
 
 		if (this->b_long_interval)
@@ -6644,6 +6712,8 @@ bool Card::cftool(float *data_x, float *data_y, int data_n, std::vector<float> &
 	vResult.push_back(b);
 	vResult.push_back(r*r);
 
+	fit_r = vResult.at(2);
+
 	if(vResult.at(2) < 0.7)
 		return true;
 	else
@@ -6743,6 +6813,10 @@ int Card::colourfulCloudsChasingTheMoon(std::shared_ptr<Cell> &cell, std::list<s
 				float b = 0.0;
 				b = vResult.at(1);
 
+				last_k = 0;
+				last_k = fit_k;
+				delta_ct = pY[4] - pY[3];
+
 				fit_k = k;
 				fit_b = b;
 

classdef.h

@@ -1335,7 +1335,11 @@ public:
 	int nOverThreCounts;	//连续大跳次数
 
 	double fit_k;	//最新的格子拟合的斜率
+	double last_k;
+	double delta_ct; //ct的差值
 	double fit_b;	//最新的格子拟合的截距
+	double fit_r;	//求斜率时的拟合度
+	double cell_a;	//格子算法中的加速度值
 
 	bool isIdling;		//怠速状态
 
@@ -1362,6 +1366,7 @@ public:
 	int SetCellPathMap(std::shared_ptr<CellPathMap> pCellPath);
 	int ChangeSystemTime(SYSTEMTIME& st,const int condition);
 	int ChangeSystemTime(SYSTEMTIME& st);	//在st基础上加500ms
+	int ChangeSystemTimeBySecond(SYSTEMTIME& st);	//按1s的方式更改st时间
 	int colourfulCloudsChasingTheMoon(std::shared_ptr<Cell> &cell, std::list<std::shared_ptr<Cell>> &myCellList);
 	int MappingCell2FitLinear(std::shared_ptr<Cell>& cell);		//将格子拟合到线上
 	bool cftool(float *data_x, float *data_y, int data_n, std::vector<float> &vResult);

mylog/MyLog.cpp

@@ -41,10 +41,7 @@ MyLog::~MyLog()
 
 int MyLog::WriteLog(const std::string msg)
 {
-	//pCategory->info(msg.c_str());
-	std::string log = "";
-	log = msg;
-	pCategory->info(log);
+	pCategory->info(msg.c_str());
 
 	//如果日志文件超过大小，则开启新文件名
 	if (false)