server/SyncTime/SyncManager.cpp

#include"SyncManager.h"
#include <deque>
#include <Eigen/Dense>

HostServer::SyncManager::SyncManager()
{
    init();
}

void HostServer::SyncManager::init()
{
    _anchors.swap(unordered_map<unsigned long long, Position>());
    _syncTimeMsgs.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTimeMsg>>());
    _historySync.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTime>>());
    _distance.swap(unordered_map<unsigned long long,unordered_map<unsigned long long, double>>());
}

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<unsigned long long, long long> 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<TDOAFunctor> numDiff(functor);
    //Eigen::LevenbergMarquardt<Eigen::NumericalDiff<TDOAFunctor>,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<SyncTime> 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<unsigned short, unordered_map<unsigned long long, SyncTime>>());
    _syncTimeMsgs.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTimeMsg>>());
}

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<unsigned short, unordered_map<unsigned long long, SyncTime>>());
    _syncTimeMsgs.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTimeMsg>>());
}