server/sync_time/sync_manager.cpp

#include"sync_manager.h"
#include <deque>
#include <Eigen/Dense>
#include <fstream>  
#include <iostream>  
#include <log.h>

host_server::sync_manager::sync_manager()
{
    init();
}

host_server::sync_manager::sync_manager(bool status)
{
    init();
    m_log_status = status;
}

void host_server::sync_manager::init()
{
    unordered_map<unsigned long long, position> a;
    ump_anchors.swap(a);
    unordered_map<unsigned long long, unordered_map<unsigned long long, double>> b;
    ump_distance.swap(b);
	m_log_status = false;
}

void host_server::sync_manager::analyze_sync_msg(sync_time_message& msg)
{
    std::lock_guard<std::recursive_mutex> lg(m_mu_sync_time);
	// 查找root节点的时间同步序号
	int idx = find_sync_time_msg(msg.get_root_id(), msg.get_sync_num());

	if( -1 == idx){ 
		// 没找到
		// 如果时间同步消息的版本数量超过最大限制，则删除最早添加的消息
		if(ump_sync_time_msg[msg.get_root_id()].size() >= MAX_SYNCTIME_NUM){ 
			// 删除第一个,可能有泄漏
			ump_sync_time_msg[msg.get_root_id()].pop_front();
		}
		// 构造此时间同步数据保存到队列中
		sync_time_msg_item it;
		it.sync_num = msg.get_sync_num();
		it.ump_sync_time_msg[msg.get_local_id()] = msg;
		ump_sync_time_msg[msg.get_root_id()].push_back(it);

		idx = ump_sync_time_msg[msg.get_root_id()].size() - 1;
	}else{
		ump_sync_time_msg[msg.get_root_id()][idx].ump_sync_time_msg[msg.get_local_id()] = msg;
	}

	// 如果历史同步队列中的时间同步数据大于指定容量，清除数据直到满足指定容量
	while(ump_history_sync[msg.get_root_id()].size() > MAX_SYNCTIME_NUM){
		ump_history_sync[msg.get_root_id()].pop_front();
	}

    // 更新时间同步并计算
	for(auto it : ump_sync_time_msg[msg.get_root_id()][idx].ump_sync_time_msg)
	{
		update_sync(msg.get_root_id(), idx, msg.get_sync_num(), it.first);
	}
}

bool host_server::sync_manager::update_sync(unsigned long long root_id_code, int idx, unsigned short sync_num, unsigned long long local_id_code)
{
	if(-1 == idx){
		return false;
	}

    // 如果当前版本的时间同步消息未收到则返回false
	unordered_map<unsigned long long, sync_time_message>::iterator itSyncTime = ump_sync_time_msg[root_id_code][idx].ump_sync_time_msg.find(local_id_code);
	if(itSyncTime == ump_sync_time_msg[root_id_code][idx].ump_sync_time_msg.end()){
		return false;
	}

    sync_time_message& msg = ump_sync_time_msg[root_id_code][idx].ump_sync_time_msg[local_id_code];

    // 如果当前节点为root，则返回true
    if(msg.get_sync_level() == 0){
        return true;
    } 

	int idx_synctime = find_his_sync_time(root_id_code, sync_num); 

	// 如果时间同步已经计算过，则返回true
	if(-1 != idx_synctime){
		unordered_map<unsigned long long, sync_time>::iterator itHistSync = ump_history_sync[root_id_code][idx_synctime].hist_sync.find(local_id_code);
		if(itHistSync != ump_history_sync[root_id_code][idx_synctime].hist_sync.end()){
			if(itHistSync->second.get_delay_time())
				return true;
		}
	}

    // 如果已经收到了上一级的同步消息
    if(ump_sync_time_msg[root_id_code][idx].ump_sync_time_msg.count(msg.get_upper_id())){
        if(!update_sync(root_id_code, idx, sync_num, msg.get_upper_id())){
            return false;
        }

        sync_time_message &upperMsg = ump_sync_time_msg[root_id_code][idx].ump_sync_time_msg[msg.get_upper_id()];
        sync_time* s = nullptr;
        for(auto it = ump_history_sync[root_id_code].rbegin(); it != ump_history_sync[root_id_code].rend(); ++it)
        {
            if(it->sync_num != msg.get_sync_num() && it->hist_sync.count(local_id_code))
            {
                s = &(it->hist_sync.find(local_id_code)->second);
                break;
            }
        }

		idx_synctime = find_his_sync_time(root_id_code, sync_num);

		if(-1 == idx_synctime){
			sync_time_item it;
			it.sync_num = msg.get_sync_num();
			it.hist_sync[local_id_code] = sync_time(msg, upperMsg, s);
			ump_history_sync[root_id_code].push_back(it);

			idx_synctime = ump_history_sync[root_id_code].size() - 1;
		}else{
			ump_history_sync[root_id_code][idx_synctime].hist_sync[local_id_code] = sync_time(msg, upperMsg, s);
		}

        // 计算时间同步
        long long upperTimeDelay = 0;
        if(ump_sync_time_msg[root_id_code][idx].ump_sync_time_msg[msg.get_upper_id()].get_sync_level() != 0)
        {
            upperTimeDelay = ump_history_sync[root_id_code][idx_synctime].hist_sync[msg.get_upper_id()].get_delay_time();
        }
        long long sendTime = ump_history_sync[root_id_code][idx_synctime].hist_sync[local_id_code].get_send_time();
        long long receiveTime = ump_history_sync[root_id_code][idx_synctime].hist_sync[local_id_code].get_receive_time();
        long long timeDelay = receiveTime - sendTime - ump_distance[local_id_code][msg.get_upper_id()];

        timeDelay += upperTimeDelay;
		
		// 从不同的upper来的同步数据，跨周期判断可能不正确，将时间差控制在一个周期内
		while(timeDelay > TIME_MAX){
			timeDelay -= TIME_MAX;
		}
		while(timeDelay + TIME_MAX < 0 ){
			timeDelay += TIME_MAX;
		}
        ump_history_sync[root_id_code][idx_synctime].hist_sync[local_id_code].set_delay_time(timeDelay);

		if(m_log_status){
        }

        return true;
    }

    return false;
}

// 线性插值外插计算tt值
unsigned long long host_server::sync_manager::cal_time_by_linear(tag_message& tag)
{
    std::lock_guard<std::recursive_mutex> lg(m_mu_sync_time);
    log_info("[tdoa] tdoa_sync begin calc");
    // 获取历史记录中与此tag最近的两条
    deque<sync_time> hisSync;

	unsigned long long rootIdCode = tag.m_sync_root_id;

    int i = 0;
	int idx = find_sync_time_msg(rootIdCode, tag.m_sync_num);
	if(-1 != idx){
		unordered_map<unsigned long long, sync_time_message>::iterator it = ump_sync_time_msg[rootIdCode][idx].ump_sync_time_msg.find(tag.m_local_id);
		if(it != ump_sync_time_msg[rootIdCode][idx].ump_sync_time_msg.end()){
			if(it->second.get_sync_level() == 0){
				return tag.m_receive_time;
			}
		}
	}
	
	int idx_sync = -1;
    while(hisSync.size() < 2 && i < MAX_CALCLINER_NUM)
    {
        auto syncNum = tag.m_sync_num - i;
		idx_sync = find_his_sync_time(rootIdCode, syncNum);
		if(-1 != idx_sync){
			if(ump_history_sync[rootIdCode][idx_sync].hist_sync.count(tag.m_local_id)){
				hisSync.push_front(ump_history_sync[rootIdCode][idx_sync].hist_sync[tag.m_local_id]);
			}
		}
        i++;
    }
    // 如果满足条件的历史记录不足两个则返回
    if(hisSync.size() < 2){
        log_info("[tdoa] tdoa_sync his_sync's size is less 2");
		return LLONG_MAX;
	}

    // 计算预估值
    long long y1(hisSync.at(0).get_receive_time() - hisSync.at(0).get_delay_time()),y2(hisSync.at(1).get_receive_time() - hisSync.at(1).get_delay_time());
    long long x1(hisSync.at(0).get_real_receive_time()), x2(hisSync.at(1).get_real_receive_time()), x3(tag.m_receive_time);

    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;

    log_info("[tdoa] tdoa_sync_cal end, value=%lld", res);

    return res;
}

/*
 * 使用内插值计算tt值
 *
 * param
 *      tag     卡的参数信息
 *
 * return
 *      返回插值后的tt值
 * */
unsigned long long host_server::sync_manager::cal_time_by_inter_linear(tag_message& tag)
{
    std::lock_guard<std::recursive_mutex> lg(m_mu_sync_time);
    deque<sync_time> hisSync;

	unsigned long long rootIdCode = tag.m_sync_root_id;

	int i = 0;
	int idx = find_sync_time_msg(rootIdCode, tag.m_sync_num);
	if(-1 != idx){
		unordered_map<unsigned long long, sync_time_message>::iterator it = ump_sync_time_msg[rootIdCode][idx].ump_sync_time_msg.find(tag.m_local_id);
		if(it != ump_sync_time_msg[rootIdCode][idx].ump_sync_time_msg.end()){
			if(it->second.get_sync_level() == 0){
				return sub(tag.m_receive_time, it->second.get_local_send_time());
			}
		}
	}

	int idx_sync = -1;

	long long y[2] = {0};
	long long x[2] = {0};
	unsigned int r[2] = {0};

	i = 0;
	r[0] = tag.m_local_id;
	while (i<2)
	{
		idx_sync = find_sync_time_msg(rootIdCode, tag.m_sync_num + i);
		if(-1 != idx_sync){
			if(ump_sync_time_msg[rootIdCode][idx_sync].ump_sync_time_msg.count(tag.m_local_id)){
				x[i] = ump_sync_time_msg[rootIdCode][idx_sync].ump_sync_time_msg[tag.m_local_id].get_local_receive_time();
				for (auto it = ump_sync_time_msg[rootIdCode][idx_sync].ump_sync_time_msg.begin();it != ump_sync_time_msg[rootIdCode][idx_sync].ump_sync_time_msg.end();++it)
				{
					if (it->second.get_sync_level() == 0)
					{
						y[i] = it->second.get_local_send_time();
						r[1] = it->second.get_local_id();
						break;
					}
				}
			}
		}
		i++;
	}

	if (0 == x[0] || 0 == x[1] || 0 == y[0] || 0 == y[1])
	{
		return 0;
	}

	long long y1(y[0]),y2(y[1]);
	long long x1(x[0]),x2(x[1]);
	long long x3 = tag.m_receive_time;
	long long Tf = ump_distance[r[0]][r[1]];

	unsigned long long res = 0;
    //long double k = long double (sub(y2,y1)) / long double(sub(x2,x1));
	res = (long double)(sub(y2,y1)) / (long double)(sub(x2,x1)) * sub(x3,x1) + Tf;

	return res;
}

void host_server::sync_manager::update_distance(unsigned int local_id, uint8_t local_ant_num, unsigned int upper_id, uint8_t upper_ant_num, double d)
{
    unsigned long long lId = sync_helper::parse_id(local_id, local_ant_num);
    unsigned long long uId = sync_helper::parse_id(upper_id, upper_ant_num);
    if(ump_anchors.count(lId) == 0)
    {
        ump_anchors[lId] = position();
    }
    if(ump_anchors.count(uId) == 0) 
    {
        ump_anchors[uId] = position();
    }
    ump_distance[lId][uId] = d;
    ump_distance[uId][lId] = d;

    // 删除所有版本的消息记录
	ump_history_sync.clear();
	ump_sync_time_msg.clear();
}

void host_server::sync_manager::update_anchor(unsigned int local_id, uint8_t local_ant_num, double x, double y, double z)
{
    delete_anchor(local_id, local_ant_num);

    unsigned long long lId = sync_helper::parse_id(local_id, local_ant_num);
    ump_anchors[lId] = position( x, y, z);
}

/*void host_server::sync_manager::update_anchor(unsigned int local_id, unsigned char local_ant_num, position p)
{
    delete_anchor(local_id, local_ant_num);

    unsigned long long lid = sync_helper::parse_id(local_id, local_ant_num);
    ump_anchors[lid] = p;
}*/

void host_server::sync_manager::delete_anchor(unsigned int local_id, uint8_t local_ant_num)
{
    unsigned long long lId = sync_helper::parse_id(local_id, local_ant_num);
    auto it = ump_anchors.find(lId);
    if(it == ump_anchors.end()) 
        return;

    // 删除anchor
    ump_anchors.erase(it);

    // 删除与此anchor相关的距离信息
    ump_distance.erase(ump_distance.find(lId));
    for(auto it = ump_distance.begin(); it != ump_distance.end(); ++it)
    {
        it->second.erase(it->second.find(lId));
        if(it->second.size() == 0)
        {
            it = ump_distance.erase(it);
        }
    }

    // 删除所有版本的消息记录
	ump_history_sync.clear();
	ump_sync_time_msg.clear();
}

host_server::sync_manager::~sync_manager()
{
}

int host_server::sync_manager::find_sync_time_msg(unsigned long long root_id_code, unsigned short sync_num )
{
	int idx = -1;
	for(int i = ump_sync_time_msg[root_id_code].size() - 1; i >= 0; --i){
		if(ump_sync_time_msg[root_id_code][i].sync_num == sync_num ){
			idx = i;
            break;
		}
	}

	return idx;
}

int host_server::sync_manager::find_his_sync_time(unsigned long long root_id_code, unsigned short sync_num )
{
	int idx = -1;
	for(int i = ump_history_sync[root_id_code].size() - 1; i >= 0; --i){
		if(ump_history_sync[root_id_code][i].sync_num == sync_num ){
			idx = i;
            break;
		}
	}

	return idx;
}