Simulation.cpp

Go to the documentation of this file.

/*
    Copyright (C) 2010-2012
    Johannes Feuser <feuser@uni-bremen.de>
    This file is part of the openETCS library.

    The openETCS library is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    The openETCS library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with the openETCS library.  If not, see <http://www.gnu.org/licenses/>.
*/

/*!
 *  \author Johannes Feuser <feuser@uni-bremen.de>
 *  \brief classes for simulative usage
 */

#include "Simulation.h"




namespace oETCS {

namespace DF {

namespace PS {

namespace SIM {


COdometer::COdometer(const double& dInitialPosition, const double& dInitialVelocity)  throw()  
: ::oETCS::DF::PS::COdometer(nullptr),
 m_dVelocity(dInitialVelocity),
 m_dPosition(dInitialPosition)
{
  // Bouml preserved body begin 000D6482
    // set last calculation time point as now
    //m_LastCalculation = ::std::chrono::high_resolution_clock::now();
  // Bouml preserved body end 000D6482

} // COdometer::COdometer()  throw() 




COdometer::~COdometer()  throw()  
{
  // Bouml preserved body begin 000D6502
    // empty destructor
  // Bouml preserved body end 000D6502

} // COdometer::~COdometer()  throw() 




double COdometer::GetAbsolutePosition()   
{
  // Bouml preserved body begin 000D6582
    ::std::chrono::duration< long long, ::std::chrono::system_clock::duration::period >      Duration;
    
    
    
    
    // lock mutex
    m_Mutex.lock();
    
    // calculate duration with current velocity
    Duration = ::std::chrono::system_clock::now() - m_LastCalculation;
    
    // calculate new position by integration (simplified by multiplication)
    m_dPosition += m_dVelocity / 3.6 * double(Duration.count()) * (double(::std::chrono::system_clock::duration::period::num) / double(::std::chrono::system_clock::duration::period::den)); 
    
    // set last calculation to now
    m_LastCalculation = ::std::chrono::system_clock::now();
    
    // unlock mutex
    m_Mutex.unlock();
    
    // return new position
    return (m_dPosition);
  // Bouml preserved body end 000D6582

} // double COdometer::GetAbsolutePosition()  




double COdometer::GetVelocity()   
{
  // Bouml preserved body begin 000D6602
    double      dVelocity(0.0);
    
    
    
    // lock mutex
    m_Mutex.lock();
    
    // get velocity from member
    dVelocity = m_dVelocity;
    
    // unlock mutex
    m_Mutex.unlock();
    
    // return current velocity
    return (dVelocity);
  // Bouml preserved body end 000D6602

} // double COdometer::GetVelocity()  




void COdometer::SetVelocity(const double& dVelocity)  throw()  
{
  // Bouml preserved body begin 000D8202
    ::std::chrono::duration< long long, ::std::chrono::system_clock::duration::period >      Duration;
    
    
    
    
    // lock mutex
    m_Mutex.lock();
    
    // calculate duration with old velocity
    Duration = ::std::chrono::system_clock::now() - m_LastCalculation;
    
    // calculate new position by integration (simplified by multiplication)
    m_dPosition += m_dVelocity / 3.6 * double(Duration.count()) * (double(::std::chrono::system_clock::duration::period::num) / double(::std::chrono::system_clock::duration::period::den)); 
    
    // set new velocity in member
    m_dVelocity = dVelocity;
    
    // set last calculation to now
    m_LastCalculation = ::std::chrono::system_clock::now();
    
    // unlock mutex
    m_Mutex.unlock();
  // Bouml preserved body end 000D8202

} // void COdometer::SetVelocity()  throw() 




void COdometer::ResetPosition(const double& dPosition)  throw()  
{
  // Bouml preserved body begin 000D8282
    // lock mutex
    m_Mutex.lock();
    
    // set new position from parameter
    m_dPosition = dPosition;
    
    // set last calculation to now
    m_LastCalculation = ::std::chrono::system_clock::now();
    
    // unlock mutex
    m_Mutex.unlock();
  // Bouml preserved body end 000D8282

} // void COdometer::ResetPosition()  throw() 




CServiceBrake::CServiceBrake(::oETCS::DF::PS::SIM::COdometer * const pOdometer, ::oETCS::DF::PS::SIM::CEmergencyBrake * const pEmergencyBrake, const double & dIntensity, const double & dMass, const double & dAdhesion)  throw()  
: ::oETCS::DF::PS::CServiceBrake(nullptr),
 m_bCalculate(false),
 m_dIntensity(dIntensity),
 m_dMass(dMass),
 m_dAdhesion(dAdhesion),
 m_pOdometer(pOdometer),
 m_pEmergencyBrake(pEmergencyBrake),
 m_pPhysicalCalculation(nullptr)
{
  // Bouml preserved body begin 000D8302
    // check, if pointer to odometer is valid
    if (m_pOdometer != nullptr)
    {
        try
        {
            // set calculate flag for thread to true
            m_bCalculate = true;
            
            // try to create new thread object with thread method
            m_pPhysicalCalculation = new ::std::thread(&::oETCS::DF::PS::SIM::CServiceBrake::OdometerThread, this);
            
        } // try
        catch (const ::std::bad_alloc& Exception)
        {
            // print error message to stderr
            ::std::cerr << "In file " << __FILE__ << " at line " << __LINE__ << ": creation of thread for physical calculation failed" << ::std::endl;
            
            // reset calculation flag
            m_bCalculate = false;
            
        } // catch (const ::std::bad_alloc& Exception)
        
    } // if (m_pOdometer != nullptr)
  // Bouml preserved body end 000D8302

} //  CServiceBrake::CServiceBrake()  throw() 




CServiceBrake::~CServiceBrake()  throw()  
{
  // Bouml preserved body begin 000D8382
    // check, if calculation thread is executed
    if (m_bCalculate)
    {
        // set calculation flag to false to notify thread
        m_bCalculate = false;
        
        // check, if thread is joinable
        if (m_pPhysicalCalculation->joinable())
        {
            // wait for thread to terminate
            m_pPhysicalCalculation->join();

        } // if (m_pPhysicalCalculation->joinable())
        
    } // if (m_bCalculate)
  // Bouml preserved body end 000D8382

} // CServiceBrake::~CServiceBrake()  throw() 




double CServiceBrake::GetIntensity()   
{
  // Bouml preserved body begin 000D8482
    double        dIntensity(0.0);
    
    
    
    // lock mutex
    m_Mutex.lock();
    
    // copy value from member
    dIntensity = m_dIntensity;
    
    // unlock mutex
    m_Mutex.unlock();
    
    // return flag
    return (dIntensity);
  // Bouml preserved body end 000D8482

} // double CServiceBrake::GetIntensity()  




void CServiceBrake::SetIntensity(const double& dIntensity)   
{
  // Bouml preserved body begin 000D8402
    // lock mutex
    m_Mutex.lock();
    
    // check, if value is in accepted range (0 - 100)
    if (dIntensity < 0.0)
    {
        // set local intensity to 0.0
        m_dIntensity = 0.0;
        
    } // if (dIntensity < 0.0)
    else if (dIntensity > 100.0)
    {
        // set local intensity to 100.0
        m_dIntensity = 100.0;
        
    } // else if (dIntensity > 100.0)
    else
    {
        // copy value from parameter to member
        m_dIntensity = dIntensity;
        
    } // else
    
    // unlock mutex
    m_Mutex.unlock();
  // Bouml preserved body end 000D8402

} // void CServiceBrake::SetIntensity()  




void CServiceBrake::SetMass(const double& dMass)  throw()  
{
  // Bouml preserved body begin 000D8602
    // lock mutex
    m_Mutex.lock();

    // check, if value is in accepted range (> 0)
    if (dMass < 0.0)
    {
        // set local value to 0.0
        m_dMass = 0.0;
        
    } // if (dMass < 0.0)
    else
    {
        // copy value from parameter to member
        m_dMass = dMass;
        
    } // else

    // unlock mutex
    m_Mutex.unlock();
  // Bouml preserved body end 000D8602

} // void CServiceBrake::SetMass()  throw() 




void CServiceBrake::SetAdhesion(const double& dAdhesion)  throw()  
{
  // Bouml preserved body begin 000D8682
    // lock mutex
    m_Mutex.lock();

    // check, if value is in accepted range (0...1)
    if (dAdhesion < 0.0)
    {
        // set local value to 0.0
        m_dAdhesion = 0.0;

    } // if (dAdhesion < 0.0)
    else if (dAdhesion > 1.0)
    {
        // set local value to 1.0
        m_dAdhesion = 1.0;
        
    } // else if (dAdhesion > 1.0)
    else
    {
        // copy value from parameter to member
        m_dAdhesion = dAdhesion;

    } // else

    // unlock mutex
    m_Mutex.unlock();
  // Bouml preserved body end 000D8682

} // void CServiceBrake::SetAdhesion()  throw() 




void CServiceBrake::OdometerThread()  throw()  
{
  // Bouml preserved body begin 000D8502
    long double                                             dIntensity(0.0);
    long double                                             dAccelration(0.0);
    long double                                             dCurrentVelocity(0.0);
    long double                                             dNewVelocity(0.0);
    const long double                                       FRICTION_COEFICIENT(0.25);
    const long double                                       GRAVITATION_ACCELERATION(9.81);
    ::std::chrono::high_resolution_clock::time_point        StartTime;
    ::std::chrono::high_resolution_clock::duration          SleepTime;
    const ::std::chrono::high_resolution_clock::duration    SAMPLE_TIME(::std::chrono::microseconds(__DATA_FLOW_SAMPLE_TIME__));
    
    
    
    
    // measure start time once
    StartTime = ::std::chrono::high_resolution_clock::now();
    
    // execute thread while flag is true
    while (m_bCalculate)
    {
        // get current velocity from odometer
        dCurrentVelocity = m_pOdometer->GetVelocity();
        
        // lock mutex
        m_Mutex.lock();
        
        // set intensity (take emergency brake into account, if available)
        dIntensity  = (m_pEmergencyBrake != nullptr && m_pEmergencyBrake->GetActivation()) ? 120.0 : m_dIntensity;
        
        // calculate negative acceleration in [km/h²] (with linear adaptation for mass) 
        dAccelration = ((dIntensity == 0.0 || dCurrentVelocity == 0.0) ? 0.0 : FRICTION_COEFICIENT * GRAVITATION_ACCELERATION * dIntensity / 100.0 * m_dAdhesion * (m_dMass / 10000.0 ) * 12960.0);
        
        // calculate sign of acceleration
        dAccelration = dCurrentVelocity > 0 ? dAccelration : dAccelration * -1.0;
        
        // unlock mutex
        m_Mutex.unlock();
        
        // calculate new velocity with negative acceleration and sample time
        dNewVelocity = (dAccelration == 0.0 ? dCurrentVelocity : dCurrentVelocity - dAccelration * (__DATA_FLOW_SAMPLE_TIME__ * 1e-6) / 3600.0 );
        
        // DEBUG
        //::std::cout << "CServiceBrake::OdometerThread(): deaccelration: " << dAccelration << " intensity: " << dIntensity << " current v:" << dCurrentVelocity << " new v:" << dNewVelocity << ::std::endl;
        
        // check, if new velocity has changed sign
        if ((dNewVelocity < 0.0L && dCurrentVelocity >= 0.0L) || (dNewVelocity > 0.0L && dCurrentVelocity <= 0.0L))
        {
            // set velocity to 0.0 (train stopped)
            dNewVelocity = 0.0L;
            
        } // if ((dNewVelocity < 0.0L && dCurrentVelocity >= 0.0L) || (dNewVelocity > 0.0L && dCurrentVelocity <= 0.0L))
        
        // set new velocity in odometer
        m_pOdometer->SetVelocity(dNewVelocity);
        
        // compute sleep time by execution time until now
        SleepTime = SAMPLE_TIME - (::std::chrono::high_resolution_clock::now() - StartTime);

        // check, if sample time was no exceeded
        if (SleepTime.count() >= 0 )
        {
            // put thread to sleep
            ::std::this_thread::sleep_for(SleepTime);

        } // if (SleepTime.count() >= 0 )
        else
        {
            // print warning to stderr
            ::std::cerr << "In file " << __FILE__ << " in line " << __LINE__ << ": sample time exceeded in thread" << std::endl;

        } // else

        // calculate next start time point to avoid global drifts
        StartTime += SAMPLE_TIME;
        
    } // while (m_bCalculate)
  // Bouml preserved body end 000D8502

} // void CServiceBrake::OdometerThread()  throw() 




CEmergencyBrake::CEmergencyBrake(const bool & bActive)  throw()  
: ::oETCS::DF::PS::CEmergencyBrake(nullptr),
 m_bActive(bActive)
{
  // Bouml preserved body begin 000DA582
    // empty constructor
  // Bouml preserved body end 000DA582

} //  CEmergencyBrake::CEmergencyBrake()  throw() 




CEmergencyBrake::~CEmergencyBrake()  throw()  
{
  // Bouml preserved body begin 000DA602
    // empty destructor
  // Bouml preserved body end 000DA602

} // CEmergencyBrake::~CEmergencyBrake()  throw() 




bool CEmergencyBrake::GetActivation()   
{
  // Bouml preserved body begin 000DA702
    bool        bActive(false);
    
    
    
    // lock mutex
    m_Mutex.lock();
    
    // get value from member
    bActive = m_bActive;
    
    // unlock mutex
    m_Mutex.unlock();
    
    // return flag
    return (bActive);
  // Bouml preserved body end 000DA702

} // bool CEmergencyBrake::GetActivation()  




void CEmergencyBrake::SetActivation(bool bActivated)   
{
  // Bouml preserved body begin 000DA802
    #ifdef __OETCS_DF_SIM_DEBUG_OUTPUT__
    // DEBUG
    ::std::cout << "DEBUG: started CEmergencyBrake::SetActivation("<< bActivated << ")..." << ::std::flush;
    #endif
    
    // lock mutex
    m_Mutex.lock();
    
    #ifdef __OETCS_DF_SIM_DEBUG_OUTPUT__
    // DEBUG
    ::std::cout << "locked..." << ::std::flush;
    #endif

    // set value of member from parameter
    m_bActive = bActivated;

    // unlock mutex
    m_Mutex.unlock();
    
    #ifdef __OETCS_DF_SIM_DEBUG_OUTPUT__
    // DEBUG
    ::std::cout << "done" << ::std::endl;
    #endif   
  // Bouml preserved body end 000DA802

} // void CEmergencyBrake::SetActivation()  




CBaliseDeviceOut::CBaliseDeviceOut()  throw()  
: ::oETCS::DF::PS::CBaliseDeviceOut(nullptr)
{
  // Bouml preserved body begin 000D8982
    // empty constructor
  // Bouml preserved body end 000D8982

} // CBaliseDeviceOut::CBaliseDeviceOut()  throw() 




CBaliseDeviceOut::~CBaliseDeviceOut()  throw()  
{
  // Bouml preserved body begin 000D8A02
    // empty destructor
  // Bouml preserved body end 000D8A02

} // CBaliseDeviceOut::~CBaliseDeviceOut()  throw() 




void CBaliseDeviceOut::SendTelegram(const ::QBitArray& Bits)   
{
  // Bouml preserved body begin 000D8A82
    unsigned int            x(0);
    const unsigned int      SIZE(Bits.size());
    
    
    
    // print prefix to stdout
    ::std::cout << "Telegram sent: ";
    
    // print telegram bit-wise from MSB to LSB
    for (x = SIZE - 1; x >= 0; x--)
    {
        // print current bit
        ::std::cout << (Bits[x] ? "1" : "0");
        
    } // for (x = SIZE - 1; x >= 0; x--)
    
    // new line after each telegram
    ::std::cout << ::std::endl;
  // Bouml preserved body end 000D8A82

} // void CBaliseDeviceOut::SendTelegram()  




CBaliseDeviceIn::CBaliseDeviceIn()  throw()  
: ::oETCS::DF::PS::CBaliseDeviceIn(nullptr)
{
  // Bouml preserved body begin 000DC182
    // empty constructor
  // Bouml preserved body end 000DC182

} // CBaliseDeviceIn::CBaliseDeviceIn()  throw() 




CBaliseDeviceIn::~CBaliseDeviceIn()  throw()  
{
  // Bouml preserved body begin 000DC202
    // empty destructor
  // Bouml preserved body end 000DC202

} // CBaliseDeviceIn::~CBaliseDeviceIn()  throw() 




void CBaliseDeviceIn::SetNewInputTelegram(const ::QBitArray& Bits)  throw()  
{
  // Bouml preserved body begin 000DC302
    ::QByteArray            Bytes;
    int                     by(0);
    int                     bi(0);
    const int               BITS(Bits.size());
    const int               BYTES(BITS / 8 + (BITS % 8 == 0 ? 0 : 1));
    char                    cByte(0);
    
    
    
    // resize byte array
    Bytes.resize(BYTES);
    
    // process all bits byte-wise
    for (by = 0; by < BYTES; by++)
    {
        // reset current byte
        cByte = 0;
        
        // compose current byte bit-wise
        for (bi = 0; bi < 8 && (bi + by * 8) < BITS; bi++)
        {
            // add current bit to current byte
            cByte |= Bits[bi + by * 8] ? (1 << bi) : 0; 
            
        } // for (bi = 0; bi < 8 && (bi + by * 8) < BITS; bi++)
        
        // add current byte to byte array
        Bytes[by] = cByte;
        
    } // for (by = 0; by < BYTES; by++)
    
    
    // emit signal with new telegram
    Q_EMIT this->NewTelegram(Bytes, BITS);
  // Bouml preserved body end 000DC302

} // void CBaliseDeviceIn::SetNewInputTelegram()  throw() 






} // namespace oETCS::DF::PS::SIM

} // namespace oETCS::DF::PS

} // namespace oETCS::DF

} // namespace oETCS