|
@@ -34,158 +34,6 @@ 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 },
|