防碰撞系统坐标定位

ant.cpp

@@ -725,3 +725,393 @@ int main()
 }
 #endif
 */
+
+void sit_list_v::read_sit_list(int id /*= -1*/)
+{
+	std::string sql = "SELECT antenna_id, a.reader_id, idx, a.x, a.y, a.z, a.angle \
+            FROM dat_antenna_v a, dat_reader_v r \
+            WHERE a.reader_id = r.reader_id";
+
+	if (-1 == id)
+	{
+		sql.append(";");
+	}
+	else
+	{
+		sql.append(" AND antenna_id=");
+		sql.append(std::to_string(id));
+		sql.append(";");
+
+		log_info("基础数据 增加或修改天线 sql=%s", sql.c_str());
+	}
+
+	std::string Error;
+	YADB::CDBResultSet DBRes;
+	sDBConnPool.Query(sql.c_str(), DBRes, Error);
+	int nCount = DBRes.GetRecordCount(Error);
+	if (nCount < 1)
+	{
+		log_error("基础数据 增加或修改失败，数据库中找不到: 天线id=%d", id);
+		return;
+	}
+
+	log_info("init_antenna. The record count=%ld\n", nCount);
+	while (DBRes.GetNextRecod(Error))
+	{
+		int antenna_id = 0;
+		DBRes.GetField("antenna_id", antenna_id, Error);
+
+		int reader_id = 0;
+		DBRes.GetField("reader_id", reader_id, Error);
+
+		int idx = 0;
+		DBRes.GetField("idx", idx, Error);
+
+		int antid = idx - 1;
+		if (antid >= 2 || antid < 0)
+			continue;
+
+		double x = 0;
+		DBRes.GetField("x", x, Error);
+
+		double y = 0;
+		DBRes.GetField("y", y, Error);
+
+		double z = 0;
+		DBRes.GetField("z", z, Error);
+
+		double _angle = 0;
+		DBRes.GetField("angle", _angle, Error);
+
+		log_info("ant-position:reader=%d, antid=%d, x=%.2lf, y=%.2lf, angle=%.2f", reader_id, antid, x, y, _angle);
+
+		if (-1 == id)
+		{
+			std::shared_ptr<site> site_ptr = nullptr;
+			site_ptr = sit_list_v::instance()->get(reader_id);
+			if (!site_ptr)
+				continue;
+			site_ptr->m_ant[antid].m_id = antenna_id;
+			site_ptr->m_ant[antid].set(x, y);
+			site_ptr->m_ant[antid].m_angle = _angle;
+			site_ptr->set_ex();
+		}
+		else
+		{
+			auto site_ptr = sit_list_v::instance()->get(reader_id);
+			if (site_ptr)
+			{
+				site_ptr->m_ant[antid].set(x, y);
+				site_ptr->m_ant[antid].m_angle = _angle;
+				site_ptr->set_ex();
+			}
+			log_info("基础数据 增加或修改天线成功：天线id:%d,分站id:%d", id, reader_id);
+		}
+	}
+}
+
+void sit_list_v::read_ant_path(int id /*= -1*/)
+{
+	std::string sql = "SELECT reader_id,tof_flag,b_x,b_y,b_z,e_x,e_y,e_z,spacing_ratio FROM dat_reader_path_tof_n_v";
+
+	std::map<int, std::vector<line_v>> map_path;
+
+	if (-1 == id)
+	{
+		sql.append(";");
+	}
+	else
+	{
+		sql.append(" where reader_id=");
+		sql.append(std::to_string(id));
+		sql.append(";");
+
+		std_debug("修改path sql=%s", sql.c_str());
+		log_info("修改path sql=%s", sql.c_str());
+	}
+
+	std::string Error;
+	YADB::CDBResultSet DBRes;
+	sDBConnPool.Query(sql.c_str(), DBRes, Error);
+	int nCount = DBRes.GetRecordCount(Error);
+	if (nCount < 1)
+	{
+		log_error("修改path失败，数据库中找不到(dat_reader_path_tof_n_v): 分站id=%d", id);
+		return;
+	}
+
+	log_info("read_ant_path. The record count=%ld\n", nCount);
+	while (DBRes.GetNextRecod(Error))
+	{
+		int reader_id = 0;
+		DBRes.GetField("reader_id", reader_id, Error);
+
+		auto site_ptr = sit_list_v::instance()->get(reader_id);
+		if (nullptr == site_ptr)
+		{
+			log_error("[site] 定义了分站路径，但是没有定义分站:%d", reader_id);
+			continue;
+		}
+		else if (site_ptr->m_num_dims != 1) {
+			log_info("[site] 多维定位分站，无需定义路径:%d", reader_id);
+			continue;
+		}
+
+		int pid = 0;
+		DBRes.GetField("tof_flag", pid, Error);
+
+		double b_x = 0;
+		DBRes.GetField("b_x", b_x, Error);
+
+		double b_y = 0;
+		DBRes.GetField("b_y", b_y, Error);
+
+		double b_z = 0;
+		DBRes.GetField("b_z", b_z, Error);
+
+		double e_x = 0;
+		DBRes.GetField("e_x", e_x, Error);
+
+		double e_y = 0;
+		DBRes.GetField("e_y", e_y, Error);
+
+		double e_z = 0;
+		DBRes.GetField("e_z", e_z, Error);
+
+		double spacing_ratio = 0;
+		DBRes.GetField("spacing_ratio", spacing_ratio, Error);
+
+		log_info("src-path:site=%d,x0=%.2f,y0=%.2f,x1=%.2f,y1=%.2f", reader_id, b_x, b_y, e_x, e_y);
+
+		point p1(b_x, b_y, spacing_ratio);
+		point p2(e_x, e_y, spacing_ratio);
+
+		map_path.insert(std::make_pair(reader_id, std::vector<line_v>()));
+		map_path.find(reader_id)->second.push_back(line_v(p1, p2));
+#if 0
+
+		auto &sit_ = *site_ptr;
+		if (-1 != id)//清空path
+		{
+			sit_.clear_path();
+			log_info("修改path 清空path，分站id=%d\n", id);
+		}
+
+		if (pid == 0)
+		{
+			line_v l(p1, p2);
+			{
+				point px = l.line::projection(sit_);
+				sit_.set(px);
+				for (int i = 0; i < 2; i++)
+				{
+					path p;
+					p.m_slope[0] = spacing_ratio;
+					p.m_line[0] = line_v(px, l[i]);
+					sit_.m_ant[i].m_path.push_back(p);
+				}
+			}
+		}
+		else
+		{
+			ant &a = pid < 0 ? sit_.m_ant[0] : sit_.m_ant[1];
+			if (a.m_path.size() != 0)
+			{
+				path &p = a.m_path[0];
+				p.m_line[abs(pid) - 1] = line_v(p1, p2);
+				p.m_slope[abs(pid) - 1] = spacing_ratio;
+			}
+			else
+			{
+				path p;
+				p.m_line[abs(pid) - 1] = line_v(p1, p2);
+				p.m_slope[abs(pid) - 1] = spacing_ratio;
+				a.m_path.push_back(p);
+			}
+			if (abs(pid) == 1)
+				sit_.set(p1);
+		}
+#endif
+	}
+
+#if 0
+	if (-1 == id)
+	{
+		for (auto&_s : sit_list::instance()->m_map)
+		{
+			_s.second->deal_path();
+		}
+	}
+	else
+	{
+		auto sit_ptr = sit_list::instance()->get(id);
+		if (sit_ptr)
+		{
+			sit_ptr->deal_path();
+			log_info("修改path成功，分站id=%d\n", id);
+		}
+	}
+#endif
+
+	for (auto&vl : map_path)
+	{
+		auto sit_ptr = sit_list_v::instance()->get(vl.first);
+		if (!sit_ptr)
+		{
+			log_error("[site] 定义了分站路径，但是没有定义分站:%d", vl.first);
+			continue;
+		}
+		else if (sit_ptr->m_num_dims > 1) {
+			log_info("[site] 多维定位分站，无需定义路径:%d", vl.first);
+			continue;
+		}
+
+		sit_ptr->set_path(vl.second);
+		point p;
+		if (!sit_ptr->m_path_empty) {
+			p = (*sit_ptr)[0][0][0].line::projection(*sit_ptr);
+			sit_ptr->set(p);
+		}
+		if (p == *sit_ptr)
+			log_info("[site_path]%s", sit_ptr->to_string().c_str());
+		else
+			log_info("[site_path_diff](%f,%f)--%f--%s", p.x, p.y, p.dist(*sit_ptr), sit_ptr->to_string().c_str());
+	}
+
+	card_path::init();
+}
+
+void sit_list_v::init_site(const std::string &ids /*= ""*/)
+{
+	/*std::string sql = "SELECT reader_id, reader_type_id, dat_reader_v.map_id, \
+	  area_id, device_type_id, dimension, dat_map.scale,need_power_alarm \
+	  ,x,y, pdoa_offset, pdoa_direction, isSpecial, down_stream_idx \
+	  FROM dat_reader_v, dat_map where \
+	  dat_reader_v.map_id=dat_map.map_id and state=0";*/
+	std::string sql = "SELECT r.reader_id, r.reader_type_id, r.map_id, r.area_id, r.device_type_id, r.dimension, m.scale, r.need_power_alarm, r.x, r.y, \
+					   r.pdoa_offset, r.pdoa_direction, r.isSpecial, r.down_stream_idx, \
+					   rc.plus_dist, rc.plus_card_id, rc.plus_occur_time, rc.minus_dist, rc.minus_card_id, rc.minus_occur_time \
+					   FROM dat_reader_v AS r \
+					   LEFT JOIN dat_map AS m ON r.map_id=m.map_id \
+					   LEFT JOIN his_reader_coverage AS rc ON r.reader_id = rc.reader_id \
+					   WHERE r.state=0;";
+
+	if (ids.empty())
+	{
+		sql.append(";");
+	}
+	else
+	{
+		sql.append(" AND reader_id in (");
+		sql += ids;
+		sql.append(");");
+
+		std_debug("增加或修改分站 sql=%s", sql.c_str());
+		log_info("增加或修改分站 sql=%s", sql.c_str());
+	}
+
+	std::string Error;
+	YADB::CDBResultSet DBRes;
+	sDBConnPool.Query(sql.c_str(), DBRes, Error);
+	int nCount = DBRes.GetRecordCount(Error);
+	if (nCount < 1)
+	{
+		log_error("增加或修改失败，数据库中找不到: 分站id=%s", ids.c_str());
+		return;
+	}
+
+	log_info("init_site. The record count=%ld\n", nCount);
+
+	while (DBRes.GetNextRecod(Error))
+	{
+		int reader_id = 0;
+		DBRes.GetField("reader_id", reader_id, Error);
+
+		auto site_ptr = sit_list_v::instance()->get(reader_id);
+		if (nullptr == site_ptr)
+		{
+			site_ptr = std::make_shared<site>(reader_id);
+			sit_list_v::instance()->add(reader_id, site_ptr);
+		}
+
+		int reader_type_id = 0;
+		DBRes.GetField("reader_type_id", reader_type_id, Error);
+
+		int map_id = 0;
+		DBRes.GetField("map_id", map_id, Error);
+
+		int area_id = 0;
+		DBRes.GetField("area_id", area_id, Error);
+
+		int device_type_id = 0;
+		DBRes.GetField("device_type_id", device_type_id, Error);
+
+		int dimension = 0;
+		DBRes.GetField("dimension", dimension, Error);
+
+		double scale = 0;
+		DBRes.GetField("scale", scale, Error);
+
+		int power_alarm = 0;
+		DBRes.GetField("need_power_alarm", power_alarm, Error);
+
+		int down_stream_idx = 0;
+		DBRes.GetField("down_stream_idx", down_stream_idx, Error);
+
+		site_ptr->m_reader_type_id = reader_type_id;
+		site_ptr->m_map_id = map_id;
+		site_ptr->m_area_id = area_id;
+		site_ptr->m_power_check_enable = power_alarm;
+		site_ptr->m_device_type_id = device_type_id;
+		site_ptr->m_num_dims = dimension;
+		site_ptr->m_scale = scale;
+		site_ptr->m_down_stream_idx = down_stream_idx;
+		site_ptr->create_area();
+
+		double x = 0, y = 0;
+		DBRes.GetField("x", x, Error);
+		DBRes.GetField("y", y, Error);
+		site_ptr->x = x;
+		site_ptr->y = y;
+
+		double offset = 0.0;
+		DBRes.GetField("pdoa_offset", offset, Error);
+		site_ptr->m_pdoa_offset = offset;
+
+		int direction = 0;
+		if (DBRes.GetField("pdoa_direction", direction, Error)) {
+			site_ptr->m_pdoa_direction = direction;
+		}
+
+		int special = 0;
+		if (DBRes.GetField("isSpecial", special, Error)) {
+			site_ptr->m_special = special;
+		}
+
+		double dist = 0;
+		DBRes.GetField("plus_dist", dist, Error);
+		site_ptr->m_coverage[0].m_distance = dist;
+
+		std::string ctx = "";
+		DBRes.GetField("plus_card_id", ctx, Error);
+		site_ptr->m_coverage[0].m_cid = ctx;
+
+		ctx.clear();
+		DBRes.GetField("plus_occur_time", ctx, Error);
+		site_ptr->m_coverage[0].m_time = tool_time::to_time(ctx);
+
+		dist = 0;
+		DBRes.GetField("plus_dist", dist, Error);
+		site_ptr->m_coverage[1].m_distance = dist;
+
+		ctx.clear();
+		DBRes.GetField("plus_card_id", ctx, Error);
+		site_ptr->m_coverage[1].m_cid = ctx;
+
+		ctx.clear();
+		DBRes.GetField("plus_occur_time", ctx, Error);
+		site_ptr->m_coverage[1].m_time = tool_time::to_time(ctx);
+
+		log_info("site_position: site=%d, x=%.2lf, y=%.2lf, area_id=%d, m_scale=%.2f, pdoa_offset=%.2f, pdoa_direction=%d, special=%d, device_type_id=%d, plus_dist=%.2f, minus_dist=%.2f", reader_id, x, y, area_id, scale, offset, direction, special, device_type_id, site_ptr->m_coverage[0].m_distance, site_ptr->m_coverage[1].m_distance);
+	}
+}

ant.h

@@ -382,5 +382,25 @@ struct sit_list:single_base<sit_list,int,std::shared_ptr<site>>
     void read_ant_path(int id=-1);
     void init_site(const std::string &ids="");
 };
+
+struct sit_list_v :single_base<sit_list_v, int, std::shared_ptr<site>>
+{
+	void load(const std::string &id)
+	{
+		int sid = -1;
+		if (!id.empty())sid = std::stoi(id);
+		read_sit_list(sid);
+		read_ant_path(sid);
+	}
+	///id=-1为初始化所有
+	void load_from_db(const std::string&ids = "")
+	{
+		init_site(ids);
+		load(ids);
+	}
+	void read_sit_list(int id = -1);
+	void read_ant_path(int id = -1);
+	void init_site(const std::string &ids = "");
+};
 #endif
 

card_base.cpp

@@ -134,7 +134,7 @@ void card_location_base::make_his_location(uint64_t t,const point & pt,bool bclo
 }
 
 //坐标点输入业务入口
-void card_location_base::on_location(const std::vector<point>&vp,const std::vector<loc_message> &lm )
+void card_location_base::on_location(const std::vector<point>&vp, const std::vector<loc_message> &lm, bool is_v_map)
 {
     loc_point pt = m_sel_tool->select_solution(vp, lm);
     //pt.y = pt.y;
@@ -145,8 +145,18 @@ void card_location_base::on_location(const std::vector<point>&vp,const std::vect
         sid = site_ptr->m_id;
 	if(pt.m_useless)
 	{
-		x = tool_other::round(pt.x,3);
-		y = tool_other::round(pt.y,3);
+		if (!is_v_map)
+		{
+			x = tool_other::round(pt.x, 3);
+			y = tool_other::round(pt.y, 3);
+		}
+		else
+		{
+			m_v_point.x = tool_other::round(pt.x, 3);
+			m_v_point.y = tool_other::round(pt.y, 3);
+			log_info("useful loc m_v_point: type=%d, card=%d, site=%d, ct=%d, timestamp=%llu, x=%f, y=%f, speed=%.2f, acc=%.2f", m_type, m_id, sid, m_ct, m_time, m_v_point.x, m_v_point.y, m_speed, lm[0].m_acc);
+			return;
+		}
 
 		double acc = lm[0].m_acc;
         m_acc = lm[0].m_acc;

card_base.h

@@ -48,6 +48,7 @@ struct card:point
         ,m_freq(0.0)
         ,m_acc(0)
 		,m_speed(0)
+		,m_v_point(0, 0, 0)
 	{}
 	uint64_t type_(){return m_type;}
 	uint64_t time_(){return m_time;}
@@ -69,6 +70,7 @@ struct card:point
     ///人卡(加速度状态)，其值就是0和1，掘进机和采煤机，其值就是0~255，车辆，其值就需要乘以0.01，用于表示加速度值
     double   m_acc;
 	double   m_speed;			//速度
+	point	m_v_point;			//车辆定位系统的定位结果
 };
 
 struct card_location_base:card,std::enable_shared_from_this<card_location_base>
@@ -145,7 +147,7 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
     void on_message(zloop<task*>* loop, message_tdoa_locinfo& loc, bool is_history);
     void on_message(zloop<task*>* loop, message_pdoa_locinfo& loc, bool is_history);
 
-    void on_location(const std::vector<point>&vp,const std::vector<loc_message> &lm );
+	void on_location(const std::vector<point>&vp, const std::vector<loc_message> &lm, bool is_v_map = false);
     void do_status(int st);
 	void upt_card_pos(sys::_CARD_POS_&cp, point &pt);
 	void del_card_pos();

main.cpp

@@ -84,6 +84,7 @@ struct Init_Setting
         //bulletin_broad_show::inst()->OnInit(&config);
 
         sit_list::instance()->load_from_db();
+		sit_list_v::instance()->load_from_db();
         card_list::instance()->init_card_from_db();
         area_list::instance()->init_from_db();
         area_list::instance()->init_area_persons_dynamic_thre_from_db();

net-service.cpp

@@ -223,6 +223,10 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 					module_device_net::instance()->do_business(clt->name(), site_ptr->m_id, site_ptr->m_device_type_id);
 
                     site_ptr->set_algo(LDT_PDOA);
+
+					const auto& site_ptr_v = sit_list_v::instance()->get(static_cast<int32_t>(site_id));
+					site_ptr_v->set_algo(LDT_PDOA);
+
                     //log_info("[scale_test] %.2f", site_ptr->m_scale);
                     struct timeval tv;
                     gettimeofday(&tv, NULL);