algo-l-zmg/card_message_handle.cpp

#include <vector>
#include <ev++.h>
#include "card_base.h"
#include "loc_tool.h"
#include "message.h"
#include "zloop.h"
#include "card_message_handle.h"
#include "loc_common.h"

//一张卡一个ct的所有不同天线的信息,包括tof，tdoa，pdoa等数据处理
struct one_ct_message_handle
{
	static loc_tool_main m_loc_tool;
	ev::timer m_min_timer, m_max_timer;     // 开启两个定时器
    ev::timer m_pdoa_min_timer, m_pdoa_max_timer;
    ev::timer m_tdoa_min_timer, m_tdoa_max_timer;

	std::vector<loc_message> m_msg_list;    // 定位消息缓存区，用于tof，pdoa 
    msg_deque m_msg_deque;         // 定位消息缓存区，用于tdoa, pdoa(主要是存在一个数据包内多个ct的卡数据)
    
	card_location_base* m_card;
	const algo_config* m_ac = nullptr;      // 配置文件参数
	int  m_ct;
	bool m_min_timeout = false;
    bool m_min_timeout_pdoa = false;
    bool m_min_timeout_tdoa = false;
	ev::dynamic_loop * m_loop = nullptr;
	one_ct_message_handle(card_location_base* card)
	{
		m_card = card;
		m_ct = -1;
	}

	void reset()
	{
		m_ct = -1;
		m_min_timeout = false;
        m_min_timeout_pdoa = false;
        m_min_timeout_tdoa = false;
		m_msg_list.clear();
	}

	void on_min_timer()
	{
		m_min_timer.stop();

		if((int)m_msg_list.size()>=m_ac->best_msg_cnt)
		{
			m_max_timer.stop();
			calc_location();
			return;
		}
		m_min_timeout=true;
	}

	void on_max_timer()
	{
		m_max_timer.stop();
		calc_location();
	}

    void on_pdoa_min_timer()
    {
        m_pdoa_min_timer.stop();

        if((int)(m_msg_list.size()) >= 2){
            m_pdoa_max_timer.stop();
            calc_pdoa_location();
            return;
        }
        m_min_timeout_pdoa = true;
    }

    void on_pdoa_max_timer()
    {
        m_pdoa_max_timer.stop();
        calc_pdoa_location();
    }

    void on_tdoa_min_timer()
    {
        m_tdoa_min_timer.stop();

        if((int)(m_msg_list.size()) >= 2){
            m_tdoa_max_timer.stop();
            calc_tdoa_location();
            return;
        }
        m_min_timeout_tdoa = true;
    }

    void on_tdoa_max_timer()
    {
        m_tdoa_max_timer.stop();
        calc_tdoa_location();
    }

	void set(ev::dynamic_loop * loop)
	{
		m_loop = loop;

        // tof
		m_min_timer.set(*m_loop);
		m_min_timer.set<one_ct_message_handle,&one_ct_message_handle::on_min_timer>(this);
		m_max_timer.set(*m_loop);
		m_max_timer.set<one_ct_message_handle,&one_ct_message_handle::on_max_timer>(this);

        // pdoa
        m_pdoa_min_timer.set(*m_loop);
        m_pdoa_min_timer.set<one_ct_message_handle, &one_ct_message_handle::on_pdoa_min_timer>(this);
        m_pdoa_max_timer.set(*m_loop);
        m_pdoa_max_timer.set<one_ct_message_handle, &one_ct_message_handle::on_pdoa_max_timer>(this);
        
        // tdoa
        m_tdoa_min_timer.set(*m_loop);
        m_tdoa_min_timer.set<one_ct_message_handle, &one_ct_message_handle::on_tdoa_min_timer>(this);
        m_tdoa_max_timer.set(*m_loop);
        m_tdoa_max_timer.set<one_ct_message_handle, &one_ct_message_handle::on_tdoa_max_timer>(this);
	}

	void on_message(ev::dynamic_loop *loop,const message_locinfo&loc)
	{
		if(m_loop == nullptr && loop!=nullptr)
			set(loop);
		else if(loop == nullptr)
			return;
		
        // 如果消息队列非空，且ct切换，清空消息队列
        if(!m_msg_list.empty()&& m_ct!=loc.m_card_ct)
		{
			m_msg_list.clear();
		}
		auto s = sit_list::instance()->get(loc.m_site_id);
		if(nullptr == s)
		{
			log_warn("分站信息缺失, site_id=%d", loc.m_site_id);
			return;
		}
		
        // 如果定位数据缓存区为空，则保存定位数据
        if(m_msg_list.empty())
		{
			m_ct=loc.m_card_ct;
			m_ac=&s->config();
			m_min_timeout=false;
			//这里构造loc_message 保存数据
			m_msg_list.push_back(loc_message(s, loc.m_tof, loc.m_time_stamp, loc.m_time_tmp, loc.m_card_id,
						loc.m_card_ct,loc.m_card_type,loc.m_ant_id,loc.m_rav,loc.m_acc,
						loc.m_sync_ct,loc.m_rssi,loc.m_batty_status));

			//启动本CT的最小、最大两个定时器
			m_min_timer.start(m_ac->min_wait_time);
			m_max_timer.start(m_ac->max_wait_time);
			return;
		}

		m_msg_list.push_back(loc_message(s,loc.m_tof,loc.m_time_stamp, loc.m_time_tmp, loc.m_card_id,
					loc.m_card_ct,loc.m_card_type,loc.m_ant_id,loc.m_rav,loc.m_acc,
					loc.m_sync_ct,loc.m_rssi,loc.m_batty_status));

        // 满足数据条数，计算定位坐标
		if(m_min_timeout && (int)m_msg_list.size() >= m_ac->best_msg_cnt)
		{
			calc_location();
			m_max_timer.stop();
		}
	}
    
    /* pdoa
     * 主要实现两功能：
     * 1.将数据放入数据缓冲区；
     * 2.根据条件判断是否取出数据进行定位
     * */
    void on_message(ev::dynamic_loop* loop, const message_pdoa_locinfo& loc)
    {
    	if(nullptr == m_loop && loop != nullptr)
        {
			set(loop);
        }
		else if(nullptr == loop)
        {
			return;
        }
		
        // 如果消息队列非空，且ct切换，清空消息队列 
		auto site_ptr = sit_list::instance()->get(loc.m_site_id);
		if(nullptr == site_ptr)
		{
			log_warn("[pdoa] 分站信息缺失,site_id=%d", loc.m_site_id);
			return;
		}
       
		if(m_ct == loc.m_card_ct){
			return;
		}

        if(m_msg_list.empty()){
	        m_ac = &site_ptr->config();
			m_min_timeout_pdoa = false;
            m_ct = loc.m_card_ct;

			//启动本CT的最小、最大两个定时器
		    m_pdoa_min_timer.start(m_ac->min_wait_time);
			m_pdoa_max_timer.start(m_ac->max_wait_time);
        }

       	//std::string s = concat(loc.m_site_id, loc.m_ant_id);
        //find_msg(loc.m_card_ct);

        std::shared_ptr<loc_message> _msg_v = trans_pdoa_msg_v(loc);
        if(nullptr != _msg_v){
            //add_msg(_msg, s, idx);
            m_msg_list.push_back(*_msg_v);
            m_min_timeout_pdoa = true;
        }
        
		log_info("[pdoa] size=%d, m_min_timeout_pdoa=%d", m_msg_list.size(), m_min_timeout_pdoa);
        // 默认处理
        if(m_min_timeout_pdoa && m_msg_list.size() > 0)
        {
            calc_pdoa_location(true);
            m_pdoa_max_timer.stop();
        }
		m_msg_list.clear();
		
		std::shared_ptr<loc_message> _msg = trans_pdoa_msg(loc);
		if (nullptr != _msg) {
			//add_msg(_msg, s, idx);
			m_msg_list.push_back(*_msg);
			m_min_timeout_pdoa = true;
		}

		log_info("[pdoa] size=%d, m_min_timeout_pdoa=%d", m_msg_list.size(), m_min_timeout_pdoa);
		// 默认处理
		if (m_min_timeout_pdoa && m_msg_list.size() > 0)
		{
			calc_pdoa_location();
			m_pdoa_max_timer.stop();
		}
    }

    /* tdoa
     * 主要实现两个功能:
     * 1.将数据放入数据缓冲区;
     * 2.根据条件判断是否取出数据进行定位;
     * */
    void on_message(ev::dynamic_loop* loop, const message_tdoa_locinfo& loc)
    {
 	    if(nullptr == m_loop && loop != nullptr)
        {
			set(loop);
        }
		else if(nullptr == loop)
        {
			return;
        }
		
        // 如果消息队列非空，且ct切换，清空消息队列 
		auto site_ptr = sit_list::instance()->get(loc.m_site_msg.m_site_id);
		if(nullptr == site_ptr)
		{
			log_warn("[tdoa] 分站信息缺失,site_id=%d", loc.m_site_msg.m_site_id);
			return;
		}

        if(m_msg_list.empty()){
	        m_ac = &site_ptr->config();
			m_min_timeout_tdoa = false;

			//启动本CT的最小、最大两个定时器
			m_tdoa_min_timer.start(m_ac->min_wait_time);
			m_tdoa_max_timer.start(m_ac->max_wait_time);
        }

        std::string s = concat(loc.m_site_msg.m_site_id, loc.m_card_msg.m_ant_id);
		//把信息放入定位的缓冲区，否则放入队列内
        int idx = find_msg(loc.m_card_msg.m_time_stamp);

        std::shared_ptr<loc_message> _msg = trans_tdoa_msg(loc);
        if(nullptr != _msg){
            add_msg(_msg, s, idx);
        }

        // tdoa默认处理2维
        if(m_min_timeout_tdoa && m_msg_list.size() >= 3){
            calc_tdoa_location();
            m_tdoa_max_timer.stop();
        }
    }

    // tof定位
	void calc_location()
	{
		auto v = m_msg_list;
		if(v.empty())
		{
			return;
		}

		log_info("calc_location_begin:card_id=%d,ct=%d,m_ct=%d", m_card->m_id, v[0].m_card_ct, m_ct);

		std::vector<point> rc = std::move(m_loc_tool.calc_location(v));
		log_info("calc_location:%d size:%d",m_card->m_id,rc.size());
#if 0
        for(const auto &_p:rc)
		    log_info("calc_location:%d (%.2f,%.2f)",m_card->m_id,_p.x,_p.y);
#endif

		if(!rc.empty())
        {
            m_card->on_location(std::move(rc), v);
        }

		reset();
		log_info("calc_location_end:card_id=%d",m_card->m_id);
	}

    // pdoa定位,将定位结果传给主服务程序
	void calc_pdoa_location(bool is_v_map = false)
    {
        auto v = m_msg_list;
        if(v.empty()){
            return;
        }

        logn_info(3, "[pdoa] calc_location_pdoa_begin: card_id=%d, ct=%d, m_ct=%d", m_card->m_id, v[0].m_card_ct, m_ct);
        std::vector<point> rc = std::move(m_loc_tool.calc_location(v));
        logn_info(3, "[pdoa] calc_location_pdoa: card_id=%d, size=%d", m_card->m_id, rc.size());

        if(!rc.empty()){
			m_card->on_location(std::move(rc), v, is_v_map);
        }

        reset();
    }

    void calc_location_extend()
    {
        /*int dim = get_dimension();

        switch(dim){
            case _1D:
                break;
            case _2D:
                {
                    if(LDT_TDOA == m_card->m_loc_type){
                    }else if(LDT_PDOA == m_card->m_loc_type)
                    {
                        //m_loc_tool.set_tool()
                    }else if(LDT_TOF == m_card->m_loc_type)
                    {
                    }
                }
                break;
            case _3D:
                {
                    if(LDT_TDOA == m_card->m_loc_type){
                    }else if(LDT_PDOA == m_card->m_loc_type)
                    {
                    }else if(LDT_TOF == m_card->m_loc_type)
                    {
                    }
                }
                break;
        }*/
    }

    int get_dimension()
    {
        split_data();

        /*if(vt_msg_3d.size() > _3D){
            // 构造
            return _3D;
        }else if(vt_msg_2d.size() > _2D){
            return _2D;
        }else if(vt_msg_1d.size() > _1D){
            return _1D;
        }*/

        return 0;
    }

    int split_data()
    {
        /*if(mp_msg_3d.size() > 0){
            mp_msg_3d.erase(mp_msg_3d.begin(), mp_msg_3d.end());
        }
        if(mp_msg_2d.size() > 0){
            mp_msg_2d.erase(mp_msg_2d.begin(), mp_msg_2d.end());
        }
        if(mp_msg_1d.size() > 0){
            mp_msg_1d.erase(mp_msg_1d.begin(), mp_msg_1d.end());
        }

        for(auto it : mp_msg_.front().m_msg_map)
        {
            switch(it.second->m_loc_dimension){
                case _1D:
                    mp_msg_1d.insert(std::make_pair(it.first, it.second));
                    break;
                case _2D:
                    mp_msg_2d.insert(std::make_pair(it.first, it.second));
                    break;
                case _3D:
                    mp_msg_3d.insert(std::make_pair(it.first, it.second));
                    break;
            }
        }*/

        return 0;
    }

    // tdoa定位
    void calc_tdoa_location()
    {
        auto v = m_msg_list;
        if(v.empty()){
            return;
        }

        log_info("[tdoa] calc_location_tdoa_begin: card_id=%d, ct=%d, m_ct=%d", m_card->m_id, v[0].m_card_ct, m_ct);
        std::vector<point> rc = std::move(m_loc_tool.calc_location(v));
        log_info("[tdoa] calc_location_tdoa: card_id=%d, size=%d", m_card->m_id, rc.size());

        if(!rc.empty()){
            m_card->on_location(std::move(rc), v);
        }

        reset();
        log_info("[tdoa] calc_location_tdoa_end: card_id=%d", m_card->m_id);
    }

    /*
     * 将数据放入队列中
     * */
    void add_msg(const std::shared_ptr<loc_message>& msg, const std::string& s, const int& idx)
    {
        if(-1 != idx){
            m_msg_deque[idx].m_msg_map[s] = msg;
        }else{
            int ct_delay = ceil(3 / (10*m_card->m_freq));
            ct_delay = (ct_delay > 1)?ct_delay:1;

            uint8_t _count_del = 0;
            while(true){
                bool tag = false;
                for(auto it = m_msg_deque.begin(); it != m_msg_deque.end();){
                    bool is_over = false;
                    if(1000 > msg->m_card_ct && 64536 < (*it).m_card_stamp){
                        // 说明it为历史数据，需丢弃
                        is_over = true;
                    }
                    if((!is_over && ((*it).m_card_stamp < msg->m_card_ct - ct_delay)) || (is_over && ((*it).m_card_stamp < 65536 + msg->m_card_ct - ct_delay)))
                    {
                        if((*it).m_msg_map.size() < 3){
                            it = m_msg_deque.erase(it);
                        }else{
                            tag = true;
                            break;
                        }    
                    }else if((!is_over && ((*it).m_card_stamp > msg->m_card_ct + ct_delay)) || (is_over && ((*it).m_card_stamp > (65536 + msg->m_card_ct - ct_delay))))
                    {
                        if(is_over){
                            if((*it).m_msg_map.size() < 3){
                                it = m_msg_deque.erase(it);
                            }else{
                                tag = true;
                                break;
                            }
                        }else{
                            if(20 < ++_count_del){
                                it = m_msg_deque.erase(it);
                            }else{
                                break;
                            }
                        }
                    }else{
                        ++it;
                    }
                }
     
                if(tag){
                    //1.计算
                    for(auto item : m_msg_deque.front().m_msg_map){
                        m_msg_list.push_back(*(item.second));
                    }

                    //2.删除数据
                    m_msg_deque.pop_front();
                    m_msg_list.erase(m_msg_list.begin(), m_msg_list.end());
                    tag = false;
                }else{
                    break;
                }
            }

            message_item mi;
            mi.m_card_stamp = msg->m_card_ct;
            mi.m_msg_map[s] = msg;
            m_msg_deque.push_back(mi);
        }
    }

    // pdoa定位数据
    std::shared_ptr<loc_message> trans_pdoa_msg(const message_pdoa_locinfo& loc)
    {
        // 分站
        auto s = sit_list::instance()->get(loc.m_site_id); 
        if(nullptr == s){
            log_info("[pdoa] trans_pdoa_msg: 分站信息缺失，site_id=%d", loc.m_site_id);
            return nullptr;
        }
        return std::move(std::make_shared<loc_message>(s, loc.m_tof, loc.m_time_stamp, loc.m_time_tmp, loc.m_card_id, loc.m_card_ct, loc.m_card_type, loc.m_ant_id, loc.m_rav, loc.m_acc, loc.m_sync_ct, loc.m_rssi, loc.m_batty_status, loc.m_loc_type, loc.m_loc_dimension, loc.m_poa[0], loc.m_poa[1], loc.m_poa[2]));
    }

	// pdoa定位数据，用于车辆定位系统
	std::shared_ptr<loc_message> trans_pdoa_msg_v(const message_pdoa_locinfo& loc)
	{
		// 分站
		auto s = sit_list_v::instance()->get(loc.m_site_id);
		if (nullptr == s) {
			log_info("[pdoa] trans_pdoa_msg_v: 分站信息缺失，site_id=%d", loc.m_site_id);
			return nullptr;
		}
		return std::move(std::make_shared<loc_message>(s, loc.m_tof, loc.m_time_stamp, loc.m_time_tmp, loc.m_card_id, loc.m_card_ct, loc.m_card_type, loc.m_ant_id, loc.m_rav, loc.m_acc, 0, loc.m_rssi, loc.m_batty_status, loc.m_loc_type, loc.m_loc_dimension, loc.m_poa[0], loc.m_poa[1], loc.m_poa[2]));
	}

    // tdoa 定位数据
    std::shared_ptr<loc_message> trans_tdoa_msg(const message_tdoa_locinfo& loc)
    {
        // 分站
        auto s = sit_list::instance()->get(loc.m_site_msg.m_site_id);
        if(nullptr == s){
            log_info("[pdoa] trans_tdoa_msg: 分站信息缺失，site_id=%d", loc.m_site_msg.m_site_id);
            return nullptr;
        }
        
        return std::move(std::make_shared<loc_message>(s, loc.m_card_msg.m_loc_stamp, loc.m_card_msg.m_time_stamp, loc.m_card_msg.m_id, loc.m_card_msg.m_time_stamp, loc.m_card_msg.m_type, loc.m_card_msg.m_ant_id, loc.m_card_msg.m_rav, loc.m_card_msg.m_acc, loc.m_card_msg.m_sync_num, loc.m_card_msg.m_rssi, loc.m_card_msg.m_battery_status, loc.m_interpolation, loc.m_loc_type, loc.m_loc_dimension, m_card->m_freq));
    }

    // 分站+天线
    std::string concat(const int& lhs, const int& rhs)
    {
        char msg[10] = {0};
        snprintf(msg, 10, "%d-%d", lhs, rhs);
        return std::move(std::string(msg));
    }

    /*
     * 根据卡的ct号查找数据
     *
     * 参数
     *      stamp       卡的ct号
     *
     * */
    int find_msg(const uint16_t& stamp)
    {
        int idx = -1;
        for(int i = m_msg_deque.size() - 1; i > 0; --i){
            if(m_msg_deque[i].m_card_stamp == stamp){
                idx = i;
                break;
            }
        }

        return idx;
    }
};

loc_tool_main one_ct_message_handle::m_loc_tool;

card_message_handle::card_message_handle(card_location_base*card)
{
	m_card = card;

	for(size_t i = 0;i < m_ct_list.size();i++)
	{
		m_ct_list[i] = new one_ct_message_handle(card);
	}
}

card_message_handle::~card_message_handle()
{
	for(auto&it:m_ct_list)
	{
		delete it;
	}
}

void card_message_handle::on_message(zloop<task*> * loop,const message_locinfo&loc,bool is_history)
{
	if(is_history)
	{
		log_warn("%s","当前代码没有处理历史消息记录。");
		return;
	}
	/*int c_status = STATUS_POWER_NOMARL;
	if(loc.m_batty_status == 2)
	{
		c_status ^= STATUS_POWER_NORMAL;
		c_status  = STATUS_POWER_LOWER_SERIOUS;
	}

	if(loc.m_callinfo & 0x80)
	{
		c_status  |= STATUS_HELP;
	}

	if((loc.m_callinfo & 0x01) || (loc.m_callinfo & 0x02))
	{
		c_status  |= STATUS_CALL;
	}

	m_card->do_status(c_status);*/
	m_ct_list[loc.m_card_ct&(m_ct_list.size()-1)]->on_message(loop,loc);
}

/*
 * 算法模块
 * 1.第一步处理；
 * 2.根据主服务程序传入的数据使用算法模块进行tdoa定位
 * */
void card_message_handle::on_message(zloop<task*>* loop, const message_tdoa_locinfo& loc, bool is_history)
{
    if(is_history)
    {
        log_warn("%s","当前代码没有处理历史数据！");
        return;
    }

    /*int c_status = STATUS_POWER_LOWER_SERIOUS;

    if(loc.m_card_msg.m_call_info & 0x80)
    {
        c_status |= STATUS_HELP;
    }

    if((loc.m_card_msg.m_call_info & 0x01) || (loc.m_card_msg.m_call_info& 0x02))
    {
        c_status |= STATUS_CALL;
    }
    
    //处理卡呼叫/呼救业务
    m_card->do_status(c_status);
	*/
    m_ct_list[loc.m_card_msg.m_sync_num & (m_ct_list.size()-1)]->on_message(loop, loc);
}

/*
 * 算法模块
 * 1.第一步处理；
 * 2.根据主服务程序传入的数据使用算法模块进行pdoa定位
 * */
void card_message_handle::on_message(zloop<task*>* loop, const message_pdoa_locinfo& loc, bool is_history)
{
    if(is_history)
    {
        log_warn("%s","当前代码没有处理历史数据！");
        return;
    }

    /*int c_status = STATUS_POWER_LOWER_SERIOUS;
    if(loc.m_callinfo & 0x80)
    {
        c_status |= STATUS_HELP;
    }

    if((loc.m_callinfo & 0x01) || (loc.m_callinfo& 0x02))
    {
        c_status |= STATUS_CALL;
    }
    
    //处理卡呼叫/呼救业务
    m_card->do_status(c_status);*/
    //根据取模结果调用响应的子模块处理
    m_ct_list[loc.m_card_ct & (m_ct_list.size()-1)]->on_message(loop, loc);
}