| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | #include "Delay.hpp" | ||
| 10 | |||
| 11 | #include "internal/FlowManager.hpp" | ||
| 12 | |||
| 13 | #include "NodeData/NodeData.hpp" | ||
| 14 | |||
| 15 | namespace NAV::experimental | ||
| 16 | { | ||
| 17 | |||
| 18 | ✗ | Delay::Delay() | |
| 19 | ✗ | : Node("z^-1") | |
| 20 | { | ||
| 21 | LOG_TRACE("{}: called", name); | ||
| 22 | |||
| 23 | ✗ | _hasConfig = true; | |
| 24 | ✗ | _guiConfigDefaultWindowSize = { 305, 70 }; | |
| 25 | ✗ | kind = Kind::Simple; | |
| 26 | |||
| 27 | ✗ | CreateInputPin("", Pin::Type::Flow, { NodeData::type() }, &Delay::delayObs); | |
| 28 | ✗ | CreateOutputPin("", Pin::Type::Flow, { NodeData::type() }); | |
| 29 | ✗ | } | |
| 30 | |||
| 31 | ✗ | Delay::~Delay() | |
| 32 | { | ||
| 33 | LOG_TRACE("{}: called", nameId()); | ||
| 34 | ✗ | } | |
| 35 | |||
| 36 | ✗ | std::string Delay::typeStatic() | |
| 37 | { | ||
| 38 | ✗ | return "Delay"; | |
| 39 | } | ||
| 40 | |||
| 41 | ✗ | std::string Delay::type() const | |
| 42 | { | ||
| 43 | ✗ | return typeStatic(); | |
| 44 | } | ||
| 45 | |||
| 46 | ✗ | std::string Delay::category() | |
| 47 | { | ||
| 48 | ✗ | return "Experimental/Simple"; | |
| 49 | } | ||
| 50 | |||
| 51 | ✗ | void Delay::guiConfig() | |
| 52 | { | ||
| 53 | ✗ | if (ImGui::InputInt(fmt::format("Delay length##{}", size_t(id)).c_str(), &_delayLength)) | |
| 54 | { | ||
| 55 | ✗ | _delayLength = std::max(_delayLength, 1); | |
| 56 | ✗ | LOG_DEBUG("{}: delayLength changed to {}", nameId(), _delayLength); | |
| 57 | ✗ | if (name.starts_with("z^-")) | |
| 58 | { | ||
| 59 | ✗ | name = fmt::format("z^-{}", _delayLength); | |
| 60 | } | ||
| 61 | |||
| 62 | ✗ | while (_buffer.size() > static_cast<size_t>(_delayLength)) | |
| 63 | { | ||
| 64 | ✗ | _buffer.pop_front(); | |
| 65 | } | ||
| 66 | } | ||
| 67 | ✗ | } | |
| 68 | |||
| 69 | ✗ | [[nodiscard]] json Delay::save() const | |
| 70 | { | ||
| 71 | LOG_TRACE("{}: called", nameId()); | ||
| 72 | |||
| 73 | ✗ | json j; | |
| 74 | |||
| 75 | ✗ | j["delayLength"] = _delayLength; | |
| 76 | |||
| 77 | ✗ | return j; | |
| 78 | ✗ | } | |
| 79 | |||
| 80 | ✗ | void Delay::restore(json const& j) | |
| 81 | { | ||
| 82 | LOG_TRACE("{}: called", nameId()); | ||
| 83 | |||
| 84 | ✗ | if (j.contains("delayLength")) | |
| 85 | { | ||
| 86 | ✗ | j.at("delayLength").get_to(_delayLength); | |
| 87 | } | ||
| 88 | ✗ | } | |
| 89 | |||
| 90 | ✗ | bool Delay::initialize() | |
| 91 | { | ||
| 92 | LOG_TRACE("{}: called", nameId()); | ||
| 93 | |||
| 94 | ✗ | _buffer.clear(); | |
| 95 | |||
| 96 | ✗ | return true; | |
| 97 | } | ||
| 98 | |||
| 99 | ✗ | void Delay::deinitialize() | |
| 100 | { | ||
| 101 | LOG_TRACE("{}: called", nameId()); | ||
| 102 | ✗ | } | |
| 103 | |||
| 104 | ✗ | bool Delay::onCreateLink(OutputPin& startPin, InputPin& endPin) | |
| 105 | { | ||
| 106 | LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id)); | ||
| 107 | |||
| 108 | ✗ | if (endPin.parentNode->id != id) | |
| 109 | { | ||
| 110 | ✗ | return true; // Link on Output Port | |
| 111 | } | ||
| 112 | |||
| 113 | // New Link on the Input port, but the previously connected dataIdentifier is different from the new one. | ||
| 114 | // Then remove all links. | ||
| 115 | ✗ | if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier != startPin.dataIdentifier) | |
| 116 | { | ||
| 117 | ✗ | outputPins.at(OUTPUT_PORT_INDEX_FLOW).deleteLinks(); | |
| 118 | } | ||
| 119 | |||
| 120 | // Update the dataIdentifier of the output pin to the same as input pin | ||
| 121 | ✗ | outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = startPin.dataIdentifier; | |
| 122 | |||
| 123 | // Refresh all links connected to the output pin | ||
| 124 | ✗ | for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links) | |
| 125 | { | ||
| 126 | ✗ | if (auto* connectedPin = link.getConnectedPin()) | |
| 127 | { | ||
| 128 | ✗ | outputPins.at(OUTPUT_PORT_INDEX_FLOW).recreateLink(*connectedPin); | |
| 129 | } | ||
| 130 | } | ||
| 131 | |||
| 132 | ✗ | return true; | |
| 133 | } | ||
| 134 | |||
| 135 | ✗ | void Delay::delayObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) | |
| 136 | { | ||
| 137 | ✗ | if (_buffer.size() == static_cast<size_t>(_delayLength)) | |
| 138 | { | ||
| 139 | ✗ | auto oldest = _buffer.front(); | |
| 140 | ✗ | _buffer.pop_front(); | |
| 141 | ✗ | _buffer.push_back(queue.extract_front()); | |
| 142 | |||
| 143 | LOG_DATA("{}: Delay pushing out message: {}", nameId(), _buffer.back()->insTime.toGPSweekTow()); | ||
| 144 | |||
| 145 | ✗ | if (!(NAV::Node::callbacksEnabled)) | |
| 146 | { | ||
| 147 | ✗ | NAV::Node::callbacksEnabled = true; | |
| 148 | } | ||
| 149 | |||
| 150 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, oldest); | |
| 151 | ✗ | } | |
| 152 | else | ||
| 153 | { | ||
| 154 | ✗ | _buffer.push_back(queue.extract_front()); | |
| 155 | } | ||
| 156 | ✗ | } | |
| 157 | |||
| 158 | } // namespace NAV::experimental | ||
| 159 |