#include"SyncManager.h" #include #include HostServer::SyncManager::SyncManager() { init(); } void HostServer::SyncManager::init() { _anchors.swap(unordered_map()); _syncTimeMsgs.swap(unordered_map>()); _historySync.swap(unordered_map>()); _distance.swap(unordered_map>()); } void HostServer::SyncManager::analyzeSyncMsg(SyncTimeMsg &msg) { // 如果时间同步消息的版本数量超过最大限制,则删除最早添加的消息 if(_syncTimeMsgs.size() >= MAX_SYNCTIME_NUM && _syncTimeMsgs.count(msg.SyncNum()) == 0) { if(_historySync.begin()->first == _syncTimeMsgs.begin()->first){ _historySync.erase(_historySync.begin()); } _syncTimeMsgs.erase(_syncTimeMsgs.begin()); } // 将msg添加到_historySyncTimeMsg _syncTimeMsgs[msg.SyncNum()][msg.LocalIdCode()] = msg; // 更新时间同步并计算 for(auto it :_syncTimeMsgs[msg.SyncNum()]) { updateSync(msg.SyncNum(), it.first); } } bool HostServer::SyncManager::updateSync(unsigned short SyncNum, unsigned long long localIdCode) { // 如果当前版本的时间同步消息未收到则返回false if(!_syncTimeMsgs.count(SyncNum) || !_syncTimeMsgs[SyncNum].count(localIdCode)) { return false; } SyncTimeMsg &msg = _syncTimeMsgs[SyncNum][localIdCode]; // 如果当前的为root,则返回true if(msg.SyncLevel() == 0) { return true; } // 如果时间同步已经计算过,则返回true if(_historySync.count(SyncNum) && _historySync[SyncNum].count(localIdCode) && _historySync[SyncNum][localIdCode].TimeDelay()) { return true; } // 如果已经收到了上一级的同步消息 if(_syncTimeMsgs[SyncNum].count(msg.UpperIdCode())) { if(!updateSync(SyncNum, msg.UpperIdCode())) { return false; } SyncTimeMsg &upperMsg = _syncTimeMsgs[SyncNum][msg.UpperIdCode()]; SyncTime* s = NULL; for(auto it(_historySync.rbegin()); it != _historySync.rend(); ++it) { if(it->first != msg.SyncNum() && it->second.count(localIdCode)) { s = &(it->second.find(localIdCode)->second); break; } } _historySync[SyncNum][localIdCode] = SyncTime(msg, upperMsg, s); // 计算时间同步 long long upperTimeDelay = 0; if(_syncTimeMsgs[SyncNum][msg.UpperIdCode()].SyncLevel() != 0) { upperTimeDelay = _historySync[SyncNum][msg.UpperIdCode()].TimeDelay(); } long long sendTime = _historySync[SyncNum][localIdCode].SendTime(); long long receiveTime = _historySync[SyncNum][localIdCode].ReceiveTime(); long long timeDelay = receiveTime - sendTime - _distance[localIdCode][msg.UpperIdCode()]; timeDelay += upperTimeDelay; _historySync[SyncNum][localIdCode].TimeDelay(timeDelay); return true; } return false; } HostServer::Position HostServer::SyncManager::analyzeTagMsg(TagMsg &tagMsg, unsigned long long tagId, unsigned short tagNum) { _tagRecords[tagId][tagNum].push_back(tagMsg); // 获取预估的时间差 unordered_map times; if(!_tagRecords.size() || !_tagRecords[tagId].size() || _tagRecords[tagId][tagNum].size() != 4) { return Position(DBL_MAX, DBL_MAX, DBL_MAX); } for(auto tag : _tagRecords[tagId][tagNum]) { auto res = calTimeByLinar(tag); if(res == LLONG_MAX) return Position(DBL_MAX, DBL_MAX, DBL_MAX); times[tag.StationIdCode] = res; } // 初始化矩阵 auto begin = times.begin(); auto it = begin; it++; //Eigen::VectorXd x(3); //Eigen::MatrixXd data(3,4), pos(1,3); //pos(0,0) = _anchors[begin->first].x; //pos(0,1) = _anchors[begin->first].y; //pos(0,2) = _anchors[begin->first].z; //// 设置tag的初始坐标 // x << 500,500,100; //cout << begin->first << " " << pos(0,0) << ", "<< pos(0,1) << ", "<< pos(0,2) << endl; //ofstream fout("test/linar_" + to_string(tagId) + ".txt",ios::app); //for (int i = 0; it != times.end(); it++, i++) //{ // data(i,0) = _anchors[it->first].x; // data(i,1) = _anchors[it->first].y; // data(i,2) = _anchors[it->first].z; // data(i,3) = (it->second - begin->second) * 15.65 * 2.99702547 * 0.01; // cout << it->first << " " << data(i,0) << ", " << data(i,1) << ", " << data(i,2) << ", "<< data(i,3) <<";" << endl; // fout << hex << it->first << ' ' << begin->first << ' ' << data(i,3) << endl; //} //TDOAFunctor functor(data, pos); //Eigen::NumericalDiff numDiff(functor); //Eigen::LevenbergMarquardt,double> lm(numDiff); //lm.parameters.maxfev = 2000; //lm.parameters.xtol = 1.0e-10; //// 计算后,如果计算出结果则返回该结果 //int ret = lm.minimize(x); //if(ret > 0) //{ // Position p(x[0],x[1],x[2]); // cout << p.x * 17 / 15<< " " << p.y * 17 / 15<< " " << p.z * 17 / 15<< endl; // fout << p.x * 17 / 15<< " " << p.y * 17 / 15<< " " << p.z * 17 / 15<< endl; // fout << endl; // fout.close(); // return p; //} return Position(DBL_MAX, DBL_MAX, DBL_MAX); } unsigned long long HostServer::SyncManager::calTimeByLinar(TagMsg &tag) { // 获取历史记录中与此tag最近的两条 deque hisSync; //ofstream fout("test/linar_513.txt",ios::app); int i = 0; //for(int j = 0; j < MAX_ROOT_DELAY_NUM; j++){ // 增加判断次数 if(_syncTimeMsgs[tag.SyncNum][tag.StationIdCode].SyncLevel() == 0) { //fout << hex << tag.SyncNum << ' ' << tag.StationIdCode << ' ' << tag.ReceiveTime<< endl; return tag.ReceiveTime; } //} while(hisSync.size() < 2 && i < MAX_SYNCTIME_NUM) { auto syncNum = tag.SyncNum - i; if(_historySync.count(syncNum) && _historySync[syncNum].count(tag.StationIdCode)) { // return (tag.ReceiveTime + (_historySync[syncNum][tag.StationIdCode].IsRTimeOverflow() ? TIME_MAX : 0) - _historySync[syncNum][tag.StationIdCode].TimeDelay()) & TIME_MAX; hisSync.push_front(_historySync[syncNum][tag.StationIdCode]); } i++; } // 如果满足条件的历史记录不足两个则返回 if(hisSync.size() < 2) return LLONG_MAX; // 计算预估值 auto y1(hisSync.at(0).ReceiveTime() - hisSync.at(0).TimeDelay()),y2(hisSync.at(1).ReceiveTime() - hisSync.at(1).TimeDelay()); auto x1(hisSync.at(0).RealReceiveTime()), x2(hisSync.at(1).RealReceiveTime()), x3(tag.ReceiveTime); unsigned long long res; if(x1 > x2) { x2 += TIME_MAX; } if(x2 > x3) { x3 += TIME_MAX; } if(y1 > y2) { 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; //fout << hex << tag.SyncNum << ' ' << tag.StationIdCode << endl // << "RecieveTime " << hisSync.at(0).RealReceiveTime() << ' ' << hisSync.at(1).RealReceiveTime() << ' ' << tag.ReceiveTime << endl // << "RootTime " << hisSync.at(0).RealSendTime() << ' ' << hisSync.at(1).RealSendTime() << ' ' << res << endl // << "TimeDelay " << hisSync.at(0).TimeDelay() << ' ' << hisSync.at(1).TimeDelay() << " " << endl; 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.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.swap(unordered_map>()); _syncTimeMsgs.swap(unordered_map>()); }