add tdoa and pdoa algorithm

ant.h

@@ -18,8 +18,11 @@
 #include "line.h"
 #include "point.h"
 #include "write-copy.h"
-#include"net-service.h"
-#include"common.h"
+#include "net-service.h"
+#include "common.h"
+
+#include <boost/circular_buffer.hpp>
+
 class client;
 class area;
 struct path
@@ -107,6 +110,13 @@ struct site:point,std::enable_shared_from_this<site>
     /// 指定分站定位类型：一维定位，二维定位，三维定位
     int m_dimension=0;
 
+    //pdoa
+    double m_pdoa_offset = 0.0;     // pdoa分站天线相位偏移量
+    int m_pdoa_direction = 0;       // pdoa分站天线1朝向
+    float m_pdoa = 100.0;           // pdoa分站当前上传相位差
+    float m_last_pdoa = 100.0;      // pdoa分站上一帧数据的相位差
+    boost::circular_buffer<float> m_cb_pdoa;    // pdoa分站历史相位差队列
+
     std::shared_ptr<client> m_clt=nullptr;
     std::shared_ptr<area> m_area=nullptr;
 
@@ -307,10 +317,12 @@ struct site:point,std::enable_shared_from_this<site>
 
 	void on_power_status(bool ac_down);//电源状态
 };
+
 struct visit_site_status:visitor<std::shared_ptr<site>>
 {
 	bool visit(std::shared_ptr<site> s);
 };
+
 struct sit_list:single_base<sit_list,int,std::shared_ptr<site>>
 {
     void load(const std::string &id)

card.cpp

@@ -370,7 +370,7 @@ void card_list::on_message(zloop<task*> *loop, message_locinfo&loc,bool is_histo
 	const auto &c=card_list::instance()->get(cardid);
 	if(!c)
 	{
-		log_warn("数据库中未定义该卡的信息，card_id=%d, card_type=%d,cardid:%lld",loc.m_card_id,loc.m_card_type,cardid);
+		log_warn("数据库中未定义该卡的信息，card_id=%d, card_type=%d, cardid:%lld",loc.m_card_id,loc.m_card_type,cardid);
 		return;
 	}
 	double dist_tof=loc.m_tof*15.65*2.996*1e-4;
@@ -387,6 +387,32 @@ void card_list::on_message(zloop<task*> *loop, message_locinfo&loc,bool is_histo
         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);
 }
 
+void card_list::on_message(zloop<task*>* loop, message_tdoa_locinfo& loc, bool is_history)
+{
+	uint64_t cardid = tool_other::type_id_to_u64(loc.m_card_msg.m_type,loc.m_card_msg.m_id);
+	const auto &c=card_list::instance()->get(cardid);
+	if(!c)
+	{
+		log_warn("[tdoa] 数据库中未定义该卡的信息，card_id=%d, card_type=%d, cardid:%lld",loc.m_card_msg.m_id,loc.m_card_msg.m_type, cardid);
+		return;
+	}
+    
+    c->on_message(loop, loc, is_history);
+}
+
+void card_list::on_message(zloop<task*>* loop, message_pdoa_locinfo& loc, bool is_history)
+{
+	uint64_t cardid = tool_other::type_id_to_u64(loc.m_card_type,loc.m_card_id);
+	const auto &c = card_list::instance()->get(cardid);
+	if(!c)
+	{
+		log_warn("[pdoa] 数据库中未定义该卡的信息，card_id=%d, card_type=%d, cardid:%lld",loc.m_card_id,loc.m_card_type,cardid);
+		return;
+	}
+    
+    c->on_message(loop, loc, is_history);
+}
+
 //获取卡数据  //标识id 人staff_id 车 vehicle_id
 const std::shared_ptr<card_location_base> card_list::get_card_by_cid(int cid)
 {

card.h

@@ -17,6 +17,9 @@ struct card_list:single_base<card_list,uint64_t,std::shared_ptr<card_location_ba
     void init_staffer(const std::string & lszId64);
     void init_vehicle(const std::string & lszId64);
     void on_message(zloop<task*> *loop,message_locinfo&loc,bool is_history);
+    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 init_card_from_db();
 
     void load_his_card_postion_vehicle();

card_base.cpp

@@ -127,7 +127,7 @@ void card_location_base::on_location(const std::vector<point>&vp,const std::vect
 	}
 }
 
-void card_location_base::on_message(zloop<task*> * loop,message_locinfo&loc,bool is_history)
+void card_location_base::on_message(zloop<task*>* loop, message_locinfo& loc,bool is_history)
 {
 	m_ct = loc.m_card_ct;
 	m_time = loc.m_time_stamp;
@@ -138,7 +138,6 @@ void card_location_base::on_message(zloop<task*> * loop,message_locinfo&loc,bool
 		return;
 	}
 
-
 	auto area_tool=get_area_tool();
 	area_tool->set_site(site_ptr);
 
@@ -160,6 +159,40 @@ void card_location_base::on_message(zloop<task*> * loop,message_locinfo&loc,bool
 		m_message_handle->on_message(loop,loc,is_history);
 	}
 }
+
+/*
+ * tdoa 调用算法库定位
+ * */
+void card_location_base::on_message(zloop<task*>* loop, message_tdoa_locinfo& loc, bool is_history)
+{
+    log_info("[tdoa] start calc location.");
+    m_ct = loc.m_card_msg.m_sync_num;
+    auto site_ptr = sit_list::instance()->get(loc.m_site_msg.m_site_id);
+    
+    if(!site_ptr){
+        log_warn("[tdoa] 接收到分站的数据：site=%d, card=%d, ct=%d, 但是分站未定义", loc.m_site_msg.m_site_id, m_id, loc.m_card_msg.m_time_stamp);
+        return;
+    }
+
+    m_message_handle->on_message(loop, loc, is_history);
+}
+
+/*
+ * pdoa 调用算法库定位
+ *
+ * */
+void card_location_base::on_message(zloop<task*>* loop, message_pdoa_locinfo& loc, bool is_history)
+{
+    log_info("[pdoa] start calc location");
+    m_ct = loc.m_card_ct;
+    auto site_ptr = sit_list::instance()->get(loc.m_site_id);
+    if(!site_ptr){
+        log_warn("[pdoa] 接收到分站的数据：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);
+}
 //前端推送位置函数.
 void card_location_base::upt_card_pos(YA::_CARD_POS_&cp, point &pt)
 {

card_base.h

@@ -25,6 +25,8 @@ struct area_hover;
 struct site_area_hover;
 struct site;
 struct area_tool;
+struct message_tdoa_locinfo;
+struct message_pdoa_locinfo;
 
 namespace YA{struct _CARD_POS_;}
 
@@ -42,6 +44,8 @@ struct card:point
 		,m_pwr_stat(0)
 		,m_display(dis)
 		,m_ct(0)
+        ,m_freq_id(0)
+        ,m_freq(0.0)
         ,m_acc(0)
 		,m_speed(0)
 	{}
@@ -59,6 +63,8 @@ struct card:point
 	int	     m_pwr_stat;		//电量状态
 	uint16_t m_display;			//1显示0不显示，往前端推送
 	uint16_t m_ct;				//ct
+    uint8_t  m_freq_id;         // 频率索引
+    double   m_freq;            // 频率值
     ///人卡(加速度状态)，其值就是0和1，掘进机和采煤机，其值就是0~255，车辆，其值就需要乘以0.01，用于表示加速度值
     double   m_acc;
 	double   m_speed;			//速度
@@ -81,7 +87,6 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
 	time_t m_help_last_time=0;
 	int    m_help_bit=0;
 
-
     card_location_base()=default;
     card_location_base(const std::string&type,uint32_t id,uint16_t dis,int16_t t,int32_t,int32_t,uint32_t );
 
@@ -104,7 +109,10 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
     virtual void handle_message(uint16_t ct,uint8_t& value){}
 
     void make_his_location(uint64_t t,const point & pt,bool bclose = false);
-    void on_message(zloop<task*> * loop,message_locinfo&loc,bool is_history);
+    void on_message(zloop<task*>* loop, message_locinfo&loc, bool is_history);
+    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 do_status(int st);
 	void upt_card_pos(YA::_CARD_POS_&cp, point &pt);
@@ -124,6 +132,10 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
 
     static std::shared_ptr<card_location_base> make_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);
+    void set_freq_id(const uint8_t& val)
+    {
+        m_freq_id = val;
+    }
 };
 
 #endif

card_path.cpp

@@ -10,13 +10,14 @@
 #include "line.h"
 #include "ant.h"
 #include "config_file.h"
-
 #include "card_path.h"
 #include "visit.h"
 
 namespace{
 
-static const double PI=3.1416;
+//static const double PI  = 3.1415926;
+//static const double TPI = 2*PI;
+
 inline bool eq(double a,double b,double e=0.0001)
 {
 	return fabs(a-b)<e;

common.h

@@ -4,6 +4,8 @@
 #define LENGTH_SQL 2000
 #define SPEED_COUNT_LIMIT 5
 #define READER_TIMEOUT 20
+#define PI 3.1415926
+#define TPI (2*3.1415926)
 
 /**
  * @brief 分站位置 1井上，2井下
@@ -21,6 +23,7 @@ enum STA_TYPE
 	STATUS_HELP_=0,
 	STATUS_LOW_POWER_,
 };
+
 enum STATUS_CARD
 {
 	STATUS_NORMAL=0,
@@ -61,11 +64,13 @@ enum CARD_TYPE
     ///    5  掘进机
     CT_HEADING_MACHINE=5
 };
+
 enum VEHICLE_TYPE
 {
     VT_COAL_CUTTER=25,
     VT_HEADING_MACHINE=26
 };
+
 enum AREA_TYPE
 {
 	AREA_TYPE_UPMINE =0,
@@ -89,7 +94,6 @@ enum EVENT_STATUS
     ES_END = 100
 };
 
-
 enum OBJECT_TYPE
 {
     ///矿井
@@ -103,12 +107,14 @@ enum OBJECT_TYPE
     //一人多卡
     OT_MORE_CARD=11
 };
+
 enum EVENT_DIS_TYPE
 {
     DT_NORMAL=1,//内部显示
     DT_SPECIAL=2,//外部显示
     DT_COMMON=3//全显示
 };
+
 enum EVENT_TYPE{ // 事件类型
     ET_OVER_COUNT_PERSON = 1,       // 井下人员超员
     ET_OVER_COUNT_VEHICLE = 2,      // 井下车辆超员
@@ -136,5 +142,19 @@ enum EVENT_TYPE{ // 事件类型
     CARD_EVENT_COUNT_MAX
 };
 
+/*
+ * 定位维度
+ * */
+enum DIMENSION{
+    _1D = 1,
+    _2D = 2,
+    _3D = 3,
+};
+
+enum LOCATE_DATA_TYPE{
+    LDT_TOF     = 0,
+    LDT_TDOA    = 1,
+    LDT_PDOA    = 2,
+};
 
 #endif

db/db_card.cpp

@@ -15,7 +15,7 @@ namespace db_card
 {
 	std::unordered_map<uint64_t,std::shared_ptr<card_location_base>> load_car(const std::string & lszId64)
 	{
-		std::string sql = "SELECT ve.vehicle_id, ve.card_id, c.card_type_id, \
+		std::string sql = "SELECT ve.vehicle_id, ve.card_id, c.card_type_id, c.freq_id \
 						   ve.dept_id, ve.group_id, v.vehicle_type_id, vt.vehicle_level_id, \
 						   vt.is_railroad AS vt_is_railroad,ve.need_display ,ve.power_alarm,\
 						   vt.vehicle_category_id,v.bigger_car_flag,vc.over_speed \
@@ -64,6 +64,9 @@ namespace db_card
 			unsigned int card_type_id  = 0;
 			DBRes.GetField( "card_type_id",card_type_id, Error );
 
+            int freq_id = 0;
+            DBRes.GetField("freq_id", freq_id, Error);
+
 			int dept_id = 0;
 			DBRes.GetField( "dept_id",dept_id, Error );
 
@@ -107,7 +110,9 @@ namespace db_card
 			auto clb = card_location_base::make_car(strategy,vsid,need_display,card_type_id,
 					dept_id,vehicle_category_id, vehicle_type_id,vehicle_level_id,vehicle_id);
 			uint64_t cardid = tool_other::type_id_to_u64(card_type_id,vsid);
-			log_info("cardId:%llu,id:%d dept_id:%d,need_display:%d-cardid:%s,categoryid:%d,vchile_id:%d,type:%d,vehicle_type_id:%d",
+			clb->set_freq_id(freq_id);
+
+            log_info("cardId:%llu,id:%d dept_id:%d,need_display:%d-cardid:%s,categoryid:%d,vchile_id:%d,type:%d,vehicle_type_id:%d",
 					cardid,vsid,dept_id,need_display,card_id.c_str(),vehicle_category_id,vehicle_id,card_type_id,vehicle_type_id);
 			map.insert({cardid,clb});
 		}
@@ -117,7 +122,7 @@ namespace db_card
 
 	std::unordered_map<uint64_t,std::shared_ptr<card_location_base>> load_person(const std::string & lszId64,const std::string&strategy)
 	{
-		std::string sql = "SELECT s.staff_id, s.card_id, c.card_type_id, s.dept_id, s.group_id, s.occupation_id, \
+		std::string sql = "SELECT s.staff_id, s.card_id, c.card_type_id, c.freq_id, s.dept_id, s.group_id, s.occupation_id, \
 						   ol.occupation_level_id,s.worktype_id,s.need_display,s.work_line\
 							,ds.name as staffer_name,dd.name as dept_name\
 						   FROM dat_staff_extend s \
@@ -163,6 +168,9 @@ namespace db_card
 			unsigned int card_type_id  = 0;
 			DBRes.GetField( "card_type_id",card_type_id, Error );
 
+            int freq_id = 0;
+            DBRes.GetField("freq_id", freq_id, Error);
+
 			int dept_id = 0;
 			DBRes.GetField( "dept_id",dept_id, Error );
 
@@ -200,6 +208,8 @@ namespace db_card
             std::shared_ptr<card_location_base> clb =
                     std::make_shared<person>(strategy,vsid,need_display,card_type_id,dept_id,occupation_level_id,staff_id,work_line,staffer_name,dept_name,worktype_id);
             uint64_t cardid = tool_other::type_id_to_u64(card_type_id,vsid);
+            clb->set_freq_id(freq_id);
+
 			log_info("Init_card.cardId:%llu,id:%d dept_id:%d,need_display:%d,card:%s:work_line:%d,staff_id:%d,type:%d,staffer_name:%s,dept_name:%s",
 					cardid, vsid, dept_id, need_display, card_id.c_str(), work_line, staff_id, card_type_id,staffer_name.c_str(), dept_name.c_str());
             map.insert({cardid,clb});

db/db_tool.cpp

@@ -1,11 +1,8 @@
-
 #include "log.h"
 #include "db_api/CDBSingletonDefine.h"
 #include "card_base.h"
-
 #include "mine.h"
 #include "area.h"
-
 #include "tool_time.h"
 #include "db_tool.h"
 

loc_message.h

@@ -1,6 +1,7 @@
 #ifndef __LOC_MESSAGE__HPP
 #define __LOC_MESSAGE__HPP
 #include "ant.h"
+
 struct loc_message
 {
     std::shared_ptr<site> m_sit;
@@ -16,6 +17,15 @@ struct loc_message
     uint16_t m_rssi;
 	uint16_t m_batstatus;
 
+    // tdoa内容
+    uint64_t m_interpolation;       // 线性插值
+    uint8_t  m_loc_type;            // 数据类型：0为tof，1为tdoa
+    uint8_t  m_loc_dimension;       // 定位维度
+    double   m_freq;                // 卡定位频率值
+
+    // pdoa内容
+    double m_poa[3];        // 三天线的相位值
+
 	loc_message()
 	 :m_num_ticks(0)
 	{
@@ -40,6 +50,118 @@ struct loc_message
         ,m_sync_ct(sync_ct)
         ,m_rssi(rssi)
 		,m_batstatus(batstatus)
+        ,m_interpolation(0)
+        ,m_loc_type(0)
+        ,m_loc_dimension(0)
+        ,m_freq(0.0)
+    {
+        m_poa[0] = m_poa[1] = m_poa[2] = 0.0;
+    }
+
+    // tdoa定位数据,16
+    loc_message(std::shared_ptr<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,uint16_t batstatus,
+                uint64_t _interpolation, uint8_t _loc_type, uint8_t _dim, uint8_t _freq)
+        :m_sit(s)
+        ,m_num_ticks(num_ticks)
+        ,m_loc_time(timestamp)
+        ,m_card_id(cardid)
+        ,m_card_ct(ct)
+        ,m_card_type(type)
+        ,m_ant_id(antid)
+        ,m_rav(rav)
+        ,m_acc(acc)
+        ,m_sync_ct(sync_ct)
+        ,m_rssi(rssi)
+		,m_batstatus(batstatus)
+        ,m_interpolation(_interpolation)
+        ,m_loc_type(_loc_type)
+        ,m_loc_dimension(_dim)
+        ,m_freq(_freq)
+    {
+        m_poa[0] = m_poa[1] = m_poa[2] = 0.0;
+    }
+
+    // pdoa定位数据,17
+    loc_message(std::shared_ptr<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,uint16_t batstatus
+                ,uint8_t _loc_type, uint8_t _dim, double _poa0, double _poa1, double _poa2)
+        :m_sit(s)
+        ,m_num_ticks(num_ticks)
+        ,m_loc_time(timestamp)
+        ,m_card_id(cardid)
+        ,m_card_ct(ct)
+        ,m_card_type(type)
+        ,m_ant_id(antid)
+        ,m_rav(rav)
+        ,m_acc(acc)
+        ,m_sync_ct(sync_ct)
+        ,m_rssi(rssi)
+		,m_batstatus(batstatus)
+        ,m_loc_type(_loc_type)
+        ,m_loc_dimension(_dim)
+    {
+        m_poa[0] = _poa0;
+        m_poa[1] = _poa1;
+        m_poa[2] = _poa2;
+    }
+
+    loc_message& operator=(const loc_message& lhs)
+    {
+        if(this != &lhs){
+            this->m_sit             = lhs.m_sit;
+            this->m_num_ticks       = lhs.m_num_ticks;
+            this->m_loc_time        = lhs.m_loc_time;
+            this->m_card_id         = lhs.m_card_id;
+            this->m_card_ct         = lhs.m_card_ct;
+            this->m_card_type       = lhs.m_card_type;
+            this->m_ant_id          = lhs.m_ant_id;
+            this->m_rav             = lhs.m_rav;
+            this->m_acc             = lhs.m_acc;
+            this->m_sync_ct         = lhs.m_sync_ct;
+            this->m_rssi            = lhs.m_rssi;
+            this->m_batstatus       = lhs.m_batstatus;
+            this->m_loc_type        = lhs.m_loc_type;
+            this->m_loc_dimension   = lhs.m_loc_dimension;
+            this->m_poa[0]          = lhs.m_poa[0];
+            this->m_poa[1]          = lhs.m_poa[1];
+            this->m_poa[2]          = lhs.m_poa[2];
+            this->m_interpolation   = lhs.m_interpolation;
+            this->m_freq            = lhs.m_freq;
+        }
+
+        return *this;
+    }
+};
+
+struct pdoa_message: point{
+    double angle;
+    double distance;
+    double x_ant;
+    double y_ant;
+
+    pdoa_message(const double& _x, const double& _y, const double& _angle, const double& _distance, const double& _ax, const double& _ay):point(_x, _y), angle(_angle), distance(_distance), x_ant(_ax), y_ant(_ay)
+    {}
+
+    pdoa_message():point(0.0, 0.0),angle(0.0), distance(0.0), x_ant(0.0), y_ant(0.0)
+    {}
+};
+
+using pdoa_msg_ptr = std::shared_ptr<pdoa_message>;
+
+struct pdoa_param{
+    double pdoa;
+    double r;
+    double a;
+    double ax;
+    double ay;
+
+    pdoa_param(const double& _pdoa, const double& _r, const double& _a, const double& _ax, double& _ay):pdoa(_pdoa), r(_r), a(_a), ax(_ax), ay(_ay)
+    {}
+
+    pdoa_param():pdoa(0.0), r(0.0), a(0.0), ax(0.0), ay(0.0)
     {}
 };
 

main.cpp

@@ -1,4 +1,5 @@
 #include <ev++.h>
+#include <iterator>
 #include <log.h>
 #include <net-service.h>
 #include "db_api/CDBSingletonDefine.h"
@@ -20,6 +21,7 @@
 #include "websocket/web_connect.h"
 #include "forbid_staff_down_mine.h"
 #include "bulletin_broad_show.h"
+#include "sync_time/sync_manager.h"
 
 config_file config;
 void handlereader(uint32_t readerid,bool duration,uint32_t t)
@@ -102,6 +104,8 @@ struct Init_Setting
 		    init_three_rates(dp);
         }
 
+        init_ant_sync();
+
         log_info("Init_Setting::init  Success. \n" );
     }
 
@@ -139,6 +143,24 @@ struct Init_Setting
 
         return web_connect::connect();
     }
+
+    // 初始化时间同步内的天线间距离信息
+    void init_ant_sync()
+    {
+        for(auto site : sit_list::instance()->m_map){
+            ssync_manager.update_anchor(site.second->m_id, 1, site.second->m_ant[0].x, site.second->m_ant[0].y, site.second->m_ant[0].z);
+        }
+
+        double d = 0.0;
+        for(auto it = ssync_manager.ump_anchors.begin(); it != ssync_manager.ump_anchors.end(); ++it){
+            for(auto it2 = std::next(it,1);it2 != ssync_manager.ump_anchors.end(); ++it2){
+                d = sqrt(pow((it->second.x - it2->second.x),2)
+                        + pow((it->second.y - it2->second.y),2)
+                        + pow((it->second.z - it2->second.z),2)) * 10000 / (2.99702547 * 15.65);
+                ssync_manager.update_distance(it->first>>8, it->first&0xFF, it2->first>>8, it2->first&0xFF, d);
+            }
+        }
+    }
 };
 
 
@@ -189,7 +211,7 @@ int main(int argc ,char * argv[])
     //atexit(&cleanup);
 
     net_service mh;
-    int port=config.get("service.port",4000);
+    int port = config.get("service.port",4000);
 
     int interface_port = config.get("service.interface_port",7001);
     log_info("service-position.run(%d)",interface_port);

message.cpp

@@ -42,6 +42,7 @@ task* message_locinfo::clone(message_locinfo* ml)
     m.m_ant_id = ml->m_ant_id;
     m.m_sync_ct = ml->m_sync_ct;
     m.m_rssi = ml->m_rssi;
+
 	return std::move(t);
 }
 
@@ -175,9 +176,11 @@ void message_tdoasync::zero_this()
 	m_parent_site_id=
 	m_local_ant_id=
 	m_parent_ant_id=
-	m_sync_ct=
+	m_sync_num=
 	m_local_level=
 	m_recv_time=
+    m_root_site_id = 
+    m_root_ant_id =
 	m_send_time=0;
 }
 
@@ -187,13 +190,15 @@ void message_tdoasync::load(zistream&is)
 	uint8_t b;
 
 	//本地分站ID和天线ID
-	is>>m_local_site_id>>b; m_local_ant_id=b;
+	is>>m_local_site_id>>b; 
+    m_local_ant_id=b;
 
 	//上级分站ID和天线ID
-	is>>m_parent_site_id>>b; m_parent_ant_id=b;
+	is>>m_parent_site_id>>b; 
+    m_parent_ant_id=b;
 
 	//根分站CT和本机级别
-	is>>m_sync_ct>>m_local_level;
+	is>>m_sync_num>>m_local_level;
 
 	uint32_t i;
 
@@ -206,6 +211,172 @@ void message_tdoasync::load(zistream&is)
 	is>>b>>i; 	
 	m_recv_time=b;
 	m_recv_time=(m_recv_time<<32)|i;
+
+    logn_info(4, "local_id: %d, upper_id: %d, sync_num: %d, level: %d, send: %llu, recv: %llu",
+            m_local_site_id, 
+            m_parent_site_id, 
+            m_sync_num,
+            m_local_level,
+            m_send_time,
+            m_recv_time
+            );
+}
+
+void message_tdoa_locinfo::zero_this()
+{
+    m_site_msg.clear();
+    m_card_msg.clear();
+}
+
+void message_tdoa_locinfo::load(zistream& is, uint16_t& cmd)
+{
+	zero_this();
+	uint8_t b;
+	uint32_t i;
+	//卡类型、卡号、CT、电池状态
+	is>>b>>m_card_msg.m_id>>m_card_msg.m_time_stamp>>m_card_msg.m_battery_status;
+
+	if(m_card_msg.m_id & 0xFFFF0000) 
+		log_warn("card_id=%#X,CT=%d,大于0xFFFF.",m_card_msg.m_id,m_card_msg.m_time_stamp);
+
+	m_card_msg.m_id &= 0xFFFF; //卡号不会大于65535，分站上传数据的bug
+
+	m_card_msg.m_type = b;
+
+	is>>b;
+	if(m_card_msg.m_type == 1)
+	{
+		m_card_msg.m_call_info = b;
+	}
+	else
+	{
+		m_card_msg.m_rav=((b&0x80)?-1.:1.)*(b&0x7f)*3;
+	}
+
+	//加速度
+	is>>b;
+    const auto &c=card_list::instance()->get(tool_other::type_id_to_u64(m_card_msg.m_type, m_card_msg.m_id));
+
+	if(m_card_msg.m_type == 1 ||(c && tool_other::is_coal_or_driving(m_card_msg.m_type, c->get_vehicle_type_id())))
+		m_card_msg.m_acc = b;
+	else
+		m_card_msg.m_acc = ((b&0x80)?-1.:1.)*(b&0x7f)*0.01;
+
+	//定位时间戳
+	is>>b>>i; 	
+	m_card_msg.m_loc_stamp = b;
+	m_card_msg.m_loc_stamp = (m_card_msg.m_loc_stamp<<32)|i;
+
+	//天线号
+	is>>m_card_msg.m_ant_id;
+	--m_card_msg.m_ant_id;
+
+    //序列号
+    is>>m_card_msg.m_sync_num;
+
+    //脉冲信道响应值
+    is>>m_card_msg.m_rssi;
+
+    if(CHAR_LOCATEDATA_TDOA_EXTEND == cmd || CHAR_LOCATEDATA_TDOA_EXTEND_INS == cmd){
+     	//信号电平值
+	    uint16_t sp1 = 0, sp2 = 0;
+	    is>>sp1>>sp2;
+	    m_card_msg.m_rssi = 10*log10(1.*sp1*(1<<17)/pow(sp2-64.,2))-121.74;
+	    m_card_msg.m_strength = sp1;
+        m_card_msg.m_rxpacc = sp2;
+    }
+
+    if(CHAR_LOCATEDATA_TDOA_EXTEND_INS == cmd){
+        is>>m_card_msg.m_acc_data[0]
+          >>m_card_msg.m_acc_data[1]
+          >>m_card_msg.m_acc_data[2]
+          >>m_card_msg.m_rav_data[0]
+          >>m_card_msg.m_rav_data[1]
+          >>m_card_msg.m_rav_data[2]
+          >>m_card_msg.m_walk_steps
+          >>m_card_msg.m_jump_counts
+          >>m_card_msg.m_hang_time
+          >>m_card_msg.m_hang_height;
+    }
+
+    log_info("[tdoa] type=%d, card=%d, site=%d, ct=%d, bat=%#X, acc=%d, fly_time=%llu, ant_id=%d, spq=%d",
+            m_card_msg.m_type,
+            m_card_msg.m_id,
+        	m_site_msg.m_site_id,
+        	m_card_msg.m_time_stamp,
+        	m_card_msg.m_battery_status,
+            m_card_msg.m_acc,
+        	m_card_msg.m_loc_stamp,
+        	m_card_msg.m_ant_id,
+        	m_card_msg.m_rssi
+            );
 }
 
+void message_pdoa_locinfo::zero_this()
+{
+    m_time_stamp =
+    m_site_time = 
+    m_tof =
+    m_site_id = 
+    m_card_type = 
+    m_card_id = 
+    m_card_type =
+    m_card_ct = 
+    m_batty_status = 
+    m_callinfo = 
+    m_rav = 
+    m_acc = 
+    m_ant_id = 
+    m_rssi = 0; 
+    m_poa[0] = 
+    m_poa[1] = 
+    m_poa[2] = 0.0;
+}
+
+void message_pdoa_locinfo::load(zistream& is)
+{
+    zero_this();
 
+    uint8_t b;
+    // 高4位为卡电池信息，低4位为卡类型
+    is>>b;
+
+    m_card_type = b&0x0F;
+    m_batty_status = (b>>4)&0xFF;
+    //log_info("type: %d, battery: %d", m_card_type, m_batty_status);
+
+    //2字节卡号，2字节卡的ct号
+    uint16_t id;
+    is>>id>>m_card_ct;
+    m_card_id = id;
+    
+    //角速度值：人卡车卡表示含义不同
+    is>>b;
+    if(m_card_type == 1){
+        m_callinfo = b;
+    }else{
+        m_rav = ((b&0x80)?-1.:1.)*(b&0x7F)*3;
+    }
+
+    //加速度
+    is>>b;
+    if(m_card_type == 1 || m_card_type == 4 || m_card_type == 5){
+        m_acc = b;
+    }else{
+        m_acc = ((b&0x80)?-1.:1.)*(b&0x7F)*0.01;
+    }
+
+    //信号电平值，tof测距结果
+    uint16_t tof;
+    uint8_t rssi;
+    is>>rssi>>tof;
+    m_rssi = rssi;
+    m_tof = tof;
+
+    is>>m_ant_id;
+    --m_ant_id;
+
+    is>>m_poa[0]>>m_poa[1]>>m_poa[2];
+
+    logn_info(3,"[pdoa] timestamp=%llu, type=%d, card_id=%d, site=%d, ct=%d, status=%d, acc=%d, tof=%llu, ant_id=%d, spq=%d, poa1=%d, poa2=%d, poa3=%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, m_poa[0], m_poa[1], m_poa[2]);
+}

message.h

@@ -3,19 +3,104 @@
 #include <stdint.h>
 #include <time.h>
 #include "worker.h"
+#include "protocol.h"
 
 struct zistream;
 
-#define CHAR_LOCATEDATA_TOF_EXTEND 0x843b			// TOF实时定位数据，请求、应答
-#define CHAR_LOCATEDATASMALL_TOF_EXTEND 0x753b		// TOF实时小分站定位数据，请求、应答
-#define CHAR_LOCATEDATAHIS_TOF_EXTEND 0x853b		// TOF历史定位数据，请求、应答
-#define CHAR_LOCATEDATA_TDOA_EXTEND 0x863b			// TDOA实时定位数据，请求、应答
-#define CHAR_LOCATEDATAHIS_TDOA_EXTEND 0x873b		// TDOA历史定位数据，请求、应答
-#define CHAR_TDOA_READER_SYNC_TIME 0xa78d			// TDOA分站时间同步
-#define CHAR_ADHOC	0x803b							// 上传自组网数据
-#define CHAR_CTRL_READER_CMD 0x804c					// 向分站发送控制指令，控制分站向上位机发送数据
-#define CHAR_VIRTUAL_DATA_PUSH_CMD 0x699a			//虚拟数据推送
-#define CHAR_LOCATEDATA_TOF_OPTIMIZE 0x753d         // TOF优化协议
+/*
+ * tdoa协议之分站数据
+ * */
+struct message_site{
+    uint16_t m_time_stamp;      // 分站时间戳
+    time_t   m_site_time;       // 分站时间
+    uint32_t m_site_id;         // 分站号
+    uint8_t  m_status;          // 状态
+    uint8_t  m_reverse;         // 保留
+    uint8_t  m_power_status;    // 电源状态
+
+    message_site():m_time_stamp(0), m_site_time(0), m_site_id(0), m_status(0), m_reverse(0), m_power_status(0)
+    {}
+
+    void clear()
+    {
+        m_time_stamp = m_site_time = m_site_id = m_status = m_reverse = m_power_status = 0;
+    }
+};
+
+/*
+ * tdoa协议之卡数据，支持以下三种数据协议：
+ * 1.tdoa实时定位数据协议;
+ * 2.扩展tdoa实时定位数据协议；
+ * 3.扩展tdoa实时定位数据协议（带惯导数据）；
+ * */
+struct message_tdoa_card{
+	uint8_t  m_type;                // 卡类型
+	uint32_t m_id;                  // 卡id
+	uint16_t m_time_stamp;          // 卡报文时间戳
+	uint8_t  m_battery_status;      //  电池状态
+	uint8_t  m_call_info;           // 0x80-呼救，0x01-一般呼叫，0x02-紧急呼叫
+	uint8_t  m_rav;                 // 角速度
+	uint8_t  m_acc;                 // 加速度
+    uint64_t m_loc_stamp;           // 定位时间戳
+	uint8_t  m_ant_id;              // 天线号
+	uint16_t m_sync_num;            // 同步序列号 
+	int16_t  m_rssi;                // 脉冲信道响应值
+    int16_t  m_strength;            // 信号强度
+    int16_t  m_rxpacc;              // 接收信号功率
+
+    // 以下参数用于tdoa带惯导数据输出
+    int16_t m_acc_data[3];          // 加速度三轴数据，0-x，1-y，2-z
+    int16_t m_rav_data[3];          // 角速度三轴数据，0-x，1-y，2-z
+    uint8_t m_walk_steps;           // 行进步数
+    uint8_t m_jump_counts;          // 跳跃次数
+    uint8_t m_hang_time;            // 滞空时间
+    uint8_t m_hang_height;          // 滞空高度
+
+    message_tdoa_card():m_type(0), m_id(0), m_time_stamp(0), m_battery_status(0), m_call_info(0), m_rav(0), m_acc(0), m_loc_stamp(0), m_ant_id(0), m_sync_num(0), m_rssi(0), m_strength(0), m_rxpacc(0), m_walk_steps(0), m_jump_counts(0), m_hang_time(0), m_hang_height(0)
+    {
+        m_acc_data[0] = m_acc_data[1] = m_acc_data[2] = 0;
+        m_rav_data[0] = m_rav_data[1] = m_rav_data[2] = 0;
+    }
+
+    int64_t get_long_id()const
+    {
+        return (((int64_t)m_type) << 32) | m_id;
+    }
+
+    void clear()
+    {
+        m_type = m_id = m_time_stamp = m_battery_status =
+        m_call_info = m_rav = m_acc = m_loc_stamp = m_ant_id =
+        m_sync_num = m_rssi = m_strength = m_rxpacc = m_walk_steps =
+        m_jump_counts = m_hang_time = m_hang_height = 0;
+    }
+};
+
+/*
+ * tdoa消息虚基类，主要定义子类重载的方法
+ *
+ * */
+struct message_loc{
+    virtual void zero_this() = 0;
+    virtual void load(zistream& is, uint16_t& cmd) = 0;
+    virtual int64_t long_id()const = 0;
+};
+
+struct message_tdoa_locinfo:task{
+    message_site m_site_msg;
+    message_tdoa_card m_card_msg;
+
+    uint64_t m_interpolation;
+    uint8_t  m_loc_type;
+    uint8_t  m_loc_dimension;
+
+    virtual void zero_this();
+    virtual void load(zistream& is, uint16_t& cmd);
+    virtual int64_t long_id()const
+    {
+        return m_card_msg.get_long_id();
+    }
+};
 
 //	分站传上来的卡定位数据,包括tof,tdoa
 struct message_locinfo:task
@@ -39,6 +124,8 @@ struct message_locinfo:task
     //优化协议
     float m_distance;
 
+    uint8_t m_loc_type;     // 数据类型，tof，tdoa，pdoa
+    
 	void zero_this();
     void load(zistream&is,bool tdoa);
     void set_rav(uint8_t rav){m_rav=rav;}
@@ -58,15 +145,27 @@ struct message_tdoasync:task
 	uint32_t m_parent_site_id;      // 上一级分站号
 	uint16_t m_local_ant_id;        // 分站分站天线号
 	uint16_t m_parent_ant_id;       // 上一级分站天线号
-	uint16_t m_sync_ct;             // 同步序列号
+	uint16_t m_sync_num;            // 同步序列号
 	uint16_t m_local_level;         // 分站本地层级
 	uint64_t m_recv_time;           // 接收时间
 	uint64_t m_send_time;           // 发送时间
+    uint64_t m_root_site_id;        // 本次同步root节点分站号
+    uint16_t m_root_ant_id;         // 本次同步root节点天线号
 
 	void zero_this();
     void load(zistream&is);
 };
 
+// pdoa分站数据
+struct message_pdoa_locinfo: public message_locinfo{
+    uint8_t m_loc_type;
+    uint8_t m_loc_dimension;
+    int16_t m_poa[3];
+    
+    void zero_this();
+    void load(zistream& is);
+};
+
 #endif
 
 

net-service.cpp

@@ -5,18 +5,19 @@
 #include <memory>
 #include <string.h>
 #include <math.h>
-
 #include <log.h>
 #include <znet.h>
-#include <zstream.h>
+//#include <zstream.h>
 #include <sys/time.h>
 #include "worker.h"
 #include "tdoa_sync.h"
+#include "protocol.h"
 #include "net-service.h"
 #include "ant.h"
 #include "card.h"
 #include "crc.h"
 #include "mine_business.h"
+#include "tool_time.h"
 
 net_service::net_service()
 {
@@ -127,6 +128,7 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 						index++;
 						task*t=task::alloc<message_locinfo>();
 						message_locinfo&m=t->body<message_locinfo>();
+                        m.m_loc_type = LDT_TOF;
 						m.m_site_id=site_id;
 						m.m_time_stamp=tstamp;
 						m.load(is,false);
@@ -196,9 +198,35 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 					}
 				}
 				break;
+            case CHAR_LOCATEDATA_PDOA:
+                {
+                    int32_t site_id = parse_data_anchor(clt, is);
+                    if(site_id < 0){
+                        return;
+                    }
+                    const auto &site_ptr = sit_list::instance()->get(static_cast<int32_t>(site_id));
+
+                    struct timeval tv;
+                    gettimeofday(&tv, NULL);
+                    uint64_t t = tv.tv_sec*1000 + tv.tv_usec/1000;
+                    int index = 0;
+                    while(!is.eof()){
+                        uint64_t tstamp = t - 1000 + 50 + index*45;
+                        ++index;
+                        task* t = task::alloc<message_pdoa_locinfo>();
+                        message_pdoa_locinfo& m = t->body<message_pdoa_locinfo>();
+                        m.load(is);
+                        m.m_loc_type = LDT_PDOA;
+                        //m.m_site_id = site_id;
+                        m.m_time_stamp = tstamp;
+                        t->m_cmd_code = cmd;
+                        t->m_hash_id = m.m_card_id;
+                        site_ptr->m_pdoa = get_pdoa(m.m_poa, site_ptr->m_pdoa_offset);
+                    }
+                }
+                break;
 			case 0x783A://tof 分站时间同步
 				{
-
 					//  从第一个字节开始，分别表示毫秒（2字节）、秒、分、时、天、月、年
 					unsigned char buf[20]={0,13,0x78,0x3b};
 
@@ -232,24 +260,28 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 									clt->name().c_str(),buf[12]+2000,buf[11],buf[9],buf[8],buf[7],buf[6],buf[5]*256+buf[4]);
 				}
 				break;
-			case CHAR_LOCATEDATA_TDOA_EXTEND://tdoa
+			case CHAR_LOCATEDATA_TDOA_EXTEND:       // 0x863b
+            case CHAR_LOCATEDATA_TDOA_EXTEND_INS:   // 0x901b
 				{
-					uint32_t site_id;
-					is>>site_id>>skip(12);
+                    int32_t site_id = parse_data_anchor(clt, is);
+					if(site_id < 0){
+                        return;
+                    }
+
 					while(!is.eof())
 					{
-						task*t=task::alloc<message_locinfo>();
-						message_locinfo&m=t->body<message_locinfo>();
-						m.load(is,true);
-						m.m_site_id=site_id;
-						t->m_cmd_code=cmd;
-						t->m_hash_id=m.m_card_id;
+						task* t = task::alloc<message_tdoa_locinfo>();
+						message_tdoa_locinfo& m = t->body<message_tdoa_locinfo>();
+                        m.m_loc_type = LDT_TDOA;
+						m.load(is, cmd);
+						t->m_cmd_code = cmd;
+						t->m_hash_id = m.m_card_msg.m_id;
 						m_sync_worker->translate(m);
 						m_loc_worker->request(t);
 					}
 				}
 				break;
-			case CHAR_TDOA_READER_SYNC_TIME://time sync
+			case CHAR_TDOA_READER_SYNC_TIME://time sync 0xa78d
 				{
 					message_tdoasync m;
 					m.load(is);
@@ -268,7 +300,6 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 			case CHAR_LOCATEDATAHIS_TOF_EXTEND://tof his
 			case CHAR_LOCATEDATAHIS_TDOA_EXTEND://tdoa his
 			case CHAR_CTRL_READER_CMD://ctrl site message
-			case CHAR_ADHOC://自组网数据
 			default:
 				message_handled=false;
 		}
@@ -286,3 +317,86 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 	}
 }
 
+int32_t net_service::parse_data_anchor(const std::shared_ptr<client>& clt, zistream& s)
+{
+    uint32_t site_id=-1;
+    uint16_t site_ct=-1;
+    uint8_t  power=-1;
+    struct tm site_tm={0}; 
+
+    // 4字节分站号
+    // 2字节时间戳
+    // 7字节分站时间
+    // 2字节保留字节
+    // 1字节电源状态
+    s>>site_id>>site_ct
+        >>*(uint8_t*)&site_tm.tm_sec >>*(uint8_t*)&site_tm.tm_min >>*(uint8_t*)&site_tm.tm_hour >>*(uint8_t*)&site_tm.tm_mday
+        >>*(uint8_t*)&site_tm.tm_mon >>*(uint8_t*)&site_tm.tm_mon >>*(uint8_t*)&site_tm.tm_year 
+        >>skip(2)>>power;
+
+    site_tm.tm_year+=2000-1900;
+    site_tm.tm_mon-=1;
+
+    const auto &site_ptr = sit_list::instance()->get(static_cast<int32_t>(site_id));
+    if(!site_ptr)
+    {
+        logn_error(1,"未定义分站：net=%s,site=%d", clt->name().c_str(), site_id);
+        return -1;
+    }
+
+    if(site_ptr->is_abnormal_site())
+    {
+        logn_error(1,"分站[%d]天线异常", site_id);
+        return -1;
+    }
+
+    check_message_time(clt, site_ptr, &site_tm, site_ct, power);
+
+    if(clt->type()!=2){
+        site_ptr->set_client(clt);
+        site_ptr->on_power_status((power&1)==0);
+    }
+
+    logn_info(1,"分站数据：site_id: %d, site_ct: %d, time: %s, power: %d", site_id, site_ct, tool_time::to_str_ex(&site_tm).c_str(), power);
+
+    return site_id;
+}
+
+void net_service::parse_data_card()
+{
+}
+
+float net_service::get_pdoa(int16_t poa[], const double& offset)
+{
+    if(poa == nullptr){
+        return -10.0;
+    }
+
+    float poa1 = poa[0]*0.001;
+    float poa2 = poa[1]*0.001;
+    float poa3 = poa[2]*0.001;
+
+    float pdoa_raw = poa2 - poa1 + PI;
+    while(pdoa_raw >= TPI){
+        pdoa_raw -= TPI;
+    }
+
+    while(pdoa_raw < 0){
+        pdoa_raw += TPI;
+    }
+
+    pdoa_raw -= PI;
+
+    //90 adjust
+    float pdoa = pdoa_raw - offset;
+    if(pdoa < -1*PI){
+        pdoa += TPI;
+    }
+    if(pdoa > PI){
+        pdoa -= TPI;
+    }
+
+    logn_info(3, "[pdoa] poa1=%.4f, poa2=%.4f, poa3=%.4f, poa_diff=%.4f", poa1, poa2, poa3, pdoa);
+
+    return pdoa;
+}

net-service.h

@@ -2,6 +2,7 @@
 #define __net_service_hpp__
 
 #include <znet.h>
+#include <zstream.h>
 
 struct client;
 struct worker;
@@ -18,6 +19,11 @@ struct net_service:service_callback
 	void on_message(const std::shared_ptr<client> &clt,const char*data,size_t len);
 	void on_timer();
     void on_connect(const std::shared_ptr<client> &clt);
+
+    int32_t parse_data_anchor(const std::shared_ptr<client>& clt, zistream& s);
+    void parse_data_card();
+    float get_pdoa(int16_t poa[], const double& offset);
+
 };
 
 #endif

tdoa_sync.cpp

@@ -1,18 +1,60 @@
 #include "tdoa_sync.h"
+#include "sync_time/sync_time_message.h"
+#include "sync_manager.h"
+#include <log.h>
+#include <config_file.h>
 
+extern config_file config;
 
 struct tdoa_sync_impl:tdoa_sync
 {
 	virtual void on_message(message_tdoasync&msg)
 	{
+        host_server::sync_time_message _sync_msg;
+
+        _sync_msg.set_local_id((unsigned long long)(msg.m_local_site_id));
+        _sync_msg.set_local_id((unsigned long long)((_sync_msg.get_local_id() << 8) + msg.m_local_ant_id));
+        _sync_msg.set_upper_id((unsigned long long)(msg.m_parent_site_id));
+        _sync_msg.set_upper_id((unsigned long long)((_sync_msg.get_upper_id() << 8) + msg.m_parent_ant_id));
+        _sync_msg.set_root_id((unsigned long long)(msg.m_root_site_id));
+        _sync_msg.set_root_id((unsigned long long)((_sync_msg.get_root_id() << 8) + msg.m_root_ant_id));
+        _sync_msg.set_sync_num(msg.m_sync_num);
+        _sync_msg.set_sync_level(msg.m_local_level);
+        _sync_msg.set_local_send_time(msg.m_send_time);
+        _sync_msg.set_local_receive_time(msg.m_recv_time);
 	
-	
+        ssync_manager.analyze_sync_msg(_sync_msg);
 	}
 
-	virtual void translate(message_locinfo&tdoa_msg)
+	virtual void translate(message_tdoa_locinfo& tdoa_msg)
 	{
-	
-	
+	    host_server::tag_message _tag_msg;
+        _tag_msg.m_receive_time = tdoa_msg.m_card_msg.m_loc_stamp;
+        _tag_msg.m_local_id = host_server::sync_helper::parse_id((unsigned long long)tdoa_msg.m_site_msg.m_site_id, (unsigned long long)(tdoa_msg.m_card_msg.m_ant_id + 1));
+        _tag_msg.m_sync_num = tdoa_msg.m_card_msg.m_time_stamp;
+
+        // 线性插值后的值
+        uint64_t ip_value = 0;
+        // 线性插值类型
+        int ip_type = config.get("service.interpolation", 0);
+
+        log_info("[tdoa] interpolation type: %d", ip_type);
+        switch(ip_type){
+            case 0:
+                ip_value = ssync_manager.cal_time_by_linear(_tag_msg);
+                break;
+            case 1:
+                ip_value = ssync_manager.cal_time_by_inter_linear(_tag_msg);
+                break;
+            default:
+                break;
+        }
+
+        if(0 == ip_value){
+            ip_value = LONGLONG_MAX;
+        }
+	    
+        tdoa_msg.m_interpolation = ip_value;
 	}
 };
 

tdoa_sync.h

@@ -6,10 +6,10 @@
 struct tdoa_sync
 {
 	//分站TDOA同步消息发送到此接口
-	virtual void on_message(message_tdoasync&sync_msg)=0;
+	virtual void on_message(message_tdoasync&sync_msg) = 0;
 
 	//将tdoa定位消息中的时间戳换算为同步链中最低级别的时间戳
-	virtual void translate(message_locinfo&tdoa_msg)=0;
+	virtual void translate(message_tdoa_locinfo& tdoa_msg) = 0;
 
 	//单例
 	static tdoa_sync*instance();

tool_time.h

@@ -1,8 +1,8 @@
 #ifndef _tool_time_h_
 #define _tool_time_h_
 
-
 #include <chrono>
+
 class tool_time
 {
 public:
@@ -106,6 +106,20 @@ public:
         return std::string(_time);
     }
 
+    static std::string to_str_ex(struct tm* t)
+    {
+        if(t == nullptr){
+            return "";
+        }
+
+        char _time[25] = {0};
+        sprintf(_time, "%d-%02d-%02d %02d:%02d:%02d", t->tm_year+1900,
+                t->tm_mon+1, t->tm_mday, t->tm_hour,
+                t->tm_min, t->tm_sec);
+
+        return std::string(_time);
+    }
+
     //"%d-%02d-%02d %02d:%02d:%02d.%03d"
     static std::string to_str_ex(std::chrono::system_clock::time_point time)
     {

worker.cpp

@@ -19,6 +19,7 @@
 #include "mine_business.h"
 #include "bulletin_broad_show.h"
 #include "event.h"
+#include "protocol.h"
 
 loop_thread::loop_thread ()
 {
@@ -109,7 +110,7 @@ struct timer_worker_thread: loop_thread
 		mine_business::inst()->run_business();
         event_list::instance()->load_his_data_from_db(false);
 
-		log_info("timer_worker_thread use time:%ldus", clock.count_us());
+		//log_info("timer_worker_thread use time:%ldus", clock.count_us());
 	}
 
 	void on_timeout()
@@ -194,21 +195,35 @@ struct worker_thread: loop_thread ,visitor<std::shared_ptr<card_location_base>>
 		log_info("update local cards,count=%d",m_local_card_list.size());
 	}
 
+    /*
+     * 任务队列处理消息队列，调用定位模块进行定位处理
+     *
+     * */
 	void do_task(task&t)
 	{
 		switch(t.m_cmd_code)
 		{
 			case 0x843b://tof
-			case 0x863b://tdoa
-				log_info("card loc message%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_PDOA:      // pdoa实时定位数据
+                log_info("card loc message: 0x%04X", t.m_cmd_code);
+                card_list::instance()->on_message(this, t.body<message_pdoa_locinfo>(), false);
+                t.destroy();
+                break;
+            case CHAR_LOCATEDATA_TDOA:              // tdoa实时定位数据
+            case CHAR_LOCATEDATA_TDOA_EXTEND:       // 扩展tdoa实时定位数据
+            case CHAR_LOCATEDATA_TDOA_EXTEND_INS:   // 扩展tdoa实时定位数据，带惯导数据
+                log_info("card loc message: 0x%04X", t.m_cmd_code);
+                card_list::instance()->on_message(this, t.body<message_tdoa_locinfo>(), false);
+                break;
 				//card_message::on_loc_message(this,t.m_param1);
 			break;
 			case 0x853b://tof his
 			case 0x873b://tdoa his
-				log_info("site history message%04X",t.m_cmd_code);
+				log_info("site history message: 0x%04X",t.m_cmd_code);
 				card_list::instance()->on_message(this,t.body<message_locinfo>(),true);
 				//site_message::on_sync(this,t.m_param1);
 				t.destroy();
@@ -260,7 +275,6 @@ struct worker_impl:worker
 	std::atomic<int> m_init_flag{-2};
 	virtual void stop()
 	{
-
 		if(m_timer_worker)
 		{
 			m_timer_worker->async_stop();
@@ -277,8 +291,6 @@ struct worker_impl:worker
 
 		for(auto&thr:m_threads)
 			thr->join();
-
-
 	}
 
 	worker_thread& hash(uint32_t i)

znet.cpp

@@ -353,11 +353,8 @@ struct sock_client:fd_io,client_ex
 	{
 		return m_fd;
 	}
-    //void set_site_id(int sid)
-    //{
-     //   m_site_id=id;
-    //}
-	void grow_buf(int len)
+
+    void grow_buf(int len)
 	{
 		if(m_size-m_clen>=len)
 			return;
@@ -470,9 +467,7 @@ struct sock_client:fd_io,client_ex
 		if(! m_can_write)
 		{
 			set(EV_READ|EV_WRITE);
-//			m_send_timer.set(5);
-//			m_send_timer.start();
-			return 0;
+    		return 0;
 		}
 
 		for(;;)