StatusSignalValue.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix/StatusCodes.h"
#include "ctre/phoenix/platform/Platform.hpp"
#include "ctre/phoenix/threading/CopyMoveMutex.hpp"
#include "ctre/phoenixpro/hardware/DeviceIdentifier.hpp"
#include "ctre/phoenixpro/Timestamp.hpp"
#include "ctre/phoenixpro/networking/interfaces/ReportError_Interface.h"
#include <functional>
#include <initializer_list>
#include <map>
#include <ostream>
#include <sstream>
#include <string>
#include <units/base.h>
#include <units/frequency.h>
#include <units/time.h>
 
#if defined(_WIN32) || defined(_WIN64)
#include <Windows.h> // For UTF-8 support
#endif
 
namespace ctre {
namespace phoenixpro {
 
    namespace hardware {
        class ParentDevice;
    }
 
    template <typename T>
    class StatusSignalValue;
 
    class BaseStatusSignalValue
    {
        friend hardware::ParentDevice; // Allow ParentDevice to set error
 
    protected:
        hardware::DeviceIdentifier deviceIdentifier;
        uint16_t spn;
        std::string units;
        AllTimestamps timestamps{};
        int timestampType;
        double baseValue = 0;
        ctre::phoenix::StatusCode error = ctre::phoenix::StatusCode::SigNotUpdated;
        std::string signalName;
 
        template <typename T>
        friend class StatusSignalValue;
 
        void CopyFrom(const BaseStatusSignalValue &other)
        {
            this->units = other.units;
            this->timestamps = other.timestamps;
            this->timestampType = other.timestampType;
            this->baseValue = other.baseValue;
            this->error = other.error;
            // We don't care about copying the signal name, because the user will expect the original name
        }
 
        BaseStatusSignalValue(hardware::DeviceIdentifier deviceIdentifier,
                              uint16_t spn,
                              std::string signalName) : deviceIdentifier{std::move(deviceIdentifier)},
                                                        spn{spn},
                                                        signalName{std::move(signalName)}
        {
        }
 
        /* Constructor for an invalid BaseStatusSignalValue */
        BaseStatusSignalValue(ctre::phoenix::StatusCode error) : deviceIdentifier{hardware::DeviceIdentifier{}},
                                                                 spn{0},
                                                                 error{error},
                                                                 signalName{"Invalid"}
        {
        }
 
        /**
         * \brief Wait for multiple signals to arrive
         *
         * \param signals Signals to wait for
         * \param network Network to wait for the signals on
         * \param timeoutSeconds Maximum time to wait for all these signals
         * \returns Status of the wait
         */
        static ctre::phoenix::StatusCode Status_WaitForAll(
            std::initializer_list<BaseStatusSignalValue *> signals,
            const char *network,
            units::time::second_t timeoutSeconds);
 
        /**
         * \brief Wait for multiple signals to arrive
         *
         * \param signals Signals to wait for
         * \param network Network to wait for the signals on
         * \param timeoutSeconds Maximum time to wait for all these signals
         * \returns Status of the wait
         */
        static ctre::phoenix::StatusCode Status_WaitForAll(
            const std::vector<BaseStatusSignalValue *> &signals,
            const char *network,
            units::time::second_t timeoutSeconds);
 
        ctre::phoenix::StatusCode SetUpdateFrequency(const char *canbus, uint32_t deviceHash, uint16_t spn, units::frequency::hertz_t frequencyHz, units::time::second_t timeoutSeconds);
 
        /**
         * Get signal update.  Caller either get last received, or wait for an update.
         *
         * \param network Name of bus (can, canfd, canivore-name, future protocols)
         * \param deviceHash Hash id of the device (based on device id and model)
         * \param signal Signal to get
         * \param bWaitForUpdate  If true, API will wait up to timeoutSeconds for an update.  If false, routine will poll last received and use timeoutSeconds to return error code if too old.
         * \param timeoutSeconds How long to wait or how old the signal can be before error'ing
         * \param outValue Value of the signal
         * \param units Unit that corresponds with signal
         * \param hwtimestamp Timestamp of the signal
         * \param swtimestamp Timestamp of the signal
         * \param ecutimestamp Timestamp of the signal
         * \param timestampType Which type of timestamp was gotten
         * \returns Status of the get
         */
        static ctre::phoenix::StatusCode Status_Get(
            const char *network,
            int deviceHash,
            uint32_t signal,
            bool bWaitForUpdate, units::time::second_t timeoutSeconds,
            double *outValue, std::string *units,
            double *hwtimestamp, double *swtimestamp, double *ecutimestamp, int32_t *timestampType);
 
    public:
        virtual ~BaseStatusSignalValue() = 0; // Declare virtual destructor to make this class abstract
 
        /**
         * \brief Wait for all provided signals up to timeout.
         *        This API is typically used with CANivore Bus signals as they will be synced using the
         *        CANivore Timesync feature and arrive simultaneously. Signals on a roboRIO bus cannot
         *        be synced and may require a significantly longer blocking call to receive all signals.
         *
         *        This can also be used with a timeout of zero to refresh many signals at once, which
         *        is faster than calling Refresh() on every signal.
         * 
         * \param timeoutSeconds How long to wait for all the signals to arrive.
         *                       Pass zero to just refresh all signals without blocking.
         * \param signals Signals to wait on, passed as initializer list of signal addresses.
         * \return An InvalidParamValue if signals array is empty,
         *         InvalidNetwork if signals are on different CAN bus networks,
         *         RxTimeout if it took longer than timeoutSeconds to receive all the signals.
         *         An OK status code means that all signals arrived within timeoutSeconds and they are all OK.
         * 
         *         Any other value represents the StatusCode of the first failed signal.
         *         Call GetError() on each signal to determine which ones failed.
         */
        static ctre::phoenix::StatusCode WaitForAll(units::time::second_t timeoutSeconds, std::initializer_list<BaseStatusSignalValue *> signals)
        {
            return WaitForAll(timeoutSeconds, 
                std::vector<BaseStatusSignalValue*>(signals.begin(), signals.end()));                
        }
        /**
         * \brief Wait for all provided signals up to timeout.
         *        This API is typically used with CANivore Bus signals as they will be synced using the 
         *        CANivore Timesync feature and arrive simultaneously. Signals on a roboRIO bus cannot
         *        be synced and may require a significantly longer blocking call to receive all signals.
         *
         *        This can also be used with a timeout of zero to refresh many signals at once, which
         *        is faster than calling Refresh() on every signal.
         *
         * \param timeoutSeconds How long to wait for all the signals to arrive.
         *                       Pass zero to just refresh all signals without blocking.
         * \param signals Signals to wait on, passed as vector of signal addresses.
         * \return An InvalidParamValue if signals array is empty,
         *         InvalidNetwork if signals are on different CAN bus networks,
         *         RxTimeout if it took longer than timeoutSeconds to receive all the signals.
         *         An OK status code means that all signals arrived within timeoutSeconds and they are all OK.
         * 
         *         Any other value represents the StatusCode of the first failed signal.
         *         Call GetError() on each signal to determine which ones failed.
         */
        static ctre::phoenix::StatusCode WaitForAll(units::time::second_t timeoutSeconds, const std::vector<BaseStatusSignalValue *> &signals)
        {
            /* static string for location */
            const char kLocation[] = "ctre::phoenixpro::BaseStatusSignalValue::WaitForAll";
 
            if (signals.size() < 1)
            {
                /* We don't have any signals to wait for, so return early */
                ctre::phoenix::StatusCode retval = ctre::phoenix::StatusCode::InvalidParamValue;
                c_ctre_phoenix_report_error(true, retval, 0,
                                            retval.GetDescription(),
                                            kLocation,
                                            ctre::phoenix::platform::GetStackTrace(1).c_str());
                return retval;
            }
            const std::string &network = (*signals.begin())->deviceIdentifier.network;
 
            for (auto signal : signals)
            {
                /* Check that they all have the same network */
                if (network != signal->deviceIdentifier.network)
                {
                    ctre::phoenix::StatusCode retval = ctre::phoenix::StatusCode::InvalidNetwork; // Networks don't match, return early
 
                    c_ctre_phoenix_report_error(true, retval, 0,
                                                retval.GetDescription(),
                                                kLocation,
                                                ctre::phoenix::platform::GetStackTrace(1).c_str());
                    return retval;
                }
            }
            /* Now wait for all the signals */
            ctre::phoenix::StatusCode retval = Status_WaitForAll(signals, network.c_str(), timeoutSeconds);
 
            /*error reporting */
            if (false == retval.IsOK())
            {
                c_ctre_phoenix_report_error(retval.IsError(), retval, 0,
                                            retval.GetDescription(),
                                            kLocation,
                                            ctre::phoenix::platform::GetStackTrace(1).c_str());
            }
            return retval;
        }
        /**
         * \brief Performs latency compensation on signal using the signalSlope and signal's latency to determine
         *        the magnitude of compensation. The caller must refresh these StatusSignalValues beforehand,
         *        this function only does the math required for latency compensation.
         *
         * \example units::turn_t compensatedTurns = GetLatencyCompensatedValue(fx.GetPosition(), fx.GetVelocity());
         *
         * \tparam U Type of signal's underlying type. This is the type that the function will return.
         * \tparam U_PER_SEC Type of signalSlope's underlying type. This must be the derivative of U.
         * \param signal Signal to be latency compensated. Caller must make sure this signal is up to date
         *               either by calling \c Refresh() or \c WaitForUpdate().
         * \param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this
         *                    signal is up to date either by calling \c Refresh() or \c WaitForUpdate().
         * \returns Latency compensated value from the signal StatusSignalValue.
         */
        template <typename U, typename U_PER_SEC>
        static U GetLatencyCompensatedValue(StatusSignalValue<U>& signal, StatusSignalValue<U_PER_SEC>& signalSlope)
        {
            static_assert(units::traits::is_unit_t_v<U>, "signal must be a unit signal");
            static_assert(units::traits::is_unit_t_v<U_PER_SEC>, "signalSlope must be a unit signal");
            using lhs = typename units::traits::unit_t_traits<U>::unit_type;
            using rhs = typename units::traits::unit_t_traits<U_PER_SEC>::unit_type;
            static_assert(units::traits::is_convertible_unit_v<lhs, units::compound_unit<rhs, units::seconds>>,
                          "Compensation can only be performed on a signal with its derivative");
            U nonCompensatedSignal = signal.GetValue();
            U_PER_SEC changeInSignal = signalSlope.GetValue();
            units::second_t latency = signal.GetTimestamp().GetLatency();
            return nonCompensatedSignal + (changeInSignal * latency);
        }
        /**
         * \brief Check if all signals have an OK error code
         *
         * \param signals Signals to check error code of.
         * \returns True if all signals are OK, false otherwise
         */
        static ctre::phoenix::StatusCode IsAllGood(std::initializer_list<BaseStatusSignalValue *> signals)
        {
            for (auto signal : signals)
            {
                if (signal->GetError() != ctre::phoenix::StatusCode::OK)
                    return false;
            }
            return true;
        }
 
        /**
         * \brief Gets the units for this signal
         *
         * \returns String representation of units for this signal
         */
        const std::string &GetUnits() const { return units; }
        /**
         * \brief Get the timestamps of this signals
         *
         * \returns All timestamps for this signal
         */
        const AllTimestamps &GetAllTimestamps() const { return timestamps; }
        /**
         * \brief Get the best timestamp available to this signal
         *
         * \returns Best available timestamp for this signal
         */
        const Timestamp &GetTimestamp() const { return timestamps.GetBestTimestamp(); }
        /**
         * \brief Get the error code from when we last received this signal
         *
         * \returns Last cached Error Code
         */
        ctre::phoenix::StatusCode GetError() const { return error; }
    };
 
    template <typename L>
    struct SignalMeasurement
    {
        /**
         * \brief The value of the signal, this may be an enum so it is stored as a string
         */
        L value;
        /**
         * \brief Timestamp of when the data point was taken
         */
        units::time::second_t timestamp;
        /**
         * \brief Code response of getting the data
         */
        ctre::phoenix::StatusCode error;
        /**
         * \brief Units that correspond to this point
         */
        std::string units;
    };
 
    template <typename T>
    class StatusSignalValue : public BaseStatusSignalValue
    {
        bool _containsUnderlyingTypes;
        std::map<int, StatusSignalValue<T>> _basicTypeMap;
        std::function<void()> _checkFirmVersFunction;
        ctre::phoenix::threading::CopyMoveMutex<std::mutex> _copyLck;
 
        void RefreshSwitch(bool block, units::time::second_t timeout)
        {
            /* Just make sure we don't do anything if we're not a switching statement */
            if (!_containsUnderlyingTypes)
                return;
 
            double switchValue;
            /* We only care about the switch value, so get that quickly */
            ctre::phoenix::StatusCode outputError = Status_Get(
                this->deviceIdentifier.network.c_str(),
                this->deviceIdentifier.deviceHash,
                this->spn,
                block, timeout,
                &switchValue, nullptr,
                nullptr, nullptr, nullptr, nullptr);
 
            if (outputError != ctre::phoenix::StatusCode::OK)
            {
                this->error = outputError;
                return;
            }
 
            auto foundValue = _basicTypeMap.find(static_cast<int>(switchValue));
            if (foundValue != _basicTypeMap.end())
            {
                /* Found it, so refresh it and set our values to it */
                foundValue->second.RefreshValue(block, timeout, false); // Top-level refresh will handle the error
 
                /* save to members, use locks because string copies are not atomic */
                {
                    std::lock_guard<std::mutex> lock{_copyLck};
                    CopyFrom(foundValue->second);
                }
            }
            else
            {
                /* Didn't find it, so set our error */
                this->error = ctre::phoenix::StatusCode::InvalidModeToGetSignal;
            }
        }
 
        void RefreshNoSwitch(bool block, units::time::second_t timeout)
        {
            /* Just make sure we don't do anything if we are a switching statement */
            if (_containsUnderlyingTypes)
                return;
 
            /* fetch values */
            std::string outUnits;
            double outHwTimestamp;
            double outSwTimestamp;
            double outEcuTimestamp;
            int32_t outTimestampType;
 
            ctre::phoenix::StatusCode outputError = Status_Get(
                this->deviceIdentifier.network.c_str(),
                this->deviceIdentifier.deviceHash,
                this->spn,
                block, timeout,
                &this->baseValue, &outUnits,
                &outHwTimestamp, &outSwTimestamp, &outEcuTimestamp, &outTimestampType);
 
            /* save to members, use locks if need be (remember though Java locks are not reliable) */
            if (outputError >= 0)
            {
                this->timestamps.Update(Timestamp{units::time::second_t{outSwTimestamp}, Timestamp::TimestampSource::System},
                                        Timestamp{units::time::second_t{outHwTimestamp}, Timestamp::TimestampSource::CANivore},
                                        Timestamp{});
            }
            this->units = outUnits; // Units are always correct since they're independent of CAN frame
            this->error = outputError;
        }
 
        void RefreshValue(bool block, units::time::second_t timeout, bool ReportOnError)
        {
            _checkFirmVersFunction();
            if (_containsUnderlyingTypes)
            {
                RefreshSwitch(block, timeout);
            }
            else
            {
                RefreshNoSwitch(block, timeout);
            }
 
            if (ReportOnError && !(this->error.IsOK()))
            {
                std::stringstream location;
                location << this->deviceIdentifier.ToString() << " StatusSignalValue " << this->signalName;
                c_ctre_phoenix_report_error(this->error.IsError(), this->error, 0, this->error.GetDescription(), location.str().c_str(), ctre::phoenix::platform::GetStackTrace(1).c_str());
            }
        }
 
        friend class hardware::ParentDevice;
 
        StatusSignalValue(const BaseStatusSignalValue &other) : BaseStatusSignalValue{other.deviceIdentifier, other.spn, other.signalName},
                                                                _containsUnderlyingTypes{false},
                                                                _basicTypeMap{},
                                                                _checkFirmVersFunction{[] {}}
        {
            CopyFrom(other);
        }
 
        StatusSignalValue(hardware::DeviceIdentifier deviceIdentifier,
                        uint16_t spn,
                        std::function<void()> checkFirmVersFunction,
                        std::string signalName) : BaseStatusSignalValue{std::move(deviceIdentifier), spn, std::move(signalName)},
                                                    _containsUnderlyingTypes{false},
                                                    _basicTypeMap{},
                                                    _checkFirmVersFunction{checkFirmVersFunction}
        {
        }
 
        StatusSignalValue(hardware::DeviceIdentifier deviceIdentifier,
                        uint16_t spn,
                        std::function<void()> checkFirmVersFunction,
                        std::function<std::map<int, StatusSignalValue<T>>()> map_filler,
                        std::string signalName) : BaseStatusSignalValue{std::move(deviceIdentifier), spn, std::move(signalName)},
                                                    _containsUnderlyingTypes{true},
                                                    _basicTypeMap{map_filler()},
                                                    _checkFirmVersFunction{checkFirmVersFunction}
        {
        }
 
        /* Constructor for an invalid StatusSignalValue */
        StatusSignalValue(ctre::phoenix::StatusCode error) : BaseStatusSignalValue{error},
                                                             _containsUnderlyingTypes{false},
                                                             _basicTypeMap{},
                                                             _checkFirmVersFunction{[] {}}
        {
        }
 
    public:
        friend std::ostream &operator<<(std::ostream &os, const StatusSignalValue<T> &data)
        {
    #if defined(_WIN32) || defined(_WIN64)
            // Set console code page to UTF-8 so console known how to interpret string data
            SetConsoleOutputCP(CP_UTF8);
    #endif
            if constexpr(units::traits::is_unit_t_v<T>)
            {
                os << data.GetValue().value() << " " << data.GetUnits();
            }
            else
            {
                os << data.GetValue() << " " << data.GetUnits();
            }
            return os;
        }
        std::string ToString() const
        {
            std::stringstream ss;
            ss << *this;
            return ss.str();
        }
 
        /**
         * \brief Gets the cached value from this status signal value
         *
         * \details Gets the cached value. To make sure the value is up-to-date
         *          call \c Refresh() or \c WaitForUpdate()
         *
         * \returns Cached value
         */
        T GetValue() const
        {
            if constexpr(units::traits::is_unit_t_v<T>)
            {
                return units::make_unit<T>(this->baseValue);
            }
            else
            {
                return static_cast<T>(this->baseValue);
            }
        }
 
        /**
         * \brief Refreshes this status signal value.
         *
         * If the user application caches this StatusSignalValue object
         * instead of periodically fetching it from the hardware device,
         * this function must be called to fetch fresh data.
         *
         * \details This performs a non-blocking refresh operation. If
         *          you want to wait until you receive the signal, call
         *          \c WaitForUpdate() instead.
         *
         * \param ReportOnError Report to the Driver Station if this refresh returns a StatusCode other than OK
         *
         * \returns Reference to itself
         */
        StatusSignalValue<T> &Refresh(bool ReportOnError = true)
        {
            RefreshValue(false, 0.300_s, ReportOnError); // Don't block and error if signal is older than 0.300 seconds
            return *this;
        }
        /**
         * \brief Waits up to timeoutSec to get the up-to-date status signal value
         *
         * \details This performs a blocking refresh operation. If
         *          you want to non-blocking refresh the signal, call
         *          \c Refresh() instead.
         *
         * \param timeoutSec Timeout to wait for an update on the signal
         * \param ReportOnError Report to the Driver Station if this refresh returns a StatusCode other than OK
         *
         * \returns Reference to itself
         */
        StatusSignalValue<T> &WaitForUpdate(units::time::second_t timeoutSec, bool ReportOnError = true)
        {
            RefreshValue(true, timeoutSec, ReportOnError);
            return *this;
        }
 
        /**
         * \brief Get a basic data-only container with this information, to be used for things such as data logging.
         *
         * \returns Basic structure with all relevant information
         */
        SignalMeasurement<T> GetDataCopy() const
        {
            SignalMeasurement<T> toRet;
            toRet.value = GetValue();
            toRet.error = GetError();
            toRet.units = GetUnits();
            toRet.timestamp = GetTimestamp().GetTime();
            return toRet;
        }
 
        /**
         * \brief Sets the rate at which the device will publish this signal.
         *
         * The minimum supported signal frequency is 4 Hz, and the maximum is 1000 Hz.
         *
         * \param frequencyHz Rate to publish the signal in Hz.
         * \param timeoutSeconds Maximum amount of time to wait when performing the action
         * \returns Status code of setting the update frequency
         */
        ctre::phoenix::StatusCode SetUpdateFrequency(units::frequency::hertz_t frequencyHz, units::time::second_t timeoutSeconds = 50_ms)
        {
            /* attempt to change the frequency */
            return BaseStatusSignalValue::SetUpdateFrequency(this->deviceIdentifier.network.c_str(),
                                                    this->deviceIdentifier.deviceHash,
                                                    this->spn,
                                                    frequencyHz,
                                                    timeoutSeconds);
        }
 
        /**
         * \brief Returns a lambda that calls Refresh and GetValue on this object. This is useful for command-based programming.
         *
         * \returns std::function<T()> that calls Refresh() and returns this value.
         */
        std::function<T()> AsSupplier()
        {
            return [this]()
            { return this->Refresh().GetValue(); };
        }
    };
 
}
}