LatencyUtil.hpp

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, INC. The Software is
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, INC.
******************************************************************************/
/*
Latency Util file
 
Author: Will Burken
*/
 
#ifndef MODELS_SUPPORT_LATENCY_UTIL_H
#define MODELS_SUPPORT_LATENCY_UTIL_H
 
#include "simulation/SimulationExecutive.h"
#include <queue>
 
namespace warptwin {

    template <typename T>
    class LatencyUtil {
    public:
        // Model params
        //         NAME                     TYPE                    DEFAULT VALUE
 
        // Model-specific implementations of startup and derivative
        LatencyUtil(SimulationExecutive &exc):_exc(exc) { }

        int start(int latency = 0, T start_val = T());

        int step(T input_value, T &output_value);

        int reset();
 
    protected:
        std::queue<T> _latency_queue; 
        SimulationExecutive &_exc;  
        clockwerk::Time _lat_sim_rate; 
        T _default_value; 
        bool _first_run = true; 
    };
 
 
    template <typename T>
    int LatencyUtil<T>::start(int latency, T start_val) {
 
        _lat_sim_rate = _exc.rate(); // Get the simulation rate in Hz
        _default_value = start_val;  // Set the default fill value
 
        // Check if simulation rate is zero hz (this means that the simulation step size is 0.0 seconds)
        // This check avoids a divide by zerror error
        if (_lat_sim_rate.asFrequency() < 1e-15) {
            return ERROR_DIVIDE_BY_ZERO;
        }
 
        // Convert latency to step size and check for valid range
        int step_size = std::ceil((double)latency / ((double)_exc.rate().asMilliseconds())); // Convert latency and sim rate to steps
        if(step_size < 0) {
            return ERROR_INVALID_RANGE;
        }
 
        // Fill the queue with the start value
        for(int i = 0; i < step_size; i++) {
            _latency_queue.push(start_val);
        }
 
        // Warn the user if the latency does not line up with the simulaiton rate
        if (not (std::abs(std::fmod((double)latency, _exc.rate().asMilliseconds())) < 1e-10)) {
            return WARNING_POORLY_MATCHED_RATES;
        }
 
        return NO_ERROR;
    }
 
    template <typename T>
    int LatencyUtil<T>::step(T input_value, T &output_value) {
        // Check if the simulation rate has changed
        // Get the current simulation rate
        clockwerk::Time current_rate;
        _exc.time().step_end_time().subtract( _exc.time().step_start_time(), current_rate); // Get the current simulation rate
 
        // Convert the current rate to frequency and check for a significant change
        if (std::abs(current_rate.asFrequency() - _lat_sim_rate.asFrequency()) > 0.001) {
            // Also check if the current_rate is zero, if so then we are preforming the zero step at the beginning of run and do not want to throw error
            if (current_rate.asFrequency() > 1e-15 and not _first_run) {
                // If the simulation rate has changed, throw an error
                std::string error = "LatencyUtil: Simulation rate has changed from " 
                    + std::to_string(_lat_sim_rate.asFloatingPoint()) 
                    + " to " + std::to_string(current_rate.asFloatingPoint());
                _exc.os().sysLog(0.0, "", error.c_str(), error.size());
                return ERROR_STEP_OVERRUN;
            }
            // Set first run boolean tracker to false
            _first_run = false;
        }
 
        // Read the input value and write it to the queue
        _latency_queue.push(input_value);
 
        // Ensure we have a value to output
        if(_latency_queue.empty()) {
            return ERROR_QUEUE_EMPTY;
        }
 
        // Take the output value from the front of the queue
        output_value = _latency_queue.front();
        _latency_queue.pop();
 
        return NO_ERROR;
    }
 
    template <typename T>
    int LatencyUtil<T>::reset() {
 
        // For the size of the queue, add default value and discard oldest value
        for (typename std::queue<T>::size_type i = 0; i < _latency_queue.size(); ++i) {
            _latency_queue.push(_default_value);
            _latency_queue.pop();
        }
 
        return NO_ERROR;
    }
 
}
 
#endif