fix pdoa algorithm

ant.cpp

@@ -456,7 +456,7 @@ void sit_list::init_site(const std::string &ids /*=""*/)
          site_ptr->m_scale = scale;
          site_ptr->create_area();
 
-         double x=0,y=0;
+         double x = 0, y = 0;
          DBRes.GetField( "x",x, Error );
          DBRes.GetField( "y",y, Error );
 

ant.h

@@ -112,11 +112,11 @@ struct site:point,std::enable_shared_from_this<site>
     //pdoa
     double m_pdoa = 100.0;          // pdoa分站当前上传相位差
     double m_last_pdoa = 100.0;      // pdoa分站上一帧上传相位差
-    double m_pdoa_offset = 0.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;
+    //float m_pdoa_diff = 0.0;        
+    std::shared_ptr<client> m_clt = nullptr;
+    std::shared_ptr<area> m_area = nullptr;
 
 	time_t m_time;
 //  time_t m_rec_time;              // 接收数据的时间

card_base.cpp

@@ -27,6 +27,15 @@ card_location_base::card_location_base(const std::string&type,uint32_t id,uint16
 {
     select_tool_manage::instance()->create_tool(type,m_sel_tool,m_smo_tool);
 	m_his_location_card.reset(new location_card(m_id,m_type,cid));
+
+    m_cb_pdoa.set_capacity(5);
+    m_cb_tof.set_capacity(5);
+    m_cb_point.set_capacity(5);
+    for(int index = 0; index < 5; ++index){
+        m_cb_pdoa.push_back(100);
+        m_cb_tof.push_back(0);
+        m_cb_point.push_back(point(0,0));
+    }
 }
 
 void card_location_base::do_status(int st)
@@ -106,10 +115,10 @@ 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 )
 {
-    auto it = lm.cbegin();
-
-    loc_point pt;
-    if(it != lm.cend()){
+    log_info("[business] card_location_base::on_location");
+    loc_point pt = m_sel_tool->select_solution(vp, lm);
+    pt.y = -pt.y;
+    /*if(it != lm.cend()){
         switch((*it).m_loc_type){
             case LDT_TOF:
     	        pt = m_sel_tool->select_solution(vp,lm);
@@ -124,7 +133,7 @@ void card_location_base::on_location(const std::vector<point>&vp,const std::vect
                 log_info("[pdoa] position's size = %d, x=%.2f, y=%.2f", vp.size(), pt.x, pt.y);
                 break;
         }
-    }
+    }*/
 
 	auto site_ptr = get_area_tool()->m_site;
 	int sid = 0;
@@ -202,7 +211,7 @@ void card_location_base::on_message(zloop<task*>* loop, message_tdoa_locinfo& lo
  * */
 void card_location_base::on_message(zloop<task*>* loop, message_pdoa_locinfo& loc, bool is_history)
 {
-    log_info("[pdoa] start calc location");
+    //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){

card_base.h

@@ -91,6 +91,7 @@ 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;
     int m_buff_size = 0;
     float m_pdoa_diff = 100.0;                  // pdoa分站当前上传相位差
     float m_last_pdoa_diff = 100.0;             // pdoa 分站上一帧数据的相位差

message.cpp

@@ -384,7 +384,7 @@ void message_pdoa_locinfo::load(zistream& is)
     is>>m_ant_id;
     --m_ant_id;
 
-    int16_t _poa = 0;
+    uint16_t _poa = 0;
     for(int i = 0; i < 3; ++i){
         is>>_poa;
         m_poa[i] = (float)((((short)_poa)*1.0)/1000.0);

net-service.cpp

@@ -223,6 +223,7 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
                         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);
+                        logn_info(3, "[pdoa] card_id=%d, poa1=%.4f, poa2=%.4f, poa3=%.4f, pdoa=%.4f, pdoa_offset=%.4f", m.m_card_id,  m.m_poa[0], m.m_poa[1], m.m_poa[2], site_ptr->m_pdoa, site_ptr->m_pdoa_offset);
                         m_loc_worker->request(t);
                     }
                 }
@@ -431,27 +432,27 @@ float net_service::get_pdoa(float poa[], const double& offset)
     float poa2 = poa[1];
     float poa3 = poa[2];
 
-    float pdoa_raw = poa2 - poa1 + PI;
-    while(pdoa_raw >= TPI){
-        pdoa_raw -= TPI;
+    float pdoa = poa2 - poa1 - offset;
+    while(pdoa >= TPI){
+        pdoa -= TPI;
     }
 
-    while(pdoa_raw < 0){
-        pdoa_raw += TPI;
+    while(pdoa < 0){
+        pdoa += TPI;
     }
 
-    pdoa_raw -= PI;
+    pdoa -= PI;
 
     //90 adjust
-    float pdoa = pdoa_raw - offset;
+    /*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);
+    //logn_info(3, "[pdoa] poa1=%.4f, poa2=%.4f, poa3=%.4f, pdoa=%.4f", poa1, poa2, poa3, pdoa);
 
     return pdoa;
 }

test.cpp

@@ -1,17 +1,13 @@
 #include <unistd.h>
 #include <limits.h>
 #include <stdlib.h>
-
 #include <stdint.h>
-
 #include <iostream>
 #include <sstream>
 #include <vector>
 #include <line.h>
-
 #include "tool_time.h"
 
-
 #define log_error printf
 #define log_info printf
 struct path