softwarePLL.cpp

Go to the documentation of this file.

/*
====================================================================================================
File: softwarePLL.cpp
====================================================================================================
*/
#include "softwarePLL.h"
#include <iostream>
// #include <chrono>
// #include <thread>
#include <math.h>
#include <iterator>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>



const double SoftwarePLL::MaxAllowedTimeDeviation = 0.001;
const uint32_t SoftwarePLL::MaxExtrapolationCounter = 5;

// Helper class for reading csv file with test data

class CSVRow
{
public:
    std::string const& operator[](std::size_t index) const
    {
        return m_data[index];
    }
    std::size_t size() const
    {
        return m_data.size();
    }
    void readNextRow(std::istream& str)
    {
        std::string         line;
        std::getline(str, line);

        std::stringstream   lineStream(line);
        std::string         cell;

        m_data.clear();
        while(std::getline(lineStream, cell, ';'))
        {
            m_data.push_back(cell);
        }
        // This checks for a trailing comma with no data after it.
        if (!lineStream && cell.empty())
        {
            // If there was a trailing comma then add an empty element.
            m_data.push_back("");
        }
    }
private:
    std::vector<std::string>    m_data;
};

std::istream& operator>>(std::istream& str, CSVRow& data)
{
    data.readNextRow(str);
    return str;
}



bool SoftwarePLL::pushIntoFifo(double curTimeStamp, uint32_t curtick)
// update tick fifo and update clock (timestamp) fifo
{
        for (int i = 0; i < fifoSize - 1; i++)
        {
                tickFifo[i] = tickFifo[i + 1];
                clockFifo[i] = clockFifo[i + 1];
        }
        tickFifo[fifoSize - 1] = curtick; // push most recent tick and timestamp into fifo
        clockFifo[fifoSize - 1] = curTimeStamp;

        if (numberValInFifo < fifoSize)
        {
                numberValInFifo++; // remember the number of valid number in fifo
        }
        FirstTick(tickFifo[0]);
        FirstTimeStamp(clockFifo[0]);

        return(true);
}

double SoftwarePLL::extraPolateRelativeTimeStamp(uint32_t tick)
{
        uint32_t tempTick = tick;
        tempTick -= (uint32_t)(0xFFFFFFFF & FirstTick());
        double timeDiff = tempTick * this->InterpolationSlope();
        return(timeDiff);

}

int SoftwarePLL::findDiffInFifo(double diff, double tol)
{
    int numFnd = 0;
    double minAllowedDiff = (1.0 - tol) * diff;
    double maxAllowedDiff = (1.0 + tol) * diff;

    for (int i = 0; i < numberValInFifo - 1; i++)
    {
        double diffTime = this->clockFifo[i+1] - clockFifo[i];
        if ((diffTime >= minAllowedDiff) && (diffTime <= maxAllowedDiff))
        {
            numFnd++;
        }
    }

    return(numFnd);
}

bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick)
{
  double start = sec + nanoSec * 1E-9;
  bool bRet = true;

  if (false == IsInitialized())
  {
    pushIntoFifo(start, curtick);
    bool bCheck = this->updateInterpolationSlope();
    if (bCheck)
    {
      IsInitialized(true);
    }
  }

  if (IsInitialized() == false)
  {
    return(false);
  }

  double relTimeStamp = extraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen
  double cmpTimeStamp = start - this->FirstTimeStamp();

  bool timeStampVerified = false;
  if (nearSameTimeStamp(relTimeStamp, cmpTimeStamp) == true)// if timestamp matches prediction update FIFO
  {
    timeStampVerified = true;
    pushIntoFifo(start, curtick);
    updateInterpolationSlope();
    ExtrapolationDivergenceCounter(0);
  }

  if (timeStampVerified == false)
  {
    // BEGIN HANDLING Extrapolation divergence
    uint32_t tmp = ExtrapolationDivergenceCounter();
    tmp++;
    ExtrapolationDivergenceCounter(tmp);
    if (ExtrapolationDivergenceCounter() >= SoftwarePLL::MaxExtrapolationCounter)
    {
      IsInitialized(false); // reset FIFO - maybe happened due to abrupt change of time base
    }
    // END HANDLING Extrapolation divergence
  }

  return(true);
}
//TODO Kommentare
bool SoftwarePLL::getCorrectedTimeStamp(uint32_t& sec, uint32_t& nanoSec, uint32_t curtick)
{
  if (IsInitialized() == false)
  {
    return(false);
  }

        double relTimeStamp = extraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen
        double corrTime = relTimeStamp + this->FirstTimeStamp();
        sec = (uint32_t)corrTime;
        double frac = corrTime - sec;
        nanoSec = (uint32_t)(1E9 * frac);
        return(true);
}

bool SoftwarePLL::nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2)
{
        double dTAbs = fabs(relTimeStamp1 - relTimeStamp2);
        if (dTAbs < AllowedTimeDeviation())
        {
                return(true);
        }
        else
        {
                return(false);
        }
}

bool SoftwarePLL::updateInterpolationSlope() // fifo already updated
{

        if (numberValInFifo < fifoSize)
        {
                return(false);
        }
        std::vector<uint64_t> tickFifoUnwrap;
        std::vector<double> clockFifoUnwrap;
        clockFifoUnwrap.resize(fifoSize);
        tickFifoUnwrap.resize(fifoSize);
        uint64_t tickOffset = 0;
        clockFifoUnwrap[0] = 0.00;
        tickFifoUnwrap[0] = 0;
        FirstTimeStamp(this->clockFifo[0]);
        FirstTick(this->tickFifo[0]);

        uint64_t tickDivisor = 0x100000000;



        for (int i = 1; i < fifoSize; i++)  // typical 643 for 20ms -> round about 32150 --> near to 32768 standard clock in many watches
        {
                if (tickFifo[i] < tickFifo[i - 1]) // Overflow
                {
                        tickOffset += tickDivisor;
                }
                tickFifoUnwrap[i] = tickOffset + tickFifo[i] - FirstTick();
                clockFifoUnwrap[i] = (this->clockFifo[i] - FirstTimeStamp());
        }

        double sum_xy = 0.0;
        double  sum_x = 0.0;
        double sum_y = 0.0;
        double sum_xx = 0.0;
        for (int i = 0; i < fifoSize; i++)
        {
                sum_xy += tickFifoUnwrap[i] * clockFifoUnwrap[i];
                sum_x += tickFifoUnwrap[i];
                sum_y += clockFifoUnwrap[i];
                sum_xx += tickFifoUnwrap[i] * tickFifoUnwrap[i];
        }

        // calculate slope of regression line, interception is 0 by construction
        double m = (fifoSize * sum_xy - sum_x * sum_y) / (fifoSize * sum_xx - sum_x*sum_x);

        int matchCnt = 0;
        for (int i = 0; i < fifoSize; i++)
        {
                double yesti = m * tickFifoUnwrap[i];
                if (this->nearSameTimeStamp(yesti, clockFifoUnwrap[i]))
                {
                        matchCnt++;
                }
        }

        bool retVal = false;
        if (matchCnt == fifoSize)
        {
                InterpolationSlope(m);
                retVal = true;
        }

        return(retVal);
}

#if 0
bool SoftwarePLL::getDemoFileData(std::string fileName, std::vector<uint32_t>& tickVec,std::vector<uint32_t>& secVec, std::vector<uint32_t>& nanoSecVec )
{
    std::ifstream file(fileName);

    CSVRow row;
    tickVec.clear();
    secVec.clear();
    nanoSecVec.clear();
    int lineCnt = 0;
    while (file >> row) {
        if (lineCnt > 0)
                {
                uint32_t tickVal = (uint32_t)std::stoi(row[0]);
                        uint32_t secVal = (uint32_t)std::stoi(row[1]);
                        uint32_t nanoSecVal = (uint32_t)std::stoi(row[2]);
                        tickVec.push_back(tickVal);
                        secVec.push_back(secVal);
                        nanoSecVec.push_back(nanoSecVal);
                }
        lineCnt++;
    }
    if (lineCnt <= 1)
        return false;
    else
        return true;
}
#endif

//TODO update testbed
void SoftwarePLL::testbed()
{
    std::cout << "Running testbed for SofwarePLL" << std::endl;
    uint32_t curtick = 0;
    int cnt = 0;

    SoftwarePLL testPll;
    uint32_t sec = 9999;
    uint32_t nanoSec = 0;
    double tickPerSec = 1E6;
    uint32_t tickInc = 1000;
    int maxLoop = 20;

        std::vector<uint32_t> tickVec;
        std::vector<uint32_t> secVec;
        std::vector<uint32_t> nanoSecVec;
    bool bRet = false;

    bool testWithDataFile = true;
    if (testWithDataFile)
        {
                // commented for trusty bRet = testPll.getDemoFileData("/home/rosuser/dumpimu3.csv", tickVec, secVec, nanoSecVec);
                maxLoop = tickVec.size();
        }

    for (int i = 0; i < maxLoop; i++)
    {
        if (testWithDataFile)
                {
                curtick = tickVec[i];
                sec = secVec[i];
                nanoSec = nanoSecVec[i];
                }
                else
                {
        cnt++;
        curtick += tickInc; // increment tick counter
        double deltaT = tickInc / tickPerSec;
        nanoSec += (int)(deltaT * 1E9);
        if (nanoSec >= 1E9)
        {
            nanoSec = 0;
            sec++;
        }
        if (cnt >= 8)
        {
            sec++;
        }
                }
        printf("Before correction: %3d.%09d\n", sec, nanoSec);
                uint32_t org_sec = sec;
                uint32_t org_nanoSec = nanoSec;

         bool bRet = testPll.getCorrectedTimeStamp(sec, nanoSec, curtick);

        bool corrected = false;
        if ((nanoSec != org_nanoSec) || (sec != org_sec))
                {
                corrected =true;
                }
        printf("After correction : %3d.%09d %s %s\n", sec, nanoSec, bRet ? "OK     " : "DISMISS", corrected ? "MODI." : "OK   ");
    }

    return;
}



#ifdef softwarePLL_MAINTEST
int main(int argc, char **argv)
{
        printf("Test for softwarePLL-Class\n");
        printf("\n");
        SoftwarePLL::testbed();
}
#endif



/* 
Example CMakeLists.txt to generate test-binary-file for testing this class
--- CUT ---
#
#
# softwarePLL
#
#
cmake_minimum_required(VERSION 2.8)
cmake_policy(SET CMP0015 NEW)
project( softwarePLL )
#
#
add_definitions(-D${PROJECT_NAME}_MAINTEST)

MESSAGE( STATUS "CMKAKE for " ${PROJECT_NAME} )

include_directories( inc)
file( GLOB LIB_SOURCES src/ *.cpp )

if(WIN32)
else()
set(CMAKE_CXX_STANDARD 11)
endif()

add_executable( ${PROJECT_NAME} ${LIB_SOURCES} inc/${PROJECT_NAME}.h)
target_link_libraries( ${PROJECT_NAME})
--- CUT ---

*/