pdoa 2d V1.0

ant.h

@@ -110,11 +110,8 @@ struct site:point,std::enable_shared_from_this<site>
     int m_device_type_id=0;
     
     //pdoa
-    double m_pdoa = 100.0;          // pdoa分站当前上传相位差
-    double m_last_pdoa = 100.0;      // pdoa分站上一帧上传相位差
     double m_pdoa_offset = 0.0;     // pdoa分站天线相位偏移量,90度的偏移值
     int m_pdoa_direction = 0;       // pdoa分站天线1朝向
-    //float m_pdoa_diff = 0.0;        
     std::shared_ptr<client> m_clt = nullptr;
     std::shared_ptr<area> m_area = nullptr;
 

card_base.cpp

@@ -217,7 +217,8 @@ void card_location_base::upt_card_pos(YA::_CARD_POS_&cp, point &pt)
     point _p;
     if(pt.empty())
     {
-         _p=*this;pt=_p;
+         _p=*this;
+         pt=_p;
     }
     else
          _p=pt;
@@ -225,12 +226,9 @@ void card_location_base::upt_card_pos(YA::_CARD_POS_&cp, point &pt)
 
 	cp.Type=m_type;
 	cp.ID = m_id;
-
 	cp.speed = abs(ceil(m_speed));
-
 	cp.x = tool_other::round(_p.x,3);
 	cp.y = tool_other::round(_p.y,3);
-
 	cp.running_stat = m_stat;
 	cp.dept_id = m_deptid;
 	cp.display=m_display;
@@ -327,8 +325,8 @@ void card_location_base::put_traffic_light(card_pos& cp)
     lm.m_pos.z = cp.z;
     lm.m_pos.m_card_id = m_id;
     lm.m_pos.m_type = m_type;
-    auto site = site_list::instance()->get(cp.m_reader_id);
-    lm.m_pos.m_area_id = site->m_site_id;
+    //auto site = sit_list::instance()->get(cp.m_reader_id);
+    //lm.m_pos.m_area_id = site->m_site_id;
     //lm.m_pos.m_bigger = 0;
     lm.m_pos.m_speed = m_speed;
     log_info("[traffic_light] put light message to traffic light module, card_id=%d, ctype=%d, x=%.2f, y=%.2f", m_id, m_type, x, y);

card_base.h

@@ -92,12 +92,16 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
     //pdoa
     int16_t m_last_ct = -1;
     float m_last_dist = 0.0;
+    uint16_t m_last_site_id = 0;                // 卡上一次定位的分站id
+    uint64_t m_last_recv_time = 0;              // 卡上一次接收时间
+    int m_last_site_dir = -1;                   // 卡上一次定位的分站天线1朝向
     int m_buff_size = 0;
     float m_pdoa_diff = 100.0;                  // pdoa分站当前上传相位差
     float m_last_pdoa_diff = 100.0;             // pdoa 分站上一帧数据的相位差
     boost::circular_buffer<float> m_cb_pdoa;    // the list of current pdoa's different of phase;
     boost::circular_buffer<float> m_cb_tof;     // the list of tof that is pdoa history value
     boost::circular_buffer<point> m_cb_point;
+    point m_last_point;
 
     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 );

card_car.cpp

@@ -40,6 +40,7 @@ void car::do_business(const std::shared_ptr<site>&site,const point &pt,double ac
 	m_area_tool->on_point(shared_from_this(),pt);
 	m_timeval=m_time;
 	handle_three_rates(pt);
+    handle_traffic_light(pt);
 	uint64_t id=tool_other::type_id_to_u64(m_type,m_id);
 	mine_business::inst()->make_arg(id,pt,m_time);
 }

card_person.cpp

@@ -125,7 +125,11 @@ void person::do_business(const std::shared_ptr<site>&site,const point &pt,double
     //一人多卡数据输入
     if(m_person_cards_flag){
 	    Msg m;
-	    m.type=m_type;m.x=(int)x;m.y=(int)y;m.cmd=CMD_HANDLE;m.cardid=m_type<<32|m_id;
+	    m.type=m_type;
+        m.x=(int)x;
+        m.y=(int)y;
+        m.cmd=CMD_HANDLE;
+        m.cardid=m_type<<32|m_id;
 	    cardMgr::instance()->tryPut(m);
     }
 }
@@ -174,7 +178,8 @@ void person::on_timer()
 	cp.work_time = m_mine_tool->get_work_time();
 	cp.is_on_duty= m_mine_tool->is_on_duty();
     cp.m_freq = m_freq;
-	upt_card_pos(cp,pt);
+	cp.speed = m_speed;
+    upt_card_pos(cp,pt);
 	log_info("on_timer here ...%d,%lld,%.2f,%.2f,%d,%d--%d,speed=%.2f",m_id,_time,pt.x,pt.y,cp.map_id,cp.area_info.size(),person::m_limit_detained_time,m_speed);
     //int sid=0; if(auto st=m_area_tool->m_site)sid=st->m_area_id; m_his_location_card->push(_time,pt,sid,cp.map_id);
 

loc_message.h

@@ -138,25 +138,35 @@ struct loc_message
 
     float get_pdoa(const int i)
     {
-	if(i>1){
-		return 10.0;
-	}
-	float poa1 = m_poa[i];
-	float poa2 = m_poa[i+1];
-	    //float poa3 = poa[2];
+        if(i>1){
+            return 10.0;
+        }
+        float poa1 = m_poa[i];
+        float poa2 = m_poa[i+1];
 
-	float pdoa = poa2 - poa1 + PI - m_sit->m_pdoa_offset;
-	while(pdoa >= TPI){
-		pdoa -= TPI;
-	}
+        float pdoa = poa2 - poa1 - m_sit->m_pdoa_offset;
+        while(pdoa >= TPI){
+            pdoa -= TPI;
+        }
 
-	while(pdoa < 0){
-		pdoa += TPI;
-	}
+        if(fabs(pdoa) > 0.3){
+            while(pdoa < 0){
+                pdoa += TPI;
+            }
+        }
+        pdoa -= PI;
+
+        /*pdoa -= m_sit->m_pdoa_offset;
+
+        if(pdoa < -1.0*PI){
+            pdoa += TPI;
+        }
 
-	pdoa -= PI;
+        if(pdoa > PI){
+            pdoa -= TPI;
+        }*/
 
-	return pdoa;
+        return pdoa;
     }
 };
 
@@ -182,12 +192,11 @@ struct pdoa_param{
     double a;
     double ax;
     double ay;
-    double p;
 
-    pdoa_param(const double& _pdoa, const double& _r, const double& _a, const double& _ax, double& _ay, double& _p):pdoa(_pdoa), r(_r), a(_a), ax(_ax), ay(_ay), p(_p)
+    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), p(0.0)
+    pdoa_param():pdoa(0.0), r(0.0), a(0.0), ax(0.0), ay(0.0)
     {}
 };
 

net-service.cpp

@@ -489,9 +489,8 @@ float net_service::get_pdoa(float poa[], const double& offset)
 
     float poa1 = poa[0];
     float poa2 = poa[1];
-    //float poa3 = poa[2];
 
-    float pdoa = poa2 - poa1 + PI - offset;
+    float pdoa = poa2 - poa1 - offset;
     while(pdoa >= TPI){
         pdoa -= TPI;
     }