server-l-zmg/ant.cpp

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <message.h>
#include <ant.h>
#include <log.h>

int ant::index()const
{
	return m_algo+(m_num_dims<<1);
}

ant_list*ant_list::instance()
{
	static ant_list _impl;
	_impl.load_from_db();
	return &_impl;
}

const algo_config&ant::config()const
{
	return g_config[m_algo+(m_num_dims<<1)];
}


loc_message::loc_message()
	:m_ant(nullptr)
	 ,m_num_ticks(0)
{
}
int loc_message::tool_index()const
{
	return m_ant->index();
}


std::vector<point> loc_tool_tdoa_3_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_3_base::index() 
{
	return 5;
}

std::vector<point> loc_tool_tdoa_2_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_2_base::index() 
{
	return 3;
}

std::vector<point> loc_tool_tdoa_1_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_1_base::index() 
{
	return 1;
}

std::vector<point> loc_tool_tof_3_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_3_base::index() 
{
	return 4;
}

std::vector<point> loc_tool_tof_2_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_2_base::index() 
{
	return 2;
}

void loc_tool_tof_1_base::on_loc_message(ant*,const message_locinfo&m)
{
	log_info("tof1-message:site=%d,ant=%d,card=%d,ct=%d,tof=%lld,rav=%02X,acc=%02X,rssi=%d",
			m.m_site_id,m.m_ant_id,m.m_card_id,m.m_card_ct,m.m_tof,m.m_rav,m.m_acc,m.m_rssi);
}

std::vector<point> loc_tool_tof_1_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_1_base::index() 
{
	return 0;
}

loc_tool_main::loc_tool_main()
{
	set_tool(new loc_tool_tof_1_base());
	set_tool(new loc_tool_tof_2_base());
	set_tool(new loc_tool_tof_3_base());
	set_tool(new loc_tool_tdoa_1_base());
	set_tool(new loc_tool_tdoa_2_base());
	set_tool(new loc_tool_tdoa_3_base());
}

loc_tool_main::~loc_tool_main()
{
	for(auto&tool:g_tool)
		delete tool;
}

loc_tool* loc_tool_main::get_tool(int index)
{
	if((uint32_t)index >= sizeof(g_tool)/sizeof(loc_tool*))
		return nullptr;
	
	return g_tool[index];
}

void loc_tool_main::set_tool(loc_tool*tool)
{
	int index=tool->index();
	if(g_tool[index])
	{
		delete g_tool[index];
		g_tool[index]=0;
	}
	g_tool[index]=tool;
}

void loc_tool_main::on_loc_message(ant*a, const message_locinfo&m)
{
	loc_tool*lt=get_tool(a->index());
	if(lt==nullptr)
	{
		log_warn("无法找到对应的loctool-message:site=%d,ant=%d,card=%d,ct=%d,tof=%lld,rav=%02X,acc=%02X,rssi=%d",
				m.m_site_id,m.m_ant_id,m.m_card_id,m.m_card_ct,m.m_tof,m.m_rav,m.m_acc,m.m_rssi);
		return;
	}

	lt->on_loc_message(a, m);
}

std::vector<point> loc_tool_main::calc_location(const std::vector<loc_message>&locm)
{
	if(locm.empty()) return {};
	int tool_index=locm[0].tool_index(),i=1,len=locm.size();
	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			break;
	}

	if(i==len)
	{
		return std::move(g_tool[tool_index]->calc_location(locm));
	}

	//包含至少两种定位方式的基站，目前只考虑两种
	std::vector<loc_message> locm1,locm2;
	locm1.assign(locm.begin(),locm.begin()+i);

	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			locm2.push_back(locm[i]);
		else
			locm1.push_back(locm[i]);
	}

	std::vector<point> rc;
	if(locm1[0].m_ant->config().best_msg_cnt<=(int)locm1.size())
	{
		rc=std::move(g_tool[tool_index]->calc_location(locm1));
	}

	if(locm1[1].m_ant->config().best_msg_cnt<=(int)locm2.size())
	{
		int index=locm2[0].tool_index();
		auto v=std::move(g_tool[index]->calc_location(locm2));
		rc.insert(rc.begin(),v.begin(),v.end());
	}

	return std::move(rc);
}

algo_config ant::g_config[]=
{
	{ "tof-1",  1, 2, 100, 1000 },
	{ "tdoa-1", 2, 2, 100, 1000 },
	{ "tof-2",  2, 3, 100, 1000 },
	{ "tdoa-2", 3, 3, 100, 1000 },
	{ "tof-3",  3, 4, 100, 1000 },
	{ "tdoa-3", 4, 4, 100, 1000 }
};