tof 1d version

Makefile.am

@@ -16,7 +16,8 @@ SRC_MODULE_SERVICE= module_service/area_business_car_attendance.cpp module_servi
 					module_service/module_call.cpp module_service/module_call_help.cpp module_service/module_mgr.cpp \
 					module_service/module_other_alarm.cpp \
 					module_service/module_traffic_light.cpp module_service/module_traffic_light_manager.cpp \
-					module_service/module_traffic_light_rule.cpp module_service/module_screen.cpp
+					module_service/module_traffic_light_rule.cpp module_service/module_screen.cpp \
+					module_service/module_device_net.cpp
 
 SRC_SYNCTIME_MODULE= sync_time/sync_helper.cpp sync_time/sync_manager.cpp
 
@@ -36,7 +37,7 @@ AM_LDFLAGS=-Wall -pthread -m64 -g -std=c++11  -L${prefix}/lib  -L/usr/local/lib
 
 3xls_SOURCES=${AM_SOURCES} main.cpp
 3xls_CPPFLAGS=${AM_CPPFLAGS}
-3xls_LDFLAGS=${AM_LDFLAGS}  -lev -lzlog -lrt -lboost_chrono -lboost_system -lboost_thread -lmysqlclient -lthree_rates
+3xls_LDFLAGS=${AM_LDFLAGS}  -lev -lzlog -lrt -lboost_chrono -lboost_system -lboost_thread -lmysqlclient
 
 3xls_LDADD=db/libyadb.a websocket/libwebsocket.a ${prefix}/lib/libpoint_algorithm.a
 

card.cpp

@@ -383,14 +383,14 @@ void card_list::on_message(zloop<task*> *loop, message_locinfo&loc,bool is_histo
 		return;
 	}
 	double dist_tof = loc.m_tof*15.65*2.996*1e-4;
-	logn_info(3,"card_message:site=%d,ant=%d,type=%lu,card=%d,ct=%d,tof=%lld(%.2lfm),rav=%02X,acc=%02X,rssi=%d,bat=%02X,stamp=%llu",
-			loc.m_site_id,loc.m_ant_id,loc.m_card_type,loc.m_card_id,loc.m_card_ct,loc.m_tof,dist_tof,
+	logn_info(3,"[card_message] site=%d, ant=%d, type=%d, card=%d, ct=%d, tof=%lld(%.2lfm), rav=%02X, acc=%02X, rssi=%d, bat=%02X, time=%llu",
+			loc.m_site_id, loc.m_ant_id, loc.m_card_type,loc.m_card_id,loc.m_card_ct,loc.m_tof,dist_tof,
 			(loc.m_card_type==1?loc.m_callinfo:loc.m_rav),
 			loc.m_acc,loc.m_rssi,loc.m_batty_status,loc.m_time_stamp);
     if(loc.m_card_type != CT_PERSON && CYaSetting::m_sys_setting.check_rav(loc.m_card_type,loc.m_card_id))
         loc.set_rav(0);
     
-	c->on_message(loop,loc,is_history);
+	c->on_message(loop, loc, is_history);
 
 	if(tool_other::is_driving(loc.m_card_type,c->get_vehicle_type_id()))
         mine_business::inst()->make_reverse_condition(loc.m_card_type,loc.m_card_id,loc.m_ant_id,loc.m_card_ct,loc.m_tof,loc.m_site_id);

card_base.cpp

@@ -8,7 +8,6 @@
 #include "module_service/module_mgr.h"
 #include "websocket/wsClientMgr.h"
 #include "websocket/wsTimerThread.h"
-//#include "three_rates.h"
 #include "his_location.h"
 #include "event.h"
 #include "module_service/module_call.h"
@@ -52,7 +51,7 @@ void card_location_base::do_status(int st)
 	time_t now = time(0);
 	//bool   help_flag = false;
 
-	log_info("[help-battery] card_id=%d, help_bit=%d, status=%d", m_id, m_help_bit, st);
+	//log_info("[help-battery] card_id=%d, help_bit=%d, status=%d", m_id, m_help_bit, st);
 	// 呼救
 	if((m_help_bit & 1) && (st & STATUS_HELP))
 	{
@@ -96,9 +95,9 @@ void card_location_base::do_status(int st)
 	}
 
 	/*if(!help_flag)
-	{
-		st = st & (0xFFFFFFFF ^ STATUS_HELP);
-	}*/
+	  {
+	  st = st & (0xFFFFFFFF ^ STATUS_HELP);
+	  }*/
 
 	if((STATUS_POWER_LOWER_SERIOUS & st) != 0)
 	{
@@ -109,102 +108,122 @@ void card_location_base::do_status(int st)
 		m_pwr_stat = 0;
 	}
 
-    module_mgr::do_status((STATUS_CARD)st, m_id, m_type);
+	module_mgr::do_status((STATUS_CARD)st, m_id, m_type);
 }
 
 //写入历史轨迹
 void card_location_base::make_his_location(uint64_t t,const point & pt,bool bclose /*= false*/)
 {
 	int area_id = 0, map_id = 0, site_id = 0;
-    double scale = 0.0;
+	double scale = 0.0;
 	if(auto site_ptr = get_area_tool()->m_site)
 	{
 		area_id = site_ptr->m_area_id;
 		map_id  = site_ptr->m_map_id;
 		site_id = site_ptr->m_id;
-        scale   = site_ptr->m_scale;
+		scale   = site_ptr->m_scale;
 	}
- 
+
 	auto mine_ptr = get_mine_tool();
 	if(m_time < mine_ptr->get_down_time())
 	{
 		return;
 	}
 	m_his_location_card->push(t, pt, area_id, map_id, site_id, bclose);
-    m_his_location_card->insert(t, pt, area_id, map_id, site_id, scale);
+	m_his_location_card->insert(t, pt, area_id, map_id, site_id, scale);
 }
 
 //坐标点输入业务入口
-void card_location_base::on_location(const std::vector<point>&vp, const std::vector<loc_message> &lm, bool is_v_map)
+void card_location_base::on_location(const std::vector<point>&vp, const std::vector<loc_message> &lm)
 {
-	m_isVehicleMap = false;
-	if (is_vehicle())
-	{
-		if (!is_v_map)
-		{
-			return;
-		}
-	}
-	if (is_person())
-	{
-		if (is_v_map)
-		{
-			m_isVehicleMap = true;
-		}
+	/*std::for_each(vp.begin(), vp.end(), [](point it){
+			logn_info(3, "possible solution: x = %.2f, y = %.2f", it.x, it.y);
+			});*/
+	if(lm.size() < 2){
+		return;
 	}
-
-    loc_point pt = m_sel_tool->select_solution(vp, lm);
-    //pt.y = pt.y;
-    
+	loc_point pt = m_sel_tool->select_solution(vp, lm);
+	//logn_info(3, "select solution: x = %.2f, y = %.2f", pt.x, pt.y);
 	auto site_ptr = get_area_tool()->m_site;
 	int sid = 0;
-	if(site_ptr)
+	// 天线间间距默认为3米，实际取基站上的天线间距离配置参数
+	double ant_dist = 3.0;
+	if(site_ptr){
         sid = site_ptr->m_id;
+		ant_dist = site_ptr->m_pdoa_offset;
+	}
+
+	// first way
+	sys::tof_data tdt;
+	tdt.cid = tool_byte::to_cardid(m_type, m_id);
+	tdt.sid = 0;
+	tdt.cur_time = tool_time::to_str_ex(tool_time::now_to_ms());
+	int aid = lm[0].m_ant_id;
+	tdt.ant_dist[aid] = lm[0].m_num_ticks*15.65*2.996*1E-4;
+	aid = lm[1].m_ant_id;
+	tdt.ant_dist[aid] = lm[1].m_num_ticks*15.65*2.996*1E-4;
+
+	if(tdt.ant_dist[0] > 1000 || tdt.ant_dist[1] > 1000){
+		int a = 11;
+		pt.x = a;
+	}
+
+	logn_info(3, "[tof_data] cid=%d, ct=%d, 1ant_dist=%.2f, 2ant_dist=%.2f", m_id, m_ct, tdt.ant_dist[0], tdt.ant_dist[1]);
+
+	loc_point tp;
+	if(tdt.ant_dist[0] > tdt.ant_dist[1] && tdt.ant_dist[0] > ant_dist){
+		// 在0天线侧外部
+		tp.y = -1.5 + tdt.ant_dist[0];
+	}else if(tdt.ant_dist[1] > tdt.ant_dist[0] && tdt.ant_dist[1] > ant_dist){
+		// 在1天线测外部
+		tp.y = 1.5 - tdt.ant_dist[1];
+	}else if(tdt.ant_dist[0] < 3.0 && tdt.ant_dist[1] < ant_dist) {
+		// 在两天线之间
+		tp.y = (-1.5 + tdt.ant_dist[0]) + (1.5 - tdt.ant_dist[1])/2.0;
+	}
+
+	logn_info(3, "another solution:cid =%d, ct=%d, x = %.2f, y = %.2f", m_id, m_ct, tp.x, tp.y);
+	tdt.y = tp.y;
+	pt = tp;
+	pt.m_useless = true;
+
 	if(pt.m_useless)
 	{
-		if (is_person())
-		{
-			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;
-			}
-		}
-		else if (is_vehicle())
-		{
-			if (is_v_map)
-			{
-				x = tool_other::round(pt.x, 3);
-				y = tool_other::round(pt.y, 3);
-			}
-			else
-			{
-				return;
-			}
-		}
-
-		double acc = lm[0].m_acc;
-        m_acc = lm[0].m_acc;
-		log_info("useful loc 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,x,y, m_speed, acc);
-		do_business(lm.front().m_sit, pt, acc);
+		x = tool_other::round(pt.x, 3);
+		y = tool_other::round(pt.y, 3);
 		
+		do_business(lm.front().m_sit, pt);
+
+		sys::tof_data td;
+		td.cid = tool_byte::to_cardid(m_type, m_id);
+		td.y = y;
+		td.sid = sid;
+		td.cur_time = tool_time::to_str_ex(tool_time::now_to_ms());
+		int aid = lm[0].m_ant_id;
+		td.ant_dist[aid] = lm[0].m_num_ticks*15.65*2.996*1E-4;
+		aid = lm[1].m_ant_id;
+		td.ant_dist[aid] = lm[1].m_num_ticks*15.65*2.996*1E-4;
+		td.ant_diff = site_ptr->m_pdoa_offset;
+
+		// 将数据存入数据库中
+		char sql[1024] = {0};
+		snprintf(sql, 1024, "insert into his_tof_locate_data(card_id, ant_one_dist, ant_two_dist, ant_dist,site_id, y, cur_time) values('%s', %.2f, %.2f, %.2f, %d, %.2f, '%s')", tool_byte::to_cardid(m_type, m_id).c_str(), td.ant_dist[0], td.ant_dist[1], site_ptr->m_pdoa_offset, sid, y, tool_time::to_str_ex(tool_time::now_to_ms()).c_str());
+		db_tool::PushAsync(sql);
+        log_info("[sql] %s", sql);
+
+		//add json to send to web
+		swsTimerThrd.upt_tof_data(std::move(td));
+
 		// 呼救128,正常0
-		int val = (lm[0].m_rav?STATUS_HELP:STATUS_NORMAL);
+		/*int val = (lm[0].m_rav?STATUS_HELP:STATUS_NORMAL);
 		if(m_battery_value > 3){
 			val += STATUS_POWER_NORMAL;
 		}else{
 			val += STATUS_POWER_LOWER_SERIOUS;
-		}
+		}*/
 
-		log_info("[help-battery] card_id=%d, battery=%d, val=%d", m_id, m_battery_value, val);
-		do_status(val);
+		//log_info("[help-battery] card_id=%d, battery=%d, val=%d", m_id, m_battery_value, val);
+		//do_status(STATUS_NORMAL);
     }
 	else
 	{
@@ -245,6 +264,24 @@ void card_location_base::on_message(zloop<task*>* loop, message_locinfo& loc,boo
 	}
 }
 
+/*
+ * 实心球原始数据调用算法模块进行定位处理
+ * */
+void card_location_base::on_message(zloop<task*>* loop, message_ins_locinfo& loc,bool is_history)
+{
+	log_info("[solid_ball] start calc location");
+	m_ct = loc.m_card_ct;
+	m_time = loc.m_time_stamp;
+	auto site_ptr = sit_list::instance()->get(loc.m_site_id);
+	if(!site_ptr)
+	{
+		log_warn("接收到分站的数据，site=%d,card=%d,ct=%d,但是分站未定义",loc.m_site_id,m_id,loc.m_card_ct);
+		return;
+	}
+
+	m_message_handle->on_message(loop, loc, is_history);
+}
+
 /*
  * tdoa 调用算法库定位
  * */
@@ -414,31 +451,6 @@ void  card_location_base::clear()
     m_cb_point.clear();
 }
 
-/*void card_location_base::put_three_rates(card_pos & cp)
-{   
-	if(!three_rates_flag)
-        return;
-	
-    cp.rec_time     = m_time;
-    cp.type         = m_type;
-    cp.id           = m_id;
-	cp.identifier_id= m_cid;
-    cp.running_stat = m_stat;
-    cp.final_v      = m_speed;
-	cp.dpt_id       = m_deptid;
-	std::shared_ptr<area_tool> _areatool = get_area_tool();
-	if(nullptr != _areatool && nullptr != _areatool->m_site)
-	{
-		cp.reader_x = _areatool->m_site->x;
-		cp.reader_y = _areatool->m_site->y;
-		cp.reader_id = _areatool->m_site->m_id;
-	}
-
-	log_info("three_rates:type:%d,id:%d,cid:%d",cp.type,cp.id,cp.identifier_id);
-	three_rates::get_instance()->put(cp);
-	
-}*/
-
 void card_location_base::put_traffic_light(card_pos& cp)
 {
     if(!traffic_light_flag){
@@ -606,4 +618,5 @@ void card_location_base::do_site_locate_event(const message_pdoa_locinfo& loc)
 	
 	event_tool::instance()->handle_event(OT_DEVICE_READER, ET_READER_LONG_TIME_NO_POSITION, loc.m_site_id, 3, dt, (dt>=3));
 	log_info("[site-locate-event] site_id=%d, card_id=%d, ct=%d, dt=%d", loc.m_site_id, m_id, loc.m_card_ct, dt);
-}	
+}
+

card_base.h

@@ -128,7 +128,7 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
 	void inc_upmine_flag(int flag){m_upmine_flag=flag;}
     int upmine_flag(){return m_upmine_flag.load();}
 
-	virtual void do_business(const std::shared_ptr<site>&site,const point &pt,double acc)=0;
+	virtual void do_business(const std::shared_ptr<site>&site,const point &pt)=0;
 	virtual void on_timer()=0;
     virtual std::shared_ptr<mine_tool> get_mine_tool()=0;
     virtual void clear();
@@ -148,12 +148,11 @@ 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, bool is_v_map = false);
+	void on_location(const std::vector<point>&vp, const std::vector<loc_message> &lm);
     void do_status(int st);
 	void upt_card_pos(sys::_CARD_POS_&cp, point &pt);
 	void del_card_pos();
 	int	 get_stat();
-	//void put_three_rates(card_pos &);
     void put_traffic_light(card_pos& cp);
     void set_event_flag(EVENT_TYPE et,std::uint8_t f=1){m_event[et]=f;}
     bool get_event_flag(EVENT_TYPE et){return m_event[et];}

card_car.cpp

@@ -4,7 +4,6 @@
 #include "his_location.h"
 #include "area.h"
 #include "mine.h"
-#include "three_rates.h"
 #include "select_tool.h"
 #include "websocket/ws_common.h"
 #include "special_area.h"
@@ -15,6 +14,7 @@
 #include "module_service/module_call.h"
 #include "sys_setting.h"
 #include "event.h"
+#include "struct_def.h"
 
 car::car(const std::string&type,uint32_t cardid,uint16_t needdisplay,int16_t t,int32_t deptid,
 		int32_t categoryid, int type_id,int32_t level_id,uint32_t cid)
@@ -37,13 +37,11 @@ void car::set_area_info(int mapid,double scale,int areaid,uint64_t t,int type)
 	m_area_tool->set_area_info(mapid, scale, areaid, *this, t, type);
 }
 
-void car::do_business(const std::shared_ptr<site>&site,const point &pt,double acc)
+void car::do_business(const std::shared_ptr<site>&site,const point &pt)
 {
-	m_acc=acc;
 	m_area_tool->on_point(shared_from_this(),pt);
 	m_timeval = m_time;
-	//handle_three_rates(pt);
-    handle_traffic_light(pt, site->m_id);
+    //handle_traffic_light(pt, site->m_id);
     //if(m_enable_anti_collision){
     if(CYaSetting::m_sys_setting.m_enable_anti_coll){
         handle_anti_coll(pt, site->m_id);
@@ -162,27 +160,6 @@ void car::handle_anti_coll(const point& pt, const int& sid)
     //module_call::instance()->send_anti_collision(cards);
 }
 
-/*
- * @brief       三率业务处理模块
- * @param       const point& pt     定位数据
- * @return      无
- * @note
- * @warning
- * @bug
- * */
-void car::handle_three_rates(const point &pt)
-{
-	card_pos cp;
-	m_biz_stat = get_stat();
-	cp.biz_stat = m_biz_stat;
-	cp.x = pt.x;
-    cp.y = pt.y;
-    cp.z = pt.z;	
-	cp.map_id = m_area_tool->get_mapid();
-	cp.vibration = m_acc;
-	//put_three_rates(cp);
-}
-
 /*
  * @brief       红绿灯业务模块
  * @param       const point& p      定位数据

card_car.h

@@ -24,7 +24,7 @@ public:
 	~car();
 
     virtual std::shared_ptr<mine_tool> get_mine_tool();
-	virtual void do_business(const std::shared_ptr<site>&site,const point &pt,double acc);
+	virtual void do_business(const std::shared_ptr<site>&site,const point &pt);
 	virtual int get_area();
     int get_vehicle_type_id();
     virtual std::shared_ptr<area_tool> get_area_tool()
@@ -40,7 +40,6 @@ public:
 	virtual void set_area_info(int mapid,double scale,int areaid,uint64_t t,int type);
 	virtual void get_card(bool);
 private:
-	void handle_three_rates(const point &pt);
     void handle_traffic_light(const point& pt, const int& sid);
     void handle_anti_coll(const point& pt, const int& sid);
 	void on_timer();

card_person.cpp

@@ -7,7 +7,6 @@
 #include "mine.h"
 #include "his_location.h"
 #include "tool_time.h"
-#include "three_rates.h"
 #include "select_tool.h"
 #include "monkey_car/monkeycar_person.h"
 #include "websocket/ws_common.h"
@@ -150,7 +149,7 @@ std::shared_ptr<mine_tool> person::get_mine_tool()
  * @bug
  * @warning
  * */
-void person::do_business(const std::shared_ptr<site>&site,const point &pt,double acc)
+void person::do_business(const std::shared_ptr<site>&site,const point &pt)
 { 
     //区域相关逻辑驱动
 	m_area_tool->on_point(shared_from_this(), pt);
@@ -159,8 +158,6 @@ void person::do_business(const std::shared_ptr<site>&site,const point &pt,double
     make_his_location(m_time, pt);
 	m_timeval = m_time;
 	
-	//三率数据处理
-	//handle_three_rates(pt);
     //一人多卡数据输入
     if(m_person_cards_flag){
 	    Msg m;
@@ -179,28 +176,6 @@ void person::reset(std::shared_ptr<monkey_person> mp)
 	m_monkeyPerson = mp;
 }
 
-/*
- * @brief   处理三率模块
- * @param   const point& pt
- * @return  无
- * @note
- * @bug
- * @warning
- * */
-void person::handle_three_rates(const point & pt)
-{
-	card_pos cp;
-	cp.work_line    = m_workLine;
-	cp.work_type_id = m_worktype_id;
-	m_biz_stat      = get_stat();
-	cp.biz_stat     = m_biz_stat;
-	cp.x = pt.x;
-    cp.y = pt.y;
-    cp.z = pt.z;
-	cp.work_type_id = m_worktype_id;
-	//put_three_rates(cp);
-}
-
 //定时推送到web函数，
 void person::on_timer()
 {

card_person.h

@@ -26,7 +26,6 @@ struct person:card_location_base, card_area
 
     void clear();
     void reset(std::shared_ptr<monkey_person> mp);
-    void handle_three_rates(const point & pt);
     void on_timer();
     void IKSDK_DB(int);
     point getSmoothPoint(uint64_t& t);
@@ -35,7 +34,7 @@ struct person:card_location_base, card_area
     virtual void site_hover(int sid);
 	virtual void get_card(bool f);
     virtual std::shared_ptr<mine_tool> get_mine_tool();
-    virtual void do_business(const std::shared_ptr<site>&site,const point &pt,double acc);
+    virtual void do_business(const std::shared_ptr<site>&site,const point &pt);
 	virtual void set_area_info(int mapid,double scale,int areaid,uint64_t t,int type);
     virtual void handle_message(uint16_t ct,uint8_t &value);
     

db/db_api/CDBCommon.h

@@ -99,7 +99,8 @@ namespace YADB
 		}
 		~_ASYNC_SQL_()
 		{
-
+			FailedCount = 0;
+			SQL = "";
 		}
 	};
 }

main.cpp

@@ -15,7 +15,6 @@
 #include "event.h"
 #include "module_service/module_mgr.h"
 #include <config_file.h>
-#include "three_rates.h"
 #include "mine_business.h"
 #include "sys_setting.h"
 #include "websocket/web_connect.h"
@@ -95,18 +94,6 @@ struct Init_Setting
 		mine_business::inst()->load();
         card_list::instance()->load_his_card_postion_from_db();
         event_list::instance()->load_his_data_from_db(true);
-        if(three_rates_flag){
-            db_para dp;	
-            dp.Host     = DBSetting.Host;
-            dp.User     = DBSetting.User;
-            dp.PWD      = DBSetting.PWD ;
-            dp.DBName   = DBSetting.DBName;
-            dp.CharSet  = DBSetting.CharSet ;
-            dp.TimeOut  = DBSetting.TimeOut;
-            dp.PoolSize = DBSetting.PoolSize;
-		    init_three_rates(dp);
-        }
-
         if(traffic_light_flag){
             init_traffic_light();
         }
@@ -117,18 +104,6 @@ struct Init_Setting
         log_info("Init_Setting::init  Success. \n" );
     }
 
-	void init_three_rates(const db_para& dbs)
-	{
-		/*log_info("init three_rates ...................");
-		init_para ip;
-		ip.send_pt = handlereader;
-		ip.driving_face_alarm = Handle_ThreeRates_Event_Callback;
-		three_rates::get_instance()->init(ip,dbs);
-		three_rates::get_instance()->start();
-        std_info("加载三率模块成功");
-		*/
-	}
-
     void init_traffic_light()
     {
         log_info("init traffic_light ...................");

message.cpp

@@ -100,8 +100,8 @@ void message_locinfo::load(zistream&is,bool tdoa)
 	uint16_t sp1=0,sp2=0;
 	is>>sp1>>sp2;
 	m_rssi=10*log10(1.*sp1*(1<<17)/pow(sp2-64.,2))-121.74;
-	log_info("timestamp=%llu,type=%d,card=%d,site=%d,ct=%d,bat=%#X,acc=%d,tof=%llu,ant_id=%d,spq=%d",
-			m_time_stamp,m_card_type,m_card_id,m_site_id,m_card_ct,m_batty_status,m_acc,m_tof,m_ant_id,m_rssi);
+	logn_info(3, "[tof] timestamp=%llu, type=%d, card=%d, site=%d, ct=%d, bat=%#X, acc=%d, tof=%llu, ant_id=%d, spq=%d",
+			m_time_stamp, m_card_type, m_card_id, m_site_id, m_card_ct, m_batty_status, m_acc, m_tof, m_ant_id, m_rssi);
 }
 
 //优化协议数据解析
@@ -396,3 +396,71 @@ void message_pdoa_locinfo::load(zistream& is)
 		logn_info(3,"[pdoa] card_type=%d, card_id=%d, ct=%d, status=%d, call_type=%d, rav=%d, acc=%d, tof=%llu, ant_id=%d, rssi=%d, poa1=%.2f, poa2=%.2f, poa3=%.2f", m_card_type, m_card_id, m_card_ct, m_batty_status, m_callinfo, m_rav, m_acc, m_tof, m_ant_id, m_rssi, m_poa[0], m_poa[1], m_poa[2]);
 	}
 }
+
+// 数据重置
+void message_ins_locinfo::zero_this()
+{
+    m_time_stamp =
+    m_site_time = 
+    m_tof =
+    m_site_id = 
+    m_card_type = 
+    m_card_id = 
+    m_card_ct = 
+    m_batty_status = 
+    m_callinfo =
+    m_rav = 	
+    m_acc = 
+    m_ant_id =
+    m_sync_ct =
+	m_loc_type =	
+    m_biz_type =
+	m_rssi = 0; 
+    m_distance = 0.0;
+	m_ins_data.clear();
+}
+
+
+void message_ins_locinfo::load(zistream& is)
+{
+	zero_this();
+
+	uint8_t bt = 0;
+	is >> bt;
+	
+	// 卡类型，1字节
+	m_card_type = bt;
+
+	uint16_t id = 0;
+	// 卡号 2字节
+	// 卡ct 2字节
+	is >> id >> m_card_ct;
+	m_card_id = id;
+
+	// 报文+电池，1字节
+	// 预留，2字节
+	is >> bt >> id;	
+
+	uint32_t tof = 0;
+	is >> bt >> tof;
+	m_tof = bt;
+	m_tof = (bt << 32) | tof;
+
+	// 天线号，1字节
+	// 信号接收功率，1字节
+	is >> m_ant_id >> m_rssi;
+	--m_ant_id;
+
+	// 加速度三轴，x,y,z
+	for(size_t i = 0;i < 3; ++i){
+		is >> id;
+		m_ins_data.m_acc_data[i] = id;
+	}
+
+	// 角速度三轴，x,y,z
+	for(size_t i = 0;i < 3; ++i){
+		is >> id;
+		m_ins_data.m_ang_data[i] = id;
+	}
+}
+

message.h

@@ -4,9 +4,94 @@
 #include <time.h>
 #include "worker.h"
 #include "protocol.h"
+#include <cmath>
 
 struct zistream;
 
+// 惯导数据，含加速度和角速度
+struct ins_data{
+	double m_acc;
+	double m_ang;
+
+	// 0,1,2分别对应加速度的X轴，Y轴，Z轴
+	std::array<uint16_t, 3> m_acc_data;
+	std::array<uint16_t, 3> m_ang_data;
+
+	ins_data()
+		: m_acc(0)
+		, m_ang(0)
+	{
+		m_acc_data.fill(0);
+		m_ang_data.fill(0);
+	}
+
+	double get_acc()
+	{
+		return (m_acc = sqrt(pow(get_acc(0), 2) + pow(get_acc(1), 2) + pow(get_acc(2), 2)));
+	}
+
+	double get_acc(int i)
+	{
+		return calc_acc(m_acc_data[i]);
+	}
+
+	double get_gyro()
+	{
+		return (m_ang = sqrt(pow(get_gyro(0), 2) + pow(get_gyro(1), 2) + pow(get_gyro(2), 2)));
+	}
+
+	double get_gyro(int i)
+	{
+		return calc_gyro(m_ang_data[i]);
+	}
+
+	double calc_acc(const uint16_t val)
+	{
+		int acc_fs = 16;
+		double ret = 0.0;
+		uint8_t H = 0, L = 0;
+		H = val >> 8;
+		L = val & 0xFF;
+
+		if(H > 0x80){
+			H = ~H;
+			L = (~L) + 0x01;
+			ret = (-(((H << 8) | L) * 0.061 * (acc_fs / 2) / 1000));
+		}else{
+			ret = ((((H << 8) | L) * 0.061 * (acc_fs / 2) / 1000));
+		}
+
+		return ret;
+	}
+
+	double calc_gyro(const uint16_t val)
+	{
+		double ret = 0.0;
+		uint8_t H = 0, L = 0;
+		H = val >> 8;
+		L = val & 0xFF;
+
+		if(H > 0X80){
+			H = ~H;
+			L = (~L) + 0X01;
+
+			ret = (-(((H << 8) | L) / 1000.0);
+		}else{
+			ret = ((((H << 8) | L) / 1000.0);
+		}
+
+		return ret;
+	}
+
+	void reset()
+	{
+		m_acc = m_ang = 0;
+		m_acc_data.fill(0);
+		m_ang_data.fill(0);
+	}
+};
+
+
 /*
  * tdoa协议之分站数据
  * */
@@ -165,6 +250,14 @@ struct message_pdoa_locinfo: public message_locinfo{
     void load(zistream& is);
 };
 
+struct message_ins_locinfo: public message_locinfo{
+	ins_data m_ins_data;
+	uint8_t m_biz_type;
+
+	void zero_this();
+	void load(zistream& is);	
+}
+
 struct message_light: task{
     uint32_t m_light_id;
     uint8_t  m_type;

net-service.cpp

@@ -114,7 +114,7 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
                         break;
                     }
 
-					check_message_time(clt,site_ptr,&site_tm,site_ct,power);
+					//check_message_time(clt,site_ptr,&site_tm,site_ct,power);
                     site_ptr->m_algo = LDT_TOF;
 
                     if(clt->type()!=2){
@@ -202,6 +202,48 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 						}
 					}
 				}
+				break;
+			case CHAR_LOCATEDATA_TOF_INS:
+				// 实心球项目中的硬件数据协议：带加速度和角速度6轴数据
+				int32_t site_id = parse_data_anchor_opt(clt, is);
+
+				if(site_id < 0){
+					break;
+				}
+
+				const auto& site_ptr = sit_list::instance()->get(static_cast<int32_t>(site_id));
+				if(!site_ptr){
+					log_error("在全局分站列表中找不到分站：%d", site_id);
+					break;
+				}
+
+				if(clt->type() != 2){
+					site_ptr->set_client(clt);
+				}
+
+				module_device_net::instance()->do_business(clt->name(), site_ptr->m_id, site_ptr->m_device_type_id);
+				site_ptr->set_algo(LDT_TOF);
+
+				struct timeval tv;
+				gettimeofday(&tv, NULL);
+				uint64_t cur_time = tv.tv_sec*1000 + tv.tv_usec/1000;
+				int index = 0;
+				while(!is.eof()){
+					uint64_t tstamp = cur_time - 950 + index*45;
+					++index;
+
+					task* t = task::alloc<message_ins_locinfo>();
+					message_ins_locinfo& m = t->body<message_ins_locinfo>();
+					m.load(is);
+					m.m_site_id = site_id;
+					m.m_time_stamp = tstamp;
+					m.m_loc_type = LDT_TOF;
+					t->m_cmd_code = cmd;
+					t->m_hash_id = m.m_card_id;
+					logn_info(3, "");
+					m_loc_worker->request(t);
+				}
+
 				break;
             case CHAR_LOCATEDATA_PDOA:
                 {

protocol.h

@@ -14,6 +14,7 @@
 #define CHAR_LOCATEDATASMALL_TOF_EXTEND     0x753b	// TOF实时小分站定位数据，请求、应答
 #define CHAR_LOCATEDATAHIS_TOF_EXTEND       0x853b	// TOF历史定位数据，请求、应答
 #define CHAR_LOCATEDATA_TOF_OPTIMIZE        0x753d  // TOF优化协议
+#define CHAR_LOCATEDATA_TOF_INS				0x901a	// TOF惯导数据协议
 
 // 2.tdoa
 #define CHAR_LOCATEDATA_TDOA_EXTEND         0x863b	// 扩展TDOA实时定位数据，请求、应答

websocket/jsonBuilder.cpp

@@ -5,6 +5,7 @@
 #include <rapidjson/prettywriter.h>
 #include "constdef.h"
 #include "log.h"
+#include <cmath>
 
 namespace sys
 {
@@ -877,6 +878,72 @@ namespace sys
         root.Accept(writer);
         return sb.GetString();
     }
+
+	/*
+	 * 构建tof_data的json数据
+	 * */
+	std::string jsonBuilder::build_tof_data(const std::vector<tof_data>& vtd)
+	{
+		if(vtd.size() == 0){
+			return "";
+		}
+		rapidjson::StringBuffer sb;
+		rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+		rapidjson::Document doc;
+		rapidjson::Document::AllocatorType& allocator = doc.GetAllocator();
+
+		rapidjson::Value root(rapidjson::kObjectType);
+		__SetCmd("tof_data", root, allocator);
+		__AddVersion(root, allocator);
+
+		rapidjson::Value data(rapidjson::kArrayType);
+		for(const auto& it : vtd){
+			rapidjson::Value elem(rapidjson::kArrayType);
+			rapidjson::Value value(rapidjson::kObjectType);
+
+			log_info("[error] cid = %s, length = %d", it.cid.c_str(), it.cid.length());
+
+			if(it.cid.length()> 1000){
+				int a = it.cid.length();
+			}
+
+			// 卡号
+			value.SetString(it.cid.c_str(), it.cid.length(), allocator);
+			elem.PushBack(value, allocator);
+
+			// 天线1间测距距离
+			value.SetDouble(it.ant_dist[0]);
+			elem.PushBack(value, allocator);
+
+			// 天线2的测距距离
+			value.SetDouble(it.ant_dist[1]);
+			elem.PushBack(value, allocator);
+
+			// 天线12间距离
+			value.SetDouble(it.ant_diff);
+			elem.PushBack(value, allocator);
+
+			// 基站号
+			value.SetInt(it.sid);
+			elem.PushBack(value, allocator);
+
+			// 一维位置
+			value.SetDouble(it.y);
+			elem.PushBack(value, allocator);
+
+			// 接收时间
+			value.SetString(it.cur_time.c_str(), it.cur_time.length(), allocator);
+			elem.PushBack(value, allocator);
+
+			data.PushBack(elem, allocator);
+
+		}
+		root.AddMember("data", data,  allocator);
+
+		root.Accept(writer);
+
+		return sb.GetString();
+	}
 }
 
 

websocket/jsonBuilder.h

@@ -246,6 +246,7 @@ namespace sys
         std::string build_tmp_card_pos(const std::map<uint64_t, _CARD_POS_>& cards);
         std::string build_traffic_light(const std::vector<light_state> lights);
 		std::string build_device_state(const std::vector<device_state> vtd);
+		std::string build_tof_data(const std::vector<tof_data>& vtd);
 		/**
 		* @brief
 		生成车辆进入特殊区域jason函数。

websocket/wsTimerThread.cpp

@@ -151,13 +151,38 @@ namespace sys
 		device_state_list.resize(0);
 	}
 
+	void wsTimerThread::send_tof_data()
+	{
+		if(tof_data_list.empty()){
+			return;
+		}
+
+		try{
+			std::lock_guard<std::mutex> lg(m_mtx);
+			std::vector<tof_data> vtd;
+			//vtd.insert(vtd.end(), tof_data_list.begin(), tof_data_list.end());
+			vtd.swap(tof_data_list);
+			std::string val = __jsBuilder.build_tof_data(vtd);
+			swsClientMgr.send(JSON_CMD_VALUE_PUSH, val);
+
+			//tof_data_list.clear();
+			//std::vector<tof_data>().swap(tof_data_list);
+			tof_data_list.clear();
+			//tof_data_list.resize(0);
+		}catch(...){
+			//log_error("[error] err=%s", e.what());
+			int a = 0;
+			log_error("[error] err");
+		}
+	}
+
     /*
      * 定时器线程发送定位数据
      *
      * */
 	void wsTimerThread::_ThreadFunc( wsTimerThread * pOwner )
 	{	
-        log_info("The timer thread is running");
+        //log_info("The timer thread is running");
         while ( pOwner->__Enable )
 		{
 			std::time_t t = time( 0 );
@@ -171,6 +196,7 @@ namespace sys
                 //pOwner->temp_send_card_pos();
                 //pOwner->send_light_state();
 				pOwner->send_device_state();
+				pOwner->send_tof_data();
 				__LastSendTime = t;
 			}
           	boost::this_thread::sleep( boost::posix_time::millisec( 1 ) );
@@ -188,15 +214,19 @@ namespace sys
 	void wsTimerThread::upt_card_pos( const _CARD_POS_ & pos )
 	{
 		//如果已存在就更新，否则插入
-		uint64_t type = pos.Type;
-		uint64_t id=type<<32|pos.ID;
-		if ( __CardPosList.seek( id  ) )
-		{
-			__CardPosList.update( id, pos );
-		}
-		else
-		{
-			__CardPosList.insert( id, pos );
+		try{
+			uint64_t type = pos.Type;
+			uint64_t id=type<<32|pos.ID;
+			if ( __CardPosList.seek( id  ) )
+			{
+				__CardPosList.update( id, pos );
+			}
+			else
+			{
+				__CardPosList.insert( id, pos );
+			}
+		}catch(std::exception& e){
+			log_error("[error] err=%s", e.what());
 		}
 	}
 
@@ -223,4 +253,15 @@ namespace sys
 	{
 		device_state_list.push_back(ds);
 	}
+
+	void wsTimerThread::upt_tof_data(const tof_data& td)
+	{
+		try{
+			std::lock_guard<std::mutex> lg(m_mtx);
+			tof_data_list.push_back(td);
+		}catch(std::exception& e){
+			std::string err = e.what();
+			log_error("[error] err=%s", err.c_str());
+		}
+	}
 }

websocket/wsTimerThread.h

@@ -31,6 +31,7 @@ websocket定时发送线程类
 #include "thread_safe_map.h"
 #include "ws_common.h"
 #include "jsonBuilder.h"
+#include <mutex>
 
 namespace sys
 {
@@ -46,8 +47,11 @@ namespace sys
 		thread_safe_map<uint64_t, _CARD_POS_> __CardPosList;//卡位置列表
         std::time_t  __LastSendTime{0};//上一次发送的时间
 		jsonBuilder __jsBuilder;//json构造器类
+	
+		std::mutex m_mtx;	// 数据锁
         std::vector<light_state> light_state_list;		// 红绿灯状态列表
 		std::vector<device_state> device_state_list;	// 设备状态列表
+		std::vector<tof_data> tof_data_list;			// tof数据待发送列表
 	private:
 		/**
 		* @brief
@@ -101,6 +105,7 @@ namespace sys
         void temp_send_card_pos();
         void send_light_state();
 		void send_device_state();
+		void send_tof_data();
 
 	protected:
 		/**
@@ -206,6 +211,7 @@ namespace sys
 
         void upt_light_state(const light_state& light);
 		void upt_device_state(const device_state& ds);
+		void upt_tof_data(const tof_data& td);
 	};
 }
 

websocket/ws_common.h

@@ -22,6 +22,9 @@
 #ifndef _WS_COMMON_INC_H_
 #define _WS_COMMON_INC_H_
 #include <map>
+#include <array>
+#include <string>
+
 namespace sys
 {
     const int MIN_SEND_INTERVAL = 1;//最小发送时间间隔
@@ -192,6 +195,49 @@ namespace sys
             , m_cur_time(0)
         {}
     };
+	
+	// tof_data 数据
+	struct tof_data{
+		std::string	cid;	// 卡号
+		double y;			// 一维坐标
+		std::array<double, 2> ant_dist;	// 天线的测距距离
+		int sid;	// 基站号
+		std::string cur_time;	// 当前时间
+		double ant_diff;
+
+		tof_data()
+			: cid("")
+			  , y(0.0)
+			  , sid(0)
+			  , cur_time("")
+			  , ant_diff(0)
+		{
+			ant_dist.fill(0);
+		}
+
+		~tof_data(){
+			reset();
+		}
+
+		void reset()
+		{
+			cid = cur_time = "";
+			y = 0;
+			sid = 0;
+			ant_diff = 0;
+			ant_dist.fill(0);
+		}
+		/*tof_data& operator=(const tof_data v){
+			this->cid = v.cid;
+			this->y = v.y;
+			this->sid = v.sid;
+			this->cur_time = v.cur_time;
+			this->ant_dist = ant_dist;
+			this->ant_diff = v.ant_diff;
+			return *this;
+		}*/
+	};
+
 }
 
 #endif

worker.cpp

@@ -202,10 +202,14 @@ struct worker_thread: loop_thread ,visitor<std::shared_ptr<card_location_base>>
 		switch(t.m_cmd_code)
 		{
 			case 0x843b://tof
-				log_info("card loc message: 0x%04X", t.m_cmd_code);
+				//log_info("card loc message: 0x%04X", t.m_cmd_code);
 				card_list::instance()->on_message(this,t.body<message_locinfo>(),false);
 				t.destroy();
                 break;
+			case CHAR_LOCATEDATA_TOF_INS:
+				card_list::instance()->on_message(this, t.body<message_ins_locinfo>(), false);
+				t.destroy();
+				break;
             case CHAR_LOCATEDATA_PDOA:      // pdoa实时定位数据
                 card_list::instance()->on_message(this, t.body<message_pdoa_locinfo>(), false);
                 t.destroy();

znet.cpp

@@ -620,7 +620,8 @@ struct main_loop:io_context
 		int fd=zio::listen_on(port);
 		if(fd<0)
 		{
-			log_errno("try listen_on %d",port);
+			log_errno("try listen_on %d", port);
+			std_errno("listen %d failed", port);
 			return -1;
 		}