| 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 "KvhSensor.hpp" | ||
| 10 | |||
| 11 | #include "util/Logger.hpp" | ||
| 12 | |||
| 13 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 14 | |||
| 15 | #include "internal/NodeManager.hpp" | ||
| 16 | namespace nm = NAV::NodeManager; | ||
| 17 | #include "internal/FlowManager.hpp" | ||
| 18 | |||
| 19 | #include "util/Time/TimeBase.hpp" | ||
| 20 | #include "util/Vendor/KVH/KvhUtilities.hpp" | ||
| 21 | |||
| 22 | #include "NodeData/IMU/KvhObs.hpp" | ||
| 23 | |||
| 24 | 112 | NAV::KvhSensor::KvhSensor() | |
| 25 |
3/6✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 112 times.
✗ Branch 10 not taken.
|
112 | : Imu(typeStatic()) |
| 26 | { | ||
| 27 | LOG_TRACE("{}: called", name); | ||
| 28 | |||
| 29 | 112 | _onlyRealTime = true; | |
| 30 | 112 | _hasConfig = true; | |
| 31 | 112 | _guiConfigDefaultWindowSize = { 360, 70 }; | |
| 32 | |||
| 33 | // TODO: Update the library to handle different baudrates | ||
| 34 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
112 | _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_921600); |
| 35 | |||
| 36 |
4/8✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 112 times.
✓ Branch 10 taken 112 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
448 | nm::CreateOutputPin(this, "KvhObs", Pin::Type::Flow, { NAV::KvhObs::type() }); |
| 37 | 224 | } | |
| 38 | |||
| 39 | 224 | NAV::KvhSensor::~KvhSensor() | |
| 40 | { | ||
| 41 | LOG_TRACE("{}: called", nameId()); | ||
| 42 | 224 | } | |
| 43 | |||
| 44 | 224 | std::string NAV::KvhSensor::typeStatic() | |
| 45 | { | ||
| 46 |
1/2✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
|
448 | return "KvhSensor"; |
| 47 | } | ||
| 48 | |||
| 49 | ✗ | std::string NAV::KvhSensor::type() const | |
| 50 | { | ||
| 51 | ✗ | return typeStatic(); | |
| 52 | } | ||
| 53 | |||
| 54 | 112 | std::string NAV::KvhSensor::category() | |
| 55 | { | ||
| 56 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Provider"; |
| 57 | } | ||
| 58 | |||
| 59 | ✗ | void NAV::KvhSensor::guiConfig() | |
| 60 | { | ||
| 61 | ✗ | if (ImGui::InputTextWithHint("SensorPort", "/dev/ttyUSB0", &_sensorPort)) | |
| 62 | { | ||
| 63 | ✗ | LOG_DEBUG("{}: SensorPort changed to {}", nameId(), _sensorPort); | |
| 64 | ✗ | flow::ApplyChanges(); | |
| 65 | ✗ | doDeinitialize(); | |
| 66 | } | ||
| 67 | ✗ | ImGui::SameLine(); | |
| 68 | ✗ | gui::widgets::HelpMarker("COM port where the sensor is attached to\n" | |
| 69 | "- \"COM1\" (Windows format for physical and virtual (USB) serial port)\n" | ||
| 70 | "- \"/dev/ttyS1\" (Linux format for physical serial port)\n" | ||
| 71 | "- \"/dev/ttyUSB0\" (Linux format for virtual (USB) serial port)\n" | ||
| 72 | "- \"/dev/tty.usbserial-FTXXXXXX\" (Mac OS X format for virtual (USB) serial port)\n" | ||
| 73 | "- \"/dev/ttyS0\" (CYGWIN format. Usually the Windows COM port number minus 1. This would connect to COM1)"); | ||
| 74 | |||
| 75 | ✗ | Imu::guiConfig(); | |
| 76 | ✗ | } | |
| 77 | |||
| 78 | ✗ | [[nodiscard]] json NAV::KvhSensor::save() const | |
| 79 | { | ||
| 80 | LOG_TRACE("{}: called", nameId()); | ||
| 81 | |||
| 82 | ✗ | json j; | |
| 83 | |||
| 84 | ✗ | j["UartSensor"] = UartSensor::save(); | |
| 85 | ✗ | j["Imu"] = Imu::save(); | |
| 86 | |||
| 87 | ✗ | return j; | |
| 88 | ✗ | } | |
| 89 | |||
| 90 | ✗ | void NAV::KvhSensor::restore(json const& j) | |
| 91 | { | ||
| 92 | LOG_TRACE("{}: called", nameId()); | ||
| 93 | |||
| 94 | ✗ | if (j.contains("UartSensor")) | |
| 95 | { | ||
| 96 | ✗ | UartSensor::restore(j.at("UartSensor")); | |
| 97 | } | ||
| 98 | ✗ | if (j.contains("Imu")) | |
| 99 | { | ||
| 100 | ✗ | Imu::restore(j.at("Imu")); | |
| 101 | } | ||
| 102 | ✗ | } | |
| 103 | |||
| 104 | ✗ | bool NAV::KvhSensor::resetNode() | |
| 105 | { | ||
| 106 | ✗ | return true; | |
| 107 | } | ||
| 108 | |||
| 109 | ✗ | bool NAV::KvhSensor::initialize() | |
| 110 | { | ||
| 111 | LOG_TRACE("{}: called", nameId()); | ||
| 112 | |||
| 113 | // connect to the sensor | ||
| 114 | try | ||
| 115 | { | ||
| 116 | ✗ | _sensor->connect(_sensorPort, sensorBaudrate()); | |
| 117 | |||
| 118 | ✗ | LOG_DEBUG("{} connected on port {} with baudrate {}", nameId(), _sensorPort, sensorBaudrate()); | |
| 119 | } | ||
| 120 | ✗ | catch (...) | |
| 121 | { | ||
| 122 | ✗ | LOG_ERROR("{} could not connect", nameId()); | |
| 123 | ✗ | return false; | |
| 124 | ✗ | } | |
| 125 | |||
| 126 | ✗ | _sensor->registerAsyncPacketReceivedHandler(this, asciiOrBinaryAsyncMessageReceived); | |
| 127 | |||
| 128 | ✗ | return true; | |
| 129 | } | ||
| 130 | |||
| 131 | ✗ | void NAV::KvhSensor::deinitialize() | |
| 132 | { | ||
| 133 | LOG_TRACE("{}: called", nameId()); | ||
| 134 | |||
| 135 | ✗ | if (!isInitialized()) | |
| 136 | { | ||
| 137 | ✗ | return; | |
| 138 | } | ||
| 139 | |||
| 140 | ✗ | if (_sensor->isConnected()) | |
| 141 | { | ||
| 142 | try | ||
| 143 | { | ||
| 144 | ✗ | _sensor->unregisterAsyncPacketReceivedHandler(); | |
| 145 | } | ||
| 146 | ✗ | catch (...) // NOLINT(bugprone-empty-catch) | |
| 147 | ✗ | {} | |
| 148 | ✗ | _sensor->disconnect(); | |
| 149 | } | ||
| 150 | } | ||
| 151 | |||
| 152 | ✗ | void NAV::KvhSensor::asciiOrBinaryAsyncMessageReceived(void* userData, uart::protocol::Packet& p, [[maybe_unused]] size_t index) | |
| 153 | { | ||
| 154 | ✗ | auto* kvhSensor = static_cast<KvhSensor*>(userData); | |
| 155 | |||
| 156 | ✗ | if (p.type() == uart::protocol::Packet::Type::TYPE_BINARY) | |
| 157 | { | ||
| 158 | ✗ | auto obs = std::make_shared<KvhObs>(kvhSensor->_imuPos, p); | |
| 159 | |||
| 160 | ✗ | vendor::kvh::decryptKvhObs(obs); | |
| 161 | |||
| 162 | LOG_DATA("DATA({}): {}, {}, {}", | ||
| 163 | kvhSensor->name, obs->sequenceNumber, obs->temperature.value(), fmt::streamed(obs->status)); | ||
| 164 | |||
| 165 | // Check if a packet was skipped | ||
| 166 | ✗ | if (kvhSensor->_prevSequenceNumber == UINT8_MAX) | |
| 167 | { | ||
| 168 | ✗ | kvhSensor->_prevSequenceNumber = obs->sequenceNumber; | |
| 169 | } | ||
| 170 | ✗ | if (obs->sequenceNumber != 0 && (obs->sequenceNumber < kvhSensor->_prevSequenceNumber || obs->sequenceNumber > kvhSensor->_prevSequenceNumber + 2)) | |
| 171 | { | ||
| 172 | ✗ | LOG_WARN("{}: Sequence Number changed from {} to {}", kvhSensor->name, kvhSensor->_prevSequenceNumber, obs->sequenceNumber); | |
| 173 | } | ||
| 174 | ✗ | kvhSensor->_prevSequenceNumber = obs->sequenceNumber; | |
| 175 | |||
| 176 | // Calls all the callbacks | ||
| 177 | ✗ | if (InsTime currentTime = util::time::GetCurrentInsTime(); | |
| 178 | ✗ | !currentTime.empty()) | |
| 179 | { | ||
| 180 | ✗ | obs->insTime = currentTime; | |
| 181 | } | ||
| 182 | ✗ | kvhSensor->invokeCallbacks(OUTPUT_PORT_INDEX_KVH_OBS, obs); | |
| 183 | ✗ | } | |
| 184 | ✗ | else if (p.type() == uart::protocol::Packet::Type::TYPE_ASCII) | |
| 185 | { | ||
| 186 | ✗ | LOG_WARN("{}: Received an ASCII Async message: {}", kvhSensor->name, p.datastr()); | |
| 187 | } | ||
| 188 | ✗ | } | |
| 189 |