123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131 |
- #pragma once
- #include <sys/timeb.h>
- #include <memory>
- using namespace std;
- namespace nspLocate{
- const int PERIOD_SYNCTIME = 15000; // 分站时间同步周期,约10秒,10000毫秒
- const double INTERVAL_SYNCTIME = 200.0; // 分站时间同步间隔,200毫秒
- const unsigned long long PERIOD_MAX_VALUE = 0xFFFFFFFFFF;
- const double PICOSECOND_COEFFICIENT = (15.65 * 1E-9);
- class LocateRecord
- {
- private:
- // 分站接收标识卡的时间,为线性插值后的时间,多个基站中,以参与算法最小的时间戳为准
- unsigned long long locateTime;
- // 本次记录时间的分站号
- int readerId ;
- // 本次记录分站时间同步序号
- int syncNum;
- // 卡时间戳
- int cardTimestamp;
- // 上位机接收时间,相当于time(NULL)
- unsigned long long receiveTime;
- //// 当前坐标
- //double x;
- //double y;
- //// 当前速度
- //double vx;
- //double vy;
- //double v; // 速度
- //double a; // 加速度
- //// 地图比例,用来将像素转为米
- //double scale;
- int deltaT;
- // 当前时间,1970年1月1日到现在的毫秒数
- unsigned long long millionSeconds(){
- struct timeb t1;
- ftime(&t1);
- return t1.millitm; // 毫秒数
- //t1.time // 秒数, 同time(NULL);
- }
- public:
- LocateRecord(void){}
- LocateRecord(unsigned long long revTime, int rid, int sn, int ct){
- this->locateTime = revTime;
- this->readerId = rid;
- this->syncNum = sn;
- this->cardTimestamp = ct;
- this->receiveTime = millionSeconds();
- deltaT = 0;
- }
- ~LocateRecord(void){}
- //// 根据上次的时间、位置计算速度、加速度
- //void getV(std::shared_ptr<LocateRecord> &lr){
- // double deltaT = getDeltaT(lr) / 1000.0; // 转为秒
- // // 时间差异常,应赋予默认值
- // if(0 == deltaT)
- // {
- // vx = lr->VX();
- // vy = lr->VY();
- // v = lr->V();
- // a = lr->A();
- // }
- // else
- // {
- // // 速度,加速度
- // vx = (x - lr->X()) * scale / deltaT;
- // vy = (y - lr->Y()) * scale / deltaT;
- // v = sqrt(pow(vx, 2) + pow(vy, 2));
- // double avx = (vx - lr->VX()) / deltaT;
- // double avy = (vy - lr->VY()) / deltaT;
- // a = sqrt(pow(avx,2) + pow(avy,2));
- // }
- //}
- //void SetReceiveTime(){ receiveTime = millionSeconds();}
- //void SetScale(double val){ scale = val;}
- unsigned long long ReceiveTime() const { return receiveTime; }
- unsigned long long LocateTime() const { return locateTime;}
- int SyncNum() const { return syncNum;}
- int CardTimestamp() const { return cardTimestamp;}
- int ReaderId() const { return readerId ;}
- int DeltaT() const { return deltaT; } // 毫秒
- double DeltaTSecond() const {return (double)deltaT/1000.0;}; // 秒
- // 获取两次定位的时间间隔,返回毫秒数
- void getDeltaT(std::shared_ptr<LocateRecord> &lr)
- {
- if(nullptr == lr)
- {
- return ;
- }
- unsigned long long ret = receiveTime - lr->ReceiveTime();
- // 跨时间同步周期了,以上位机时间receiveTime为准
- if( ret > PERIOD_SYNCTIME)
- {
- deltaT = (int)ret;
- }
- // 时间同步序号差500以上,认为root异常,时间同步序号重置,以上位机时间receiveTime为准
- else if((syncNum - lr->SyncNum())*INTERVAL_SYNCTIME > PERIOD_SYNCTIME) // 500
- {
- deltaT = (int)ret;
- }
- // 分站接收时间跨周期,并且在一个周期以内,以分站接收时间为主
- else if(locateTime < lr->LocateTime())
- {
- deltaT = (int)(((locateTime + PERIOD_MAX_VALUE - lr->LocateTime()) & PERIOD_MAX_VALUE) * PICOSECOND_COEFFICIENT);
- }
- // 在同一个周期内,以分站接收时间为准
- else
- {
- deltaT = (int)(((locateTime - lr->LocateTime()) & PERIOD_MAX_VALUE) * PICOSECOND_COEFFICIENT);
- }
- }
- //double VX() const { return vx;}
- //double VY() const { return vy;}
- //double V() const { return v;}
- //double A() const { return a;}
- //double X() const { return x;}
- //double Y() const { return y;}
- };
- }
|