#include "stdafx.h" #include"SyncManager.h" #include <deque> #include <Eigen/Dense> #include <fstream> #include <iostream> #include <direct.h> #include <io.h> #include "../ProcessRemodule.h" #include "../log_process_module.h" #include "./../system_basic_info/SystemAnalysis.h" #pragma warning(disable: 4244) HostServer::SyncManager::SyncManager() { init(); } void HostServer::SyncManager::init() { InitializeCriticalSectionAndSpinCount(&m_csSyncTime, 4000); _anchors.swap(unordered_map<unsigned long long, Position>()); _distance.swap(unordered_map<unsigned long long,unordered_map<unsigned long long, double>>()); logDir = ".\\synclog\\"; if(0 != _access(logDir.c_str(), 0)){ _mkdir(logDir.c_str()); } isOutputLog = false; } void HostServer::SyncManager::analyzeSyncMsg(SyncTimeMsg &msg) { LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_10); EnterCriticalSection(&m_csSyncTime); int idx = FindSyncTimeMsg(msg.RootIdCode(), msg.SyncNum()); if( -1 == idx){ // û�ҵ� LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_72); // ���ʱ��ͬ����Ϣ�İ汾��������������ƣ���ɾ���������ӵ���Ϣ if(_syncTimeMsgs[msg.RootIdCode()].size() >= MAX_SYNCTIME_NUM){ // ɾ����һ�� // ������й¶ _syncTimeMsgs[msg.RootIdCode()].pop_front(); LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_12); } SyncTimeMsgItem it; it.SyncNum = msg.SyncNum(); it.SyncTimeMsgs[msg.LocalIdCode()] = msg; _syncTimeMsgs[msg.RootIdCode()].push_back(it); idx = _syncTimeMsgs[msg.RootIdCode()].size() - 1; }else{ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_73); _syncTimeMsgs[msg.RootIdCode()][idx].SyncTimeMsgs[msg.LocalIdCode()] = msg; } // while(_historySync[msg.RootIdCode()].size() > MAX_SYNCTIME_NUM){ _historySync[msg.RootIdCode()].pop_front(); } // ����ʱ��ͬ�������� for(auto it :_syncTimeMsgs[msg.RootIdCode()][idx].SyncTimeMsgs) { updateSync(msg.RootIdCode(), idx, msg.SyncNum(), it.first); } LeaveCriticalSection(&m_csSyncTime); } bool HostServer::SyncManager::updateSync(unsigned long long rootIdCode, int idx, unsigned short SyncNum, unsigned long long localIdCode) { if(-1 == idx){ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_74); return false; } // �����ǰ�汾��ʱ��ͬ����Ϣδ�յ���false unordered_map<unsigned long long, SyncTimeMsg>::iterator itSyncTime = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(localIdCode); if(itSyncTime == _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.end()){ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_75); return false; } SyncTimeMsg &msg = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[localIdCode]; // �����ǰ��Ϊroot����true if(msg.SyncLevel() == 0){ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_76); return true; } int idx_synctime = FindHisSyncTime(rootIdCode, SyncNum); // ���ʱ��ͬ���Ѿ����������true if(-1 != idx_synctime){ unordered_map<unsigned long long, SyncTime>::iterator itHistSync = _historySync[rootIdCode][idx_synctime].HistSync.find(localIdCode); if(itHistSync != _historySync[rootIdCode][idx_synctime].HistSync.end()){ if(itHistSync->second.TimeDelay()) return true; } } // ����Ѿ��յ�����һ����ͬ����Ϣ if(_syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.count(msg.UpperIdCode())){ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_77); if(!updateSync(rootIdCode, idx, SyncNum, msg.UpperIdCode())){ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_78); return false; } SyncTimeMsg &upperMsg = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[msg.UpperIdCode()]; SyncTime* s = NULL; for(auto it(_historySync[rootIdCode].rbegin()); it != _historySync[rootIdCode].rend(); ++it) { if(it->SyncNum != msg.SyncNum() && it->HistSync.count(localIdCode)) { s = &(it->HistSync.find(localIdCode)->second); break; } } idx_synctime = FindHisSyncTime(rootIdCode, SyncNum); if(-1 == idx_synctime){ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_79); SyncTimeItem it; it.SyncNum = msg.SyncNum(); it.HistSync[localIdCode] = SyncTime(msg, upperMsg, s); _historySync[rootIdCode].push_back(it); idx_synctime = _historySync[rootIdCode].size() - 1; }else{ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_80); _historySync[rootIdCode][idx_synctime].HistSync[localIdCode] = SyncTime(msg, upperMsg, s); } // ����ʱ��ͬ�� long long upperTimeDelay = 0; if(_syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[msg.UpperIdCode()].SyncLevel() != 0) { LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_81); upperTimeDelay = _historySync[rootIdCode][idx_synctime].HistSync[msg.UpperIdCode()].TimeDelay(); } long long sendTime = _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].SendTime(); long long receiveTime = _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].ReceiveTime(); long long timeDelay = receiveTime - sendTime - _distance[localIdCode][msg.UpperIdCode()]; timeDelay += upperTimeDelay; // �Ӳ�ͬ��Upper����ͬ�����ݣ��������жϿ��ܲ���ȷ����ʱ��������һ�������� while(timeDelay > TIME_MAX){ timeDelay -= TIME_MAX; } while(timeDelay + TIME_MAX < 0 ){ timeDelay += TIME_MAX; } _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].TimeDelay(timeDelay); if(isOutputLog){ char filename[100]; char temp[200]; long long aa = timeDelay; if(aa < 0){ aa += TIME_MAX; } int bb = (receiveTime > TIME_MAX) ? 1 : 0; int cc = (sendTime > TIME_MAX) ? 1 : 0; int dd = msg.UpperIdCode() >> 8; sprintf_s(filename, "%s\\%d.log", logDir.c_str(), localIdCode>>8); ofstream outfile(filename, ofstream::app); sprintf_s(temp, "sn:%d, up:%d, rec:%I64d, snd:%I64d, del:%I64d, crt:%I64d, r_crs:%d, s_crs:%d\r", SyncNum, dd, receiveTime, sendTime, timeDelay, aa, bb, cc); outfile << temp; } LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_69); return true; }else{ LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_110); } return false; } unsigned long long HostServer::SyncManager::calTimeByLinar(TagMsg &tag) { EnterCriticalSection(&m_csSyncTime); // ��ȡ��ʷ��¼�����tag��������� deque<SyncTime> hisSync; unsigned long long rootIdCode = tag.SyncRootIdCode; int i = 0; int idx = FindSyncTimeMsg(rootIdCode, tag.SyncNum); if(-1 != idx){ unordered_map<unsigned long long, SyncTimeMsg>::iterator it = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(tag.StationIdCode); if(it != _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.end()){ if(it->second.SyncLevel() == 0){ LeaveCriticalSection(&m_csSyncTime); return tag.ReceiveTime; } } } int idx_sync = -1; while(hisSync.size() < 2 && i < MAX_CALCLINER_NUM) { auto syncNum = tag.SyncNum - i; idx_sync = FindHisSyncTime(rootIdCode, syncNum); if(-1 != idx_sync){ if(_historySync[rootIdCode][idx_sync].HistSync.count(tag.StationIdCode)){ hisSync.push_front(_historySync[rootIdCode][idx_sync].HistSync[tag.StationIdCode]); } } i++; } // ���������������ʷ��¼���������� if(hisSync.size() < 2){ LeaveCriticalSection(&m_csSyncTime); return LLONG_MAX; } // ����Ԥ��ֵ long long y1(hisSync.at(0).ReceiveTime() - hisSync.at(0).TimeDelay()),y2(hisSync.at(1).ReceiveTime() - hisSync.at(1).TimeDelay()); long long x1(hisSync.at(0).RealReceiveTime()), x2(hisSync.at(1).RealReceiveTime()), x3(tag.ReceiveTime); LeaveCriticalSection(&m_csSyncTime); unsigned long long res; if(x1 > x2) { x2 += TIME_MAX; } if(x2 > x3) { x3 += TIME_MAX; } if(y1 < 0){ // ����yֵ����С��0 y1 += TIME_MAX; } if(y2 < 0 ){ y2 += TIME_MAX; } if(y1 > y2){ y2 += TIME_MAX; }else if(y2-y1 > TIME_MAX){ y2 -=TIME_MAX; } Eigen::Matrix3d a; a << x1 ,1 , 0, x2 ,1 , 0, x3, 1, -1; Eigen::Vector3d b(y1, y2, 0); Eigen::Vector3d X = a.colPivHouseholderQr().solve(b); res = X(2); res &= TIME_MAX; return res; } void HostServer::SyncManager::updateDistance(unsigned int localId, unsigned char localAntNum, unsigned int upperId, unsigned char uppderAntNum, double d) { unsigned long long lId = SyncHelper::parseId(localId, localAntNum); unsigned long long uId = SyncHelper::parseId(upperId, uppderAntNum); if(_anchors.count(lId) == 0) { _anchors[lId] = Position(); } if(_anchors.count(uId) == 0) { _anchors[uId] = Position(); } _distance[lId][uId] = d; _distance[uId][lId] = d; // ɾ�����а汾����Ϣ��¼ _historySync.clear(); _syncTimeMsgs.clear(); } void HostServer::SyncManager::updateAnchor(unsigned int localId, unsigned char localAntNum, double x, double y, double z) { deleteAnchor(localId, localAntNum); unsigned long long lId = SyncHelper::parseId(localId, localAntNum); _anchors[lId] = Position( x, y, z); } void HostServer::SyncManager::deleteAnchor(unsigned int localId, unsigned char localAntNum) { unsigned long long lId = SyncHelper::parseId(localId, localAntNum); auto it = _anchors.find(lId); if(it == _anchors.end()) return; // ɾ��anchor _anchors.erase(it); // ɾ�����anchor��صľ�����Ϣ _distance.erase(_distance.find(lId)); for(auto it(_distance.begin()); it != _distance.end(); it++) { it->second.erase(it->second.find(lId)); if(it->second.size() == 0) { it = _distance.erase(it); } } // ɾ�����а汾����Ϣ��¼ _historySync.clear(); _syncTimeMsgs.clear(); } HostServer::SyncManager::~SyncManager() { DeleteCriticalSection(&m_csSyncTime); } int HostServer::SyncManager::FindSyncTimeMsg(unsigned long long rootIdCode, unsigned short SyncNum ) { int idx = -1; for(int i = _syncTimeMsgs[rootIdCode].size() - 1; i >= 0; i--){ if(_syncTimeMsgs[rootIdCode][i].SyncNum == SyncNum ){ return i; } } return idx; } int HostServer::SyncManager::FindHisSyncTime(unsigned long long rootIdCode, unsigned short SyncNum ) { int idx = -1; for(int i = _historySync[rootIdCode].size() - 1; i >= 0; i--){ if(_historySync[rootIdCode][i].SyncNum == SyncNum ){ return i; } } return idx; }