getDeltaT()

LocateRecord.h

@@ -0,0 +1,131 @@
+#pragma once
+
+#include <sys/timeb.h>
+#include <memory>
+using namespace std;
+
+namespace nspLocate{
+
+	const int PERIOD_SYNCTIME = 15000; // 分站时间同步周期，约10秒，10000毫秒
+	const double INTERVAL_SYNCTIME = 200.0; // 分站时间同步间隔，200毫秒
+	const unsigned long long PERIOD_MAX_VALUE = 0xFFFFFFFFFF;
+	const double PICOSECOND_COEFFICIENT = (15.65 * 1E-9);
+
+	class LocateRecord
+	{
+	private:
+		// 分站接收标识卡的时间，为线性插值后的时间，多个基站中，以参与算法最小的时间戳为准
+		unsigned long long locateTime;
+		// 本次记录时间的分站号
+		int readerId ;
+		// 本次记录分站时间同步序号
+		int syncNum; 
+		// 卡时间戳
+		int cardTimestamp;
+		// 上位机接收时间，相当于time(NULL)
+		unsigned long long receiveTime;		
+		//// 当前坐标
+		//double x; 
+		//double y;
+		//// 当前速度
+		//double vx;
+		//double vy;
+		//double v; // 速度
+		//double a; // 加速度
+		//// 地图比例，用来将像素转为米
+		//double scale;
+
+		int deltaT;
+
+		// 当前时间，1970年1月1日到现在的毫秒数
+		unsigned long long millionSeconds(){
+			struct timeb t1;
+			ftime(&t1);
+			return t1.millitm;  // 毫秒数
+			//t1.time // 秒数， 同time(NULL);
+		}
+	public:
+		LocateRecord(void);
+		LocateRecord(unsigned long long revTime, int rid, int sn, int ct){
+			this->locateTime = revTime;
+			this->readerId = rid;
+			this->syncNum = sn;
+			this->cardTimestamp = ct;
+			this->receiveTime = millionSeconds();
+			deltaT = 0;
+		}
+		~LocateRecord(void);
+
+		//// 根据上次的时间、位置计算速度、加速度
+		//void getV(std::shared_ptr<LocateRecord> &lr){
+		//	double deltaT = getDeltaT(lr) / 1000.0; // 转为秒
+		//	// 时间差异常，应赋予默认值
+		//	if(0 == deltaT)
+		//	{
+		//		vx = lr->VX();
+		//		vy = lr->VY();
+		//		v = lr->V();
+		//		a = lr->A();
+		//	}
+		//	else
+		//	{
+		//		// 速度，加速度
+		//		vx = (x - lr->X()) * scale / deltaT;
+		//		vy = (y - lr->Y()) * scale / deltaT;
+		//		v = sqrt(pow(vx, 2) + pow(vy, 2));
+
+		//		double avx = (vx - lr->VX()) / deltaT;
+		//		double avy = (vy - lr->VY()) / deltaT;
+		//		a = sqrt(pow(avx,2) + pow(avy,2));
+		//	}
+		//}
+		//void SetReceiveTime(){ receiveTime = millionSeconds();}
+		//void SetScale(double val){ scale = val;}
+
+		unsigned long long ReceiveTime() const { return receiveTime; }
+		unsigned long long LocateTime() const { return locateTime;}
+		int SyncNum() const { return syncNum;}
+		int CardTimestamp() const { return cardTimestamp;}
+		int ReaderId() const { return readerId ;}
+
+		int DeltaT() const { return deltaT; } // 毫秒
+		double DeltaTSecond() const {return (double)deltaT/1000.0;}; // 秒
+		// 获取两次定位的时间间隔，返回毫秒数
+		void getDeltaT(std::shared_ptr<LocateRecord> &lr)
+		{
+			if(nullptr == lr)
+			{
+				return ;
+			}
+			double ret = receiveTime - lr->ReceiveTime();
+			// 跨时间同步周期了，以上位机时间receiveTime为准
+			if( ret > PERIOD_SYNCTIME)
+			{
+				deltaT = (int)ret;
+			}
+			// 时间同步序号差500以上，认为root异常，时间同步序号重置，以上位机时间receiveTime为准
+			else if(syncNum - lr->SyncNum() > double(PERIOD_SYNCTIME) / INTERVAL_SYNCTIME) // 500
+			{
+				deltaT = (int)ret;
+			}
+			// 分站接收时间跨周期，并且在一个周期以内，以分站接收时间为主
+			else if(locateTime < lr->LocateTime())
+			{
+				deltaT = (int)(locateTime + PERIOD_MAX_VALUE - lr->LocateTime() & PERIOD_MAX_VALUE) * PICOSECOND_COEFFICIENT;
+			}
+			// 在同一个周期内，以分站接收时间为准
+			else
+			{
+				deltaT = (int)(locateTime - lr->LocateTime() & PERIOD_MAX_VALUE) * PICOSECOND_COEFFICIENT;
+			}
+		}
+
+		//double VX() const { return vx;}
+		//double VY() const { return vy;}
+		//double V() const { return v;}
+		//double A() const { return a;}
+
+		//double X() const { return x;}
+		//double Y() const { return y;}
+	};
+}

YAServer.vcxproj

@@ -188,6 +188,7 @@ xcopy "$(SolutionDir)..\config.ini" "D:\0a-share\$(Configuration)\" /Y /C /D /S<
     <ClInclude Include="def.h" />
     <ClInclude Include="Filter\KalmanFilter.h" />
     <ClInclude Include="KNN\KNN.h" />
+    <ClInclude Include="LocateRecord.h" />
     <ClInclude Include="LogSetting.h" />
     <ClInclude Include="log\ExRollingFileAppender.h" />
     <ClInclude Include="log\log_module.h" />

YAServer.vcxproj.filters

@@ -325,6 +325,7 @@
     <ClInclude Include="log\ExRollingFileAppender.h">
       <Filter>log</Filter>
     </ClInclude>
+    <ClInclude Include="LocateRecord.h" />
   </ItemGroup>
   <ItemGroup>
     <None Include="..\config.ini" />

YAServerDlg.cpp

@@ -6139,7 +6139,7 @@ void CYAServerDlg::store_data_card(std::shared_ptr<Card> card /*Card* card*/, in
 			//writeErrorLog(_T("SQL_S"),sql_log,true);
 			std::string strSqlLog = "";
 			strSqlLog = sql;
-			Log::write_log(FILE_TYPE::SQL_S,strSqlLog,true);
+			// Log::write_log(FILE_TYPE::SQL_S,strSqlLog,true);
 		}
 	}
 	catch (...)
@@ -10051,7 +10051,7 @@ void CYAServerDlg::parse_data_locate_reader_his( BYTE * DataBuffer, int nLen, in
 
 void CYAServerDlg::parse_data_reader_synctime(BYTE *DataBuffer, int nLen, int& nCurPos, WORD wChr)
 {
-	if(syncmanager.IsOutputLog()^m_log_parse_data){
+	if(syncmanager.IsOutputLog() != m_log_parse_data) {
 		syncmanager.SetOutputLog(m_log_parse_data);
 	}
 

classdef.cpp

@@ -90,6 +90,8 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 	last_locate.d = last_locate.d_offset = last_locate.v = 0.0;
 	last_locate.x = last_locate.y = last_locate.z = INVALID_COORDINATE;
 	last_x = last_y = last_z = INVALID_COORDINATE;
+	
+	locate = nullptr;
 
 	m_bUseFilter = false;
 	m_nFilterType = NO_FILTER;
@@ -185,13 +187,14 @@ void Card::reset()
 	cur_fit_nums = 0;
 	count_idle = 0;
 
+	locate = nullptr;
+
 	//卡尔曼参数重置
 	if (m_pKalmanFilter!=nullptr)
 	{
 		m_pKalmanFilter->m_nCounts = 0;
 		m_pKalmanFilter->Initial(0.2);
 	}
-	
 }
 
 void Card::set_reader(std::shared_ptr<Reader> preader) // 设置卡时间
@@ -1451,6 +1454,9 @@ int Card::algo_tdoa_1d(int cnt)
 		OutputCmdLog(2);
 		return ret;
 	}
+	
+	// 获取两次定位的时间差数据
+	GetDeltaT(mp_dists_locate);
 
 	//保证每个ct都输出一个坐标:
 	//特殊情况一：单条记录也输出
@@ -3440,7 +3446,6 @@ int Card::CheckSolution(std::shared_ptr<POS>& p)
 		double interval_time = 0.2;
 		double deltaT = 0;
 		sync_data sd;
-
 		if (this->b_long_interval)
 		{   
 			//此段代码用于将上一次定位是根据两个时间差是个很大值而定位出的结果
@@ -3969,6 +3974,45 @@ int Card::ChooseOneSolution(std::shared_ptr<ReceiveDataMap> pRdm, std::vector<st
 	return 0;
 }
 
+int Card::GetDeltaT( map<unsigned long long,std::shared_ptr<_coordinate>> dl )
+{
+	unsigned long long revTime = 0;
+	int rid = 0, sn = 0, ct = 0;
+
+	map<unsigned long long,std::shared_ptr<_coordinate>>::iterator it_mpdl = dl.begin();
+	int i = 0;
+	for(; it_mpdl != dl.end(); ++it_mpdl){
+		if(0 == it_mpdl->second->tt){
+			continue;
+		}
+
+		if(it_mpdl->second->reader_id == this->locate->ReaderId()){
+			revTime = it_mpdl->second->tt;
+			rid = it_mpdl->second->reader_id;
+			sn = it_mpdl->second->sync_num;
+			ct = it_mpdl->second->t;
+			break;
+		}
+
+		if(0 == revTime){
+			revTime = it_mpdl->second->tt;
+			rid = it_mpdl->second->reader_id;
+			sn = it_mpdl->second->sync_num;
+			ct = it_mpdl->second->t;
+		}
+		else if( revTime < it_mpdl->second->tt){ // 跨周期，可能会取到较大值
+			revTime = it_mpdl->second->tt;
+			rid = it_mpdl->second->reader_id;
+			sn = it_mpdl->second->sync_num;
+			ct = it_mpdl->second->t;
+		}
+	}
+
+	std::shared_ptr<nspLocate::LocateRecord> loc = std::make_shared<nspLocate::LocateRecord>(revTime, rid, sn, ct);
+	loc->getDeltaT(this->locate);
+	this->locate = loc;
+}
+
 MapInfo::MapInfo( void )
 {
 

classdef.h

@@ -51,6 +51,7 @@
 #include <kalman\locate.h>
 #include "Filter\KalmanFilter.h"
 #include "algorithm\FittingPosition.h"
+#include "LocateRecord.h"
 
 #define MIN(x,y) (x < y ? x : y)
 #define MAX(x,y) (x > y ? x : y)
@@ -406,9 +407,10 @@ struct sync_data{
 	double vy;
 
 	int sync_num;	//本次同步号
-
 	bool update;
 
+	std::shared_ptr<nspLocate::LocateRecord> locate;
+
 	sync_data(){
 		x = 0.0;
 		y = 0.0;
@@ -416,6 +418,7 @@ struct sync_data{
 		vy = 0.0;
 		sync_num = 0;
 		update = false;
+		locate = nullptr;
 	}
 
 	sync_data& operator=(sync_data&tmp){
@@ -425,6 +428,7 @@ struct sync_data{
 		vy = tmp.vy;
 		sync_num = tmp.sync_num;
 		update = tmp.update;
+		locate = tmp.locate;
 		return *this;
 	}
 };
@@ -1047,23 +1051,26 @@ public:
 	
 	INT64 m_event_list[CARD_EVENT_COUNT]; // 保存事件Id
 public:
-		void algo_tof(int cnt);
-		void algo_tdoa(int cnt);
-		int  algo_tdoa_1d(int cnt);		//一维定位
-		int  algo_tdoa_2d(int cnt);		//二维定位
-		int  algo_tdoa_3d(int cnt);		//三维定位
-
-		void algo_calc_offset();		//计算偏移坐标
-		bool algo_is_same_direction(double x,double y,double z);	//检查此次方向
-
-		int CheckDistData(int cnt);
-		int AssembleDistData(std::shared_ptr<ReceiveDataMap> pRdm);
-		int SaveCardAlgoData(std::shared_ptr<POS>& pos);
-		int SaveOriginDataBeforeFilter(std::shared_ptr<POS> pos);
-		int ChooseOneSolution(std::shared_ptr<ReceiveDataMap> pRdm, std::vector<std::shared_ptr<POS>> udm_pos, std::shared_ptr<POS>& pos);// 输出唯一解
-		int CheckSolution(std::shared_ptr<POS>& p); // 验证唯一解合法性，如速度、加速度
-		int CheckSulutionByStream(std::shared_ptr<POS> p);	//参数使用非引用，即不可修改其值
-		bool IsExistPath(int left,int right);
+	std::shared_ptr<nspLocate::LocateRecord> locate;
+
+	void algo_tof(int cnt);
+	void algo_tdoa(int cnt);
+	int  algo_tdoa_1d(int cnt);		//一维定位
+	int  algo_tdoa_2d(int cnt);		//二维定位
+	int  algo_tdoa_3d(int cnt);		//三维定位
+
+	void algo_calc_offset();		//计算偏移坐标
+	bool algo_is_same_direction(double x,double y,double z);	//检查此次方向
+
+	int CheckDistData(int cnt);
+	int AssembleDistData(std::shared_ptr<ReceiveDataMap> pRdm);
+	int GetDeltaT(map<unsigned long long,std::shared_ptr<_coordinate>> dl);
+	int SaveCardAlgoData(std::shared_ptr<POS>& pos);
+	int SaveOriginDataBeforeFilter(std::shared_ptr<POS> pos);
+	int ChooseOneSolution(std::shared_ptr<ReceiveDataMap> pRdm, std::vector<std::shared_ptr<POS>> udm_pos, std::shared_ptr<POS>& pos);// 输出唯一解
+	int CheckSolution(std::shared_ptr<POS>& p); // 验证唯一解合法性，如速度、加速度
+	int CheckSulutionByStream(std::shared_ptr<POS> p);	//参数使用非引用，即不可修改其值
+	bool IsExistPath(int left,int right);
 public:
 	// 滤波算法相关
 	std::unique_ptr<CKalmanFilter> m_pKalmanFilter;