server-l-zmg/loc_message.h

#ifndef __LOC_MESSAGE__HPP
#define __LOC_MESSAGE__HPP
#include "ant.h"

struct loc_message
{
    std::shared_ptr<site> m_sit;
    uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
    uint64_t m_loc_time;
	uint64_t m_time_tmp;
    uint32_t m_card_id;
    int32_t	 m_card_ct;
    int8_t   m_card_type;
    int8_t   m_ant_id;
    int16_t  m_rav;
	float    m_acc;
    uint16_t m_sync_ct;
    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];        // 三天线的相位值
    double m_angle;         // 天线角度

	loc_message()
	 :m_num_ticks(0)
	{
	}
	int tool_index()const
	{
		return m_sit->index();
	}

    loc_message(std::shared_ptr<site> s,uint64_t num_ticks,uint64_t timestamp,uint64_t time_tmp,
                uint32_t cardid,int32_t ct,int8_t type,int8_t antid,
                int16_t rav,float acc,uint16_t sync_ct,uint16_t rssi,uint16_t batstatus)
        :m_sit(s)
        ,m_num_ticks(num_ticks)
        ,m_loc_time(timestamp)
		,m_time_tmp(time_tmp)
        ,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(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,float 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,uint64_t time_tmp,
                uint32_t cardid,int32_t ct,int8_t type,int8_t antid,
                int16_t rav,float 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_time_tmp(time_tmp)
        ,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;
    }

    float get_pdoa(const int i)
    {
        if(i>1){
            return 10.0;
        }
        float poa1 = m_poa[i];
        float poa2 = m_poa[i+1];

        float pdoa = poa2 - poa1 - m_sit->m_pdoa_offset;
        while(pdoa >= TPI){
            pdoa -= TPI;
        }

        
        while(pdoa < 0){
            pdoa += TPI;
        }
        
        pdoa -= PI;

        /*pdoa -= m_sit->m_pdoa_offset;

        if(pdoa < -1.0*PI){
            pdoa += TPI;
        }

        if(pdoa > PI){
            pdoa -= TPI;
        }*/

        return pdoa;
    }
};

struct pdoa_message: public point{
    double angle;
    double distance;
    double x_ant;
    double y_ant;
    double pdoa;
    double relative_angle;

    pdoa_message(const double& _x, const double& _y, const double& _angle, const double& _distance, const double& _ax, const double& _ay, const double& _pdoa, const double& _relative_angle):point(_x, _y), angle(_angle), distance(_distance), x_ant(_ax), y_ant(_ay), pdoa(_pdoa), relative_angle(_relative_angle)
    {}

    pdoa_message():point(0.0, 0.0),angle(0.0), distance(0.0), x_ant(0.0), y_ant(0.0), pdoa(0.0), relative_angle(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)
    {}
};

#endif