|
@@ -15,7 +15,7 @@
|
|
|
|
|
|
struct path
|
|
|
{
|
|
|
- std::array<line_v,2> m_line;
|
|
|
+ std::array<line_v,2> m_line;
|
|
|
std::array<double,2> m_slope;
|
|
|
path()
|
|
|
{
|
|
@@ -39,25 +39,25 @@ struct path
|
|
|
{
|
|
|
return m_line[i];
|
|
|
}
|
|
|
- const line_v & operator[](int i) const
|
|
|
+ const line_v & operator[](int i) const
|
|
|
{
|
|
|
return m_line[i];
|
|
|
}
|
|
|
-
|
|
|
+
|
|
|
};
|
|
|
//?
|
|
|
struct algo_config
|
|
|
{
|
|
|
- const char*desc;
|
|
|
- int min_msg_cnt;
|
|
|
- int best_msg_cnt;
|
|
|
- double min_wait_time;
|
|
|
- double max_wait_time;
|
|
|
+ const char*desc;
|
|
|
+ int min_msg_cnt;
|
|
|
+ int best_msg_cnt;
|
|
|
+ double min_wait_time;
|
|
|
+ double max_wait_time;
|
|
|
};
|
|
|
//
|
|
|
struct ant :point
|
|
|
{
|
|
|
- std::vector<path> m_path;
|
|
|
+ std::vector<path> m_path;
|
|
|
path & operator[](int i)
|
|
|
{
|
|
|
return m_path[i];
|
|
@@ -79,7 +79,7 @@ struct ant :point
|
|
|
if(p.vaild())
|
|
|
{
|
|
|
point pt;
|
|
|
- if(dist <= p.m_line[0].length() || (dist > p.m_line[0].length() && p.m_line[1].empty()))
|
|
|
+ if(dist <= p.m_line[0].length() || (dist > p.m_line[0].length() && p.m_line[1].empty()))
|
|
|
{
|
|
|
d += d*p.m_slope[0];
|
|
|
pt = point(p.m_line[0][0].x + d*p.m_line[0].cos() , p.m_line[0][0].y + d*p.m_line[0].sin());
|
|
@@ -104,43 +104,59 @@ struct ant :point
|
|
|
/**
|
|
|
* @brief 分站位置 1井上,2井下
|
|
|
*/
|
|
|
-enum UP_DOWN_SITE
|
|
|
+enum READER_TYPE_ID
|
|
|
{
|
|
|
///井上分站
|
|
|
- UP_SITE=1,
|
|
|
+ READER_TYPE_ID_UP=1,
|
|
|
///井下分站
|
|
|
- DOWN_SITE=2
|
|
|
+ READER_TYPE_ID_DOWN=2
|
|
|
};
|
|
|
|
|
|
struct site:point
|
|
|
{
|
|
|
- static algo_config g_config[];
|
|
|
- int m_algo; //TOF:0,TDOA:1
|
|
|
- int m_num_dims; //1维:0,2维:1,3维:2
|
|
|
+ static algo_config g_config[];
|
|
|
+ int m_algo; //TOF:0,TDOA:1
|
|
|
+ int m_num_dims; //1维:0,2维:1,3维:2
|
|
|
double m_scale = 2.0; // 地图比例尺
|
|
|
|
|
|
- point m_position;
|
|
|
- int index()const;
|
|
|
- const algo_config&config()const;
|
|
|
- int id()const
|
|
|
- {
|
|
|
- return m_id;
|
|
|
- }
|
|
|
- site(int id=-1);
|
|
|
+ point m_position;
|
|
|
+ int index()const;
|
|
|
+ const algo_config&config()const;
|
|
|
+ int id()const
|
|
|
+ {
|
|
|
+ return m_id;
|
|
|
+ }
|
|
|
+ site(int id=-1);
|
|
|
+
|
|
|
+ mutable double m_height=1.5;
|
|
|
+ int m_id;
|
|
|
+ bool m_path_empty;
|
|
|
+ std::array<ant,2> m_ant;
|
|
|
+
|
|
|
+ mutable double m_ant_dist=0;
|
|
|
+ mutable double m_ant_dist_sum_new=0;
|
|
|
+ mutable int m_ant_dist_cnt_new=0;
|
|
|
|
|
|
- mutable double m_height=1.5;
|
|
|
- int m_id;
|
|
|
- bool m_path_empty;
|
|
|
- std::array<ant,2> m_ant;
|
|
|
+ ///分站位置 READER_TYPE_ID
|
|
|
+ int m_reader_type_id = 0;
|
|
|
+ int m_map_id = 0;
|
|
|
+ int m_area_id = 0;
|
|
|
+ std::string m_brief_name = "";
|
|
|
+ std::string m_name = "";
|
|
|
|
|
|
- mutable double m_ant_dist=0;
|
|
|
- mutable double m_ant_dist_sum_new=0;
|
|
|
- mutable int m_ant_dist_cnt_new=0;
|
|
|
+ double m_angle;
|
|
|
+ /// 状态 0 正常, 1 故障
|
|
|
+ int m_state=0;
|
|
|
+ std::string m_ip="";
|
|
|
|
|
|
- ///分站位置 UP_DOWN_SITE
|
|
|
- int m_up_down;
|
|
|
+ /// 设备类型,分站、通信分站、交通灯等
|
|
|
+ int m_device_type_id=0;
|
|
|
+ /// 是否为特殊分站,定位结果与分站间距4米以内,如果是特殊分站就将定位点定位在分站附近,否则抛弃计算结果
|
|
|
+ int m_isSpecial=0;
|
|
|
+ /// 指定分站定位类型:一维定位,二维定位,三维定位
|
|
|
+ int m_dimension=0;
|
|
|
|
|
|
- point get_dstp(const point pt) const
|
|
|
+ point get_dstp(const point pt) const
|
|
|
{
|
|
|
point tmp;
|
|
|
for(const auto & p : m_ant[0].m_path)
|
|
@@ -152,9 +168,9 @@ struct site:point
|
|
|
if(p[i].contain(pt,0.01))
|
|
|
{
|
|
|
//if(i==0)
|
|
|
- // return *this;
|
|
|
+ // return *this;
|
|
|
//else
|
|
|
- tmp = p[i][0];
|
|
|
+ tmp = p[i][0];
|
|
|
}
|
|
|
}
|
|
|
}
|
|
@@ -162,51 +178,51 @@ struct site:point
|
|
|
return tmp;
|
|
|
}
|
|
|
|
|
|
- void count_ant_dist(double dist_tof1, double dist_tof2)const
|
|
|
- {
|
|
|
- if(dist_tof1<10 || dist_tof2<10)
|
|
|
- return;
|
|
|
- double dist = fabs(dist_tof1 - dist_tof2);
|
|
|
- if(dist>5)
|
|
|
- return;
|
|
|
- m_ant_dist_sum_new += dist;
|
|
|
- m_ant_dist_cnt_new++;
|
|
|
-
|
|
|
- if(m_ant_dist_cnt_new >= 2500)
|
|
|
- {
|
|
|
- m_ant_dist = m_ant_dist_sum_new / m_ant_dist_cnt_new;
|
|
|
- m_ant_dist_sum_new = 0;
|
|
|
- m_ant_dist_cnt_new = 0;
|
|
|
- }
|
|
|
- }
|
|
|
+ void count_ant_dist(double dist_tof1, double dist_tof2)const
|
|
|
+ {
|
|
|
+ if(dist_tof1<10 || dist_tof2<10)
|
|
|
+ return;
|
|
|
+ double dist = fabs(dist_tof1 - dist_tof2);
|
|
|
+ if(dist>5)
|
|
|
+ return;
|
|
|
+ m_ant_dist_sum_new += dist;
|
|
|
+ m_ant_dist_cnt_new++;
|
|
|
+
|
|
|
+ if(m_ant_dist_cnt_new >= 2500)
|
|
|
+ {
|
|
|
+ m_ant_dist = m_ant_dist_sum_new / m_ant_dist_cnt_new;
|
|
|
+ m_ant_dist_sum_new = 0;
|
|
|
+ m_ant_dist_cnt_new = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
void swap()
|
|
|
{
|
|
|
- auto v0 = m_ant[0].m_path;
|
|
|
- auto v1 = m_ant[1].m_path;
|
|
|
- std::copy (std::begin(v0),std::end(v0),std::back_inserter(m_ant[1].m_path));
|
|
|
- std::copy (std::begin(v1),std::end(v1),std::back_inserter(m_ant[0].m_path));
|
|
|
+ auto v0 = m_ant[0].m_path;
|
|
|
+ auto v1 = m_ant[1].m_path;
|
|
|
+ std::copy (std::begin(v0),std::end(v0),std::back_inserter(m_ant[1].m_path));
|
|
|
+ std::copy (std::begin(v1),std::end(v1),std::back_inserter(m_ant[0].m_path));
|
|
|
+ }
|
|
|
+ double ant_dist()const
|
|
|
+ {
|
|
|
+ return m_ant[0].dist(m_ant[1]);
|
|
|
}
|
|
|
- double ant_dist()const
|
|
|
- {
|
|
|
- return m_ant[0].dist(m_ant[1]);
|
|
|
- }
|
|
|
-
|
|
|
- bool is_path_empty()const
|
|
|
- {
|
|
|
+
|
|
|
+ bool is_path_empty()const
|
|
|
+ {
|
|
|
return m_path_empty;
|
|
|
- }
|
|
|
+ }
|
|
|
|
|
|
- bool have_valid_path()const
|
|
|
- {
|
|
|
+ bool have_valid_path()const
|
|
|
+ {
|
|
|
return m_id != -1 && ant_dist() > 0.1;
|
|
|
- }
|
|
|
+ }
|
|
|
|
|
|
- std::string to_string()const
|
|
|
- {
|
|
|
+ std::string to_string()const
|
|
|
+ {
|
|
|
std::stringstream ss;
|
|
|
ss<<"site_id:"<<m_id<<"x:"<<x<<" y: "<<y<<" scale:"<<m_scale;
|
|
|
for(const auto a:m_ant)
|
|
|
- {
|
|
|
+ {
|
|
|
ss<<"<";
|
|
|
for(const auto p:a.m_path)
|
|
|
{
|
|
@@ -215,33 +231,33 @@ struct site:point
|
|
|
ss<<">";
|
|
|
}
|
|
|
return ss.str();
|
|
|
- }
|
|
|
+ }
|
|
|
|
|
|
- const point&path(int i)const
|
|
|
- {
|
|
|
+ const point&path(int i)const
|
|
|
+ {
|
|
|
static point p;
|
|
|
if(i>=(int)m_ant[0].m_path.size())
|
|
|
return p ;
|
|
|
- return m_ant[0].m_path[i].m_line[0][1];
|
|
|
- }
|
|
|
+ return m_ant[0].m_path[i].m_line[0][1];
|
|
|
+ }
|
|
|
|
|
|
std::vector<point> solving(int ant_id, double dist)const
|
|
|
- {
|
|
|
+ {
|
|
|
const ant &a = m_ant[ant_id];
|
|
|
if(dist<50 && dist>0)
|
|
|
- {
|
|
|
- if(dist<m_height)
|
|
|
- {
|
|
|
- m_height=dist;
|
|
|
- dist=0;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- dist=sqrt(dist*dist-m_height*m_height);
|
|
|
- }
|
|
|
- }
|
|
|
+ {
|
|
|
+ if(dist<m_height)
|
|
|
+ {
|
|
|
+ m_height=dist;
|
|
|
+ dist=0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ dist=sqrt(dist*dist-m_height*m_height);
|
|
|
+ }
|
|
|
+ }
|
|
|
return std::move(a.getsol(dist));
|
|
|
- }
|
|
|
+ }
|
|
|
ant operator[](int i)
|
|
|
{
|
|
|
return m_ant[i];
|
|
@@ -255,43 +271,45 @@ struct site:point
|
|
|
|
|
|
struct sit_list:single_base<sit_list,int,std::shared_ptr<site>>
|
|
|
{
|
|
|
- void load(const char*ant_file,const char*path_file)
|
|
|
- {
|
|
|
- read_sit_list(ant_file);
|
|
|
- read_ant_path(path_file);
|
|
|
- }
|
|
|
+ void load(const char*ant_file,const char*path_file)
|
|
|
+ {
|
|
|
+ read_sit_list(ant_file);
|
|
|
+ read_ant_path(path_file);
|
|
|
+ }
|
|
|
|
|
|
- void load_from_db()
|
|
|
- {
|
|
|
- load("data_reader_antenna.txt","path_tof.txt");
|
|
|
- }
|
|
|
+ void load_from_db()
|
|
|
+ {
|
|
|
+ load("data_reader_antenna.txt","path_tof.txt");
|
|
|
+ init_site();
|
|
|
+ }
|
|
|
|
|
|
- void read_sit_list(const char*fname);
|
|
|
- void read_ant_path(const char*fname);
|
|
|
+ void read_sit_list(const char*fname);
|
|
|
+ void read_ant_path(const char*fname);
|
|
|
+ void init_site();
|
|
|
};
|
|
|
|
|
|
struct loc_message
|
|
|
{
|
|
|
- site m_sit;
|
|
|
- uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
|
|
|
- uint64_t m_loc_time;
|
|
|
- uint32_t m_card_id;
|
|
|
- int32_t m_card_ct;
|
|
|
- int8_t m_card_type;
|
|
|
- int8_t m_ant_id;
|
|
|
- int16_t m_rav;
|
|
|
- int16_t m_acc;
|
|
|
- uint16_t m_sync_ct;
|
|
|
- uint16_t m_rssi;
|
|
|
+ site m_sit;
|
|
|
+ uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
|
|
|
+ uint64_t m_loc_time;
|
|
|
+ uint32_t m_card_id;
|
|
|
+ int32_t m_card_ct;
|
|
|
+ int8_t m_card_type;
|
|
|
+ int8_t m_ant_id;
|
|
|
+ int16_t m_rav;
|
|
|
+ int16_t m_acc;
|
|
|
+ uint16_t m_sync_ct;
|
|
|
+ uint16_t m_rssi;
|
|
|
|
|
|
|
|
|
- loc_message();
|
|
|
- loc_message(site s,uint64_t num_ticks,uint64_t timestamp,
|
|
|
- uint32_t cardid,int32_t ct,int8_t type,int8_t antid,
|
|
|
- int16_t rav,int16_t acc,uint16_t sync_ct,uint16_t rssi)
|
|
|
- :m_sit(s)
|
|
|
- ,m_num_ticks(num_ticks)
|
|
|
- ,m_loc_time(timestamp)
|
|
|
+ loc_message();
|
|
|
+ loc_message(site s,uint64_t num_ticks,uint64_t timestamp,
|
|
|
+ uint32_t cardid,int32_t ct,int8_t type,int8_t antid,
|
|
|
+ int16_t rav,int16_t acc,uint16_t sync_ct,uint16_t rssi)
|
|
|
+ :m_sit(s)
|
|
|
+ ,m_num_ticks(num_ticks)
|
|
|
+ ,m_loc_time(timestamp)
|
|
|
,m_card_id(cardid)
|
|
|
,m_card_ct(ct)
|
|
|
,m_card_type(type)
|
|
@@ -300,8 +318,8 @@ struct loc_message
|
|
|
,m_acc(acc)
|
|
|
,m_sync_ct(sync_ct)
|
|
|
,m_rssi(rssi)
|
|
|
- {}
|
|
|
- int tool_index()const;
|
|
|
+ {}
|
|
|
+ int tool_index()const;
|
|
|
};
|
|
|
|
|
|
#endif
|