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