SoftwarePLL.h

Go to the documentation of this file.

#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */
#ifndef SOFTWARE_PLL_H
#define SOFTWARE_PLL_H
 
#ifdef _MSC_VER
#pragma warning(disable : 4996)
#endif
 
#include <map>
#include <string>
#include <iostream>
#include <fstream>
#include <cstring>
#include <sstream>
#include <vector>
#include <cstdlib>
#include <iomanip>
#include <ctime>
#include <stdint.h>
 
class SoftwarePLL
{
public:
 
        static SoftwarePLL& Instance(const std::string & id = "", int fifo_length = 7);
        
        ~SoftwarePLL();
 
  bool GetCorrectedTimeStamp(uint32_t& sec, uint32_t& nanoSec, uint32_t tick);
 
  bool UpdatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick);
        
        bool IsInitialized() const { return IsInitialized_; }
 
protected:
        
        void IsInitialized(bool val) { IsInitialized_ = val; }
  
        uint64_t FirstTick() const { return FirstTick_; }
  
        void FirstTick(uint64_t val) { FirstTick_ = val; }
  
        double FirstTimeStamp() const { return FirstTimeStamp_; }
  
        void FirstTimeStamp(double val) { FirstTimeStamp_ = val; }
  
        double InterpolationSlope() const { return InterpolationSlope_; }
  
        void InterpolationSlope(double val) { InterpolationSlope_ = val; }
  
        double AllowedTimeDeviation() const { return AllowedTimeDeviation_; }
  
        void AllowedTimeDeviation(double val) { AllowedTimeDeviation_ = val; }
  
        uint32_t ExtrapolationDivergenceCounter() const { return ExtrapolationDivergenceCounter_; }
  
        void ExtrapolationDivergenceCounter(uint32_t val) { ExtrapolationDivergenceCounter_ = val; }
 
        bool PushIntoFifo(double curTimeStamp, uint32_t curtick);// update tick fifo and update clock (timestamp) fifo;
 
        double ExtraPolateRelativeTimeStamp(uint32_t tick);
 
private:
        
        int NumberValInFifo_;
        const int FifoSize_;
        static const double MaxAllowedTimeDeviation_;
        static const uint32_t MaxExtrapolationCounter_;
        std::vector<uint32_t> TickFifo_;
        std::vector<double> ClockFifo_;
        bool IsInitialized_;
        double FirstTimeStamp_;
        double AllowedTimeDeviation_;
        uint64_t FirstTick_;
        uint32_t Lastcurtick_ = 0;
        double InterpolationSlope_;
        bool NearSameTimeStamp(double relTimeStamp1, double relTimeStamp2);
        bool UpdateInterpolationSlope();
        uint32_t ExtrapolationDivergenceCounter_;
        SoftwarePLL(int fifo_length = 7) : FifoSize_(fifo_length), TickFifo_(fifo_length,0), ClockFifo_(fifo_length,0), IsInitialized_(false), FirstTimeStamp_(0), FirstTick_(0), InterpolationSlope_(0)
        {
                AllowedTimeDeviation(SoftwarePLL::MaxAllowedTimeDeviation_); // 1 ms
                NumberValInFifo_ = 0;
        }
        // verhindert, dass ein Objekt von ausserhalb von N erzeugt wird.
        // protected, wenn man von der Klasse noch erben moechte
        SoftwarePLL(const SoftwarePLL&); /* verhindert, dass eine weitere Instanz via
                                                                   Kopier-Konstruktor erstellt werden kann */
        SoftwarePLL & operator = (const SoftwarePLL &); //Verhindert weitere Instanz durch Kopie
        
        static std::map<std::string,SoftwarePLL*> _instances; // list of SoftwarePLL instances, mapped by id
};
 
#endif