pdoa 2d v1

line_fit.cpp

@@ -2,6 +2,7 @@
 #include <math.h>
 #include "line_fit.h"
 #include "line.h"
+
 fit_item::fit_item()
 	:x(0)
 	,y(0)
@@ -46,11 +47,7 @@ void fit_batch::add_tool(double max_x_span,int min_point,int max_point)
 
 void fit_batch::log()
 {
-//	printf("xo=%lf,yo=%.3lf\n",xo,yo);
-
-	//for(int i=0,len=size();i<len;i++)
-	//	at(i).log();
-		printf("epos-bpos:%d, xo:%f, yo:%f\n",epos - bpos, xo, yo);
+	printf("epos-bpos:%d, xo:%f, yo:%f\n",epos - bpos, xo, yo);
 	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
 		f->log();
 	});
@@ -85,7 +82,6 @@ double fit_batch::add(double x,double y)
 			f->fit();
 	});
 	
-
 	return 0;
 }
 #if 0
@@ -107,7 +103,6 @@ void fit_batch::replace(double x,double y)
 }
 #endif
 
-
 double fit_batch::x(int index)const
 {
 	return (*this)(index).x + xo;
@@ -241,118 +236,4 @@ bool card_fit::fit()
 
 	return true;
 }
-#ifdef _LINE_FIT_TEST
-#include "line_fit.h"
-int a(int x)
-{
-    return x*2+1;
-}
-double y(int x)
-{
-    return 0.25*x+13.2;
-}
-int main()
-{
-    fit_batch f;
-    f.add_tool(100,5);
-    //y=2x+1;
-    //f.add(30,a(30));
-    for(int i=0;i<15;i++)
-    {
-        f.add(i*10,a(i*10));
-    }
-    f.add(20,45);
-    f.log(); 
-    f.reset_data();
-    f.add(20,45);
-    for(int i=14;i>=0;i--)
-        f.add(i,a(i));
-    f.log(); 
-    printf("%f",f[0].testk(30,a(30)+20));
-    f.reset_data();
-    f.add(6721.744000,1273.000000);
-    f.add(6721.742000,1273.000000);
-    f.log();
-    f.add(6721.592000,1273.000000);
-    f.add(6721.545000,1273.000000);
-    f.add(6721.259000,1273.000000);
-    f.add(6721.395000,1273.000000);
-    f.add(6721.378000,1273.000000);
-    f.add(6721.081000,1273.000000);
-    f.add(6721.043000,1273.000000);
-    f.add(6720.991000,1273.000000);
-    f.add(6720.485000,1273.000000);
-    f.add(6720.633000,1273.000000);
-    f.add(6720.562000,1273.000000);
-    f.add(6720.443000,1273.000000);
-    f.add(6720.145000,1273.000000);
-    f.add(6719.575000,1273.000000);
-    f.add(6710.775000,1273.000000);
-    f.add(6710.384000,1273.000000);
-    f.add(6710.030000,1273.000000);
-    f.add(6709.847000,1273.000000);
-    f.add(6710.098000,1273.000000);
-    f.add(6706.457000,1273.000000);
-    f.add(6706.419000,1273.000000);
-    f.add(6706.608000,1273.000000);
-    f.add(6706.513000,1273.000000);
-    f.add(6706.438000,1273.000000);
-    f.add(6706.239000,1273.000000);
-    f.add(6706.048000,1273.000000);
-    f.add(6705.962000,1273.000000);
-    f.add(6705.707000,1273.000000);
-    f.add(6705.839000,1273.000000);
-    f.add(6705.594000,1273.000000);
-    f.add(6705.417000,1273.000000);
-    f.add(6705.136000,1273.000000);
-    f.add(6705.608000,1273.000000);
-    f.add(6705.505000,1273.000000);
-    f.add(6705.422000,1273.000000);
-    f.add(6705.640000,1273.000000);
-    f.add(6705.280000,1273.000000);
-    f.log();
-    zlist<int,128> m_test;
-    for(int i= 1 ;i<=20;i++)
-    {
-        m_test.grow()=i;
-        if(i%5==0) 
-        {
-            int x=0;
-            for(int j=0;j<5;j++)
-            {
-                x+=m_test(j);
-            }
-            m_test.rskip(5);
-            m_test.grow()=x;
-        }
-    }
-    for(int i=0;i<m_test.size();i++)
-      printf("%d\n",m_test(i));
-
-    line_r l(point(1,y(1)),point(13,y(13)));
-    if(l.contain(point(4,y(4))))printf("contain:%d,%f\n",4,y(4));
-    if(l.contain(point(5,y(5))))printf("contain:%d,%f\n",5,y(5));
-    if(l.contain(point(-5,y(-5))))printf("contain:%d,%f\n",-5,y(-5));
-    point p=l.projection(point(17,y(17)+1.5));
-    printf("projection:%f,%f\n",p.x,p.y);
-    p=l.projection(point(-17,y(-17)+1.5));
-    printf("projection:%f,%f\n",p.x,p.y);
-    printf("line:%s\n",l.to_string().c_str());
-    line_r ll=l;
-    printf("line:%s\n",ll.to_string().c_str());
-    bool b=l.is_same_direction(ll);
-    if(b)printf("%s [is same direction]%s\n",l.to_string().c_str(),ll.to_string().c_str());
-    line_r l2(point(4,y(4)),point(7,y(7)));
-    b=l.is_same_direction(l2);
-    if(b)printf("%s [is same direction]%s\n",l.to_string().c_str(),l2.to_string().c_str());
-    line_r l3(point(19,y(19)),point(31,y(31)));
-    b=l.is_same_direction(l3);
-    if(b)printf("%s [is same direction]%s\n",l.to_string().c_str(),l3.to_string().c_str());
-    line_r l4(point(19,y(19)),point(4,y(4)));
-    b=l.is_same_direction(l4);
-    if(!b)printf("%s [is not same direction]%s\n",l.to_string().c_str(),l4.to_string().c_str());
-
-    return 0;
-}
-#endif
 

line_fit.h

@@ -108,14 +108,10 @@ struct card_fit_stop:card_fit
 
     virtual void add_last()
 	{
-		
-	
 	}
 
 	virtual bool fit()
 	{
-		
-
 		return false;
 	}
 };

loc_message.h

@@ -135,18 +135,42 @@ struct loc_message
 
         return *this;
     }
+
+    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];
+
+	float pdoa = poa2 - poa1 + PI - m_sit->m_pdoa_offset;
+	while(pdoa >= TPI){
+		pdoa -= TPI;
+	}
+
+	while(pdoa < 0){
+		pdoa += TPI;
+	}
+
+	pdoa -= PI;
+
+	return pdoa;
+    }
 };
 
-struct pdoa_message: point{
+struct pdoa_message: public point{
     double angle;
     double distance;
     double x_ant;
     double y_ant;
+    double pdoa;
 
-    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(const double& _x, const double& _y, const double& _angle, const double& _distance, const double& _ax, const double& _ay, const double& _pdoa):point(_x, _y), angle(_angle), distance(_distance), x_ant(_ax), y_ant(_ay), pdoa(_pdoa)
     {}
 
-    pdoa_message():point(0.0, 0.0),angle(0.0), distance(0.0), x_ant(0.0), y_ant(0.0)
+    pdoa_message():point(0.0, 0.0),angle(0.0), distance(0.0), x_ant(0.0), y_ant(0.0), pdoa(0.0)
     {}
 };
 
@@ -158,11 +182,12 @@ 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):pdoa(_pdoa), r(_r), a(_a), ax(_ax), ay(_ay)
+    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():pdoa(0.0), r(0.0), a(0.0), ax(0.0), ay(0.0)
+    pdoa_param():pdoa(0.0), r(0.0), a(0.0), ax(0.0), ay(0.0), p(0.0)
     {}
 };
 

message.cpp

@@ -338,12 +338,13 @@ void message_pdoa_locinfo::load(zistream& is)
     zero_this();
 
     uint8_t b;
-    // 高4位为卡电池信息，低4位为卡类型
+    // 1字节，低4位为卡类型
     is>>b;
 
     m_card_type = b&0x0F;
     //m_batty_status = (b>>4)&0xFF;
 
+    // 1字节
     is>>b;
     m_batty_status = b;
 
@@ -373,7 +374,7 @@ void message_pdoa_locinfo::load(zistream& is)
     }
 
     //信号电平值，tof测距结果
-    uint8_t rssi = 0;
+    int8_t rssi = 0;
     uint32_t tof = 0;
     uint8_t high_tof = 0;
     is>>rssi>>high_tof>>tof;
@@ -390,5 +391,5 @@ void message_pdoa_locinfo::load(zistream& is)
         m_poa[i] = (float)((((short)_poa)*1.0)/1000.0);
     }
 
-    logn_info(3,"[pdoa] card_type=%d, card_id=%d, site=%d, ct=%d, status=%d, acc=%d, tof=%llu, ant_id=%d, rssi=%d, poa1=%.2f, poa2=%.2f, poa3=%.2f", 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]);
+    logn_info(3,"[pdoa] card_type=%d, card_id=%d, ct=%d, status=%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_acc, m_tof, m_ant_id, m_rssi, m_poa[0], m_poa[1], m_poa[2]);
 }

net-service.cpp

@@ -221,14 +221,18 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
                         task* t = task::alloc<message_pdoa_locinfo>();
                         message_pdoa_locinfo& m = t->body<message_pdoa_locinfo>();
                         m.load(is);
-                        m.m_site_id = site_ptr->m_id;
-                        m.m_time_stamp = tstamp;
-                        m.m_loc_type = LDT_PDOA;
-                        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);
+                        float pdoa = 10.0;
+                        float dist = m.m_tof*15.65*2.996*1e-4/site_ptr->m_scale;
+                        if(fabs(m.m_poa[0]) < 10.0 && fabs(m.m_poa[1]) < 10.0){
+                            m.m_site_id = site_id;  
+                            m.m_time_stamp = tstamp;
+                            m.m_loc_type = LDT_PDOA;
+                            t->m_cmd_code = cmd;
+                            t->m_hash_id = m.m_card_id;
+                            pdoa = get_pdoa(m.m_poa, site_ptr->m_pdoa_offset);
+                            m_loc_worker->request(t);
+                        }
+                        logn_info(3, "[pdoa] site_id=%d, card_id=%d, dist=%.3f, poa1=%.4f, poa2=%.4f, poa3=%.4f, pdoa=%.4f, pdoa_offset=%.4f", site_id, m.m_card_id, dist, m.m_poa[0], m.m_poa[1], m.m_poa[2], pdoa, site_ptr->m_pdoa_offset);
                     }
                 }
                 break;
@@ -487,7 +491,7 @@ float net_service::get_pdoa(float poa[], const double& offset)
     float poa2 = poa[1];
     //float poa3 = poa[2];
 
-    float pdoa = poa2 - poa1 - offset;
+    float pdoa = poa2 - poa1 + PI - offset;
     while(pdoa >= TPI){
         pdoa -= TPI;
     }