#include "stdafx.h" #include"SyncManager.h" #include #include #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); //InitializeCriticalSection(&m_csCalcLiner); _anchors.swap(unordered_map()); //_syncTimeMsgs.clear(); //_historySync.clear(); _distance.swap(unordered_map>()); } 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){ // 没找到 // 如果时间同步消息的版本数量超过最大限制,则删除最早添加的消息 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{ _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){ return false; } // 如果当前版本的时间同步消息未收到则返回false unordered_map::iterator itSyncTime = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(localIdCode); if(itSyncTime == _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.end()){ return false; } SyncTimeMsg &msg = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[localIdCode]; // 如果当前的为root,则返回true if(msg.SyncLevel() == 0){ return true; } int idx_synctime = FindSyncTime(rootIdCode, SyncNum); // 如果时间同步已经计算过,则返回true if(-1 != idx_synctime){ unordered_map::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())){ if(!updateSync(rootIdCode, idx, SyncNum, msg.UpperIdCode())){ 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 = FindSyncTime(rootIdCode, SyncNum); if(-1 == idx_synctime){ 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{ _historySync[rootIdCode][idx_synctime].HistSync[localIdCode] = SyncTime(msg, upperMsg, s); } // 计算时间同步 long long upperTimeDelay = 0; if(_syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[msg.UpperIdCode()].SyncLevel() != 0) { 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; if(timeDelay > TIME_MAX){ timeDelay -= TIME_MAX; } if(timeDelay + TIME_MAX < 0 ){ timeDelay += TIME_MAX; } _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].TimeDelay(timeDelay); debug_print_syslog(0, "Delay info, syncnum:%d, station id: 0x%x, timeDelay:%I64u", SyncNum, localIdCode, timeDelay); LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_69); return true; } return false; } unsigned long long HostServer::SyncManager::calTimeByLinar(TagMsg &tag) { //EnterCriticalSection(&m_csCalcLiner); EnterCriticalSection(&m_csSyncTime); // 获取历史记录中与此tag最近的两条 deque hisSync; //ofstream fout("test/linar_513.txt",ios::app); unsigned long long rootIdCode = tag.SyncRootIdCode; int i = 0; int idx = FindSyncTimeMsg(rootIdCode, tag.SyncNum); if(-1 != idx){ unordered_map::iterator it = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(tag.StationIdCode); if(it != _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.end()){ if(it->second.SyncLevel() == 0){ //LeaveCriticalSection(&m_csCalcLiner); 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 = FindSyncTime(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_csCalcLiner); 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(); //_historySync.swap(unordered_map>()); //_syncTimeMsgs.swap(unordered_map>()); } 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(); //_historySync.swap(unordered_map>()); //_syncTimeMsgs.swap(unordered_map>()); } HostServer::SyncManager::~SyncManager() { //DeleteCriticalSection(&m_csCalcLiner); 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::FindSyncTime(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; }