| 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 "Ln200Sensor.hpp" | ||
| 10 | |||
| 11 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 12 | #include "internal/FlowManager.hpp" | ||
| 13 | |||
| 14 | #include "util/Logger.hpp" | ||
| 15 | #include "util/Time/TimeBase.hpp" | ||
| 16 | #include "util/Vendor/Litton/Ln200UartSensor.hpp" | ||
| 17 | |||
| 18 | #include "NodeData/IMU/ImuObsWDelta.hpp" | ||
| 19 | |||
| 20 | 114 | NAV::Ln200Sensor::Ln200Sensor() | |
| 21 |
3/6✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 114 times.
✗ Branch 10 not taken.
|
114 | : Imu(typeStatic()) |
| 22 | { | ||
| 23 | LOG_TRACE("{}: called", name); | ||
| 24 | |||
| 25 | 114 | _onlyRealTime = true; | |
| 26 | 114 | _hasConfig = true; | |
| 27 | 114 | _guiConfigDefaultWindowSize = { 479, 146 }; | |
| 28 | |||
| 29 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
114 | _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_2000000); |
| 30 | |||
| 31 |
4/8✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 114 times.
✓ Branch 10 taken 114 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
456 | CreateOutputPin("ImuObs", Pin::Type::Flow, { NAV::ImuObsWDelta::type() }); |
| 32 | 228 | } | |
| 33 | |||
| 34 | 228 | NAV::Ln200Sensor::~Ln200Sensor() | |
| 35 | { | ||
| 36 | LOG_TRACE("{}: called", nameId()); | ||
| 37 | 228 | } | |
| 38 | |||
| 39 | 228 | std::string NAV::Ln200Sensor::typeStatic() | |
| 40 | { | ||
| 41 |
1/2✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
|
456 | return "Ln200Sensor"; |
| 42 | } | ||
| 43 | |||
| 44 | ✗ | std::string NAV::Ln200Sensor::type() const | |
| 45 | { | ||
| 46 | ✗ | return typeStatic(); | |
| 47 | } | ||
| 48 | |||
| 49 | 114 | std::string NAV::Ln200Sensor::category() | |
| 50 | { | ||
| 51 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
228 | return "Data Provider"; |
| 52 | } | ||
| 53 | |||
| 54 | ✗ | void NAV::Ln200Sensor::guiConfig() | |
| 55 | { | ||
| 56 | ✗ | if (ImGui::InputTextWithHint("SensorPort", "/dev/ttyUSB0", &_sensorPort)) | |
| 57 | { | ||
| 58 | ✗ | LOG_DEBUG("{}: SensorPort changed to {}", nameId(), _sensorPort); | |
| 59 | ✗ | flow::ApplyChanges(); | |
| 60 | ✗ | doDeinitialize(); | |
| 61 | } | ||
| 62 | ✗ | ImGui::SameLine(); | |
| 63 | ✗ | gui::widgets::HelpMarker("COM port where the sensor is attached to\n" | |
| 64 | "- \"COM1\" (Windows format for physical and virtual (USB) serial port)\n" | ||
| 65 | "- \"/dev/ttyS1\" (Linux format for physical serial port)\n" | ||
| 66 | "- \"/dev/ttyUSB0\" (Linux format for virtual (USB) serial port)\n" | ||
| 67 | "- \"/dev/tty.usbserial-FTXXXXXX\" (Mac OS X format for virtual (USB) serial port)\n" | ||
| 68 | "- \"/dev/ttyS0\" (CYGWIN format. Usually the Windows COM port number minus 1. This would connect to COM1)"); | ||
| 69 | |||
| 70 | ✗ | Imu::guiConfig(); | |
| 71 | ✗ | } | |
| 72 | |||
| 73 | ✗ | [[nodiscard]] json NAV::Ln200Sensor::save() const | |
| 74 | { | ||
| 75 | LOG_TRACE("{}: called", nameId()); | ||
| 76 | |||
| 77 | ✗ | json j; | |
| 78 | |||
| 79 | ✗ | j["UartSensor"] = UartSensor::save(); | |
| 80 | ✗ | j["Imu"] = Imu::save(); | |
| 81 | |||
| 82 | ✗ | return j; | |
| 83 | ✗ | } | |
| 84 | |||
| 85 | ✗ | void NAV::Ln200Sensor::restore(json const& j) | |
| 86 | { | ||
| 87 | LOG_TRACE("{}: called", nameId()); | ||
| 88 | |||
| 89 | ✗ | if (j.contains("UartSensor")) | |
| 90 | { | ||
| 91 | ✗ | UartSensor::restore(j.at("UartSensor")); | |
| 92 | } | ||
| 93 | ✗ | if (j.contains("Imu")) | |
| 94 | { | ||
| 95 | ✗ | Imu::restore(j.at("Imu")); | |
| 96 | } | ||
| 97 | ✗ | } | |
| 98 | |||
| 99 | ✗ | bool NAV::Ln200Sensor::resetNode() | |
| 100 | { | ||
| 101 | ✗ | return true; | |
| 102 | } | ||
| 103 | |||
| 104 | ✗ | bool NAV::Ln200Sensor::initialize() | |
| 105 | { | ||
| 106 | LOG_TRACE("{}: called", nameId()); | ||
| 107 | |||
| 108 | // connect to the sensor | ||
| 109 | try | ||
| 110 | { | ||
| 111 | ✗ | _sensor->connect(_sensorPort, sensorBaudrate()); | |
| 112 | |||
| 113 | ✗ | LOG_DEBUG("{} connected on port {} with baudrate {}", nameId(), _sensorPort, sensorBaudrate()); | |
| 114 | } | ||
| 115 | ✗ | catch (...) | |
| 116 | { | ||
| 117 | ✗ | LOG_ERROR("{} could not connect", nameId()); | |
| 118 | ✗ | return false; | |
| 119 | ✗ | } | |
| 120 | |||
| 121 | ✗ | _sensor->registerAsyncPacketReceivedHandler(this, binaryAsyncMessageReceived); | |
| 122 | |||
| 123 | ✗ | return true; | |
| 124 | } | ||
| 125 | |||
| 126 | ✗ | void NAV::Ln200Sensor::deinitialize() | |
| 127 | { | ||
| 128 | LOG_TRACE("{}: called", nameId()); | ||
| 129 | |||
| 130 | ✗ | if (!isInitialized()) | |
| 131 | { | ||
| 132 | ✗ | return; | |
| 133 | } | ||
| 134 | |||
| 135 | ✗ | if (_sensor->isConnected()) | |
| 136 | { | ||
| 137 | try | ||
| 138 | { | ||
| 139 | ✗ | _sensor->unregisterAsyncPacketReceivedHandler(); | |
| 140 | } | ||
| 141 | ✗ | catch (...) // NOLINT(bugprone-empty-catch) | |
| 142 | ✗ | {} | |
| 143 | LOG_TRACE("{}: Disconnecting...", nameId()); | ||
| 144 | ✗ | _sensor->disconnect(); | |
| 145 | LOG_TRACE("{}: Disconnected", nameId()); | ||
| 146 | } | ||
| 147 | } | ||
| 148 | |||
| 149 | ✗ | void NAV::Ln200Sensor::binaryAsyncMessageReceived(void* userData, uart::protocol::Packet& p, size_t /* index */) | |
| 150 | { | ||
| 151 | ✗ | auto* lnSensor = static_cast<Ln200Sensor*>(userData); | |
| 152 | |||
| 153 | ✗ | if (p.type() == uart::protocol::Packet::Type::TYPE_BINARY) | |
| 154 | { | ||
| 155 | ✗ | auto obs = std::make_shared<ImuObsWDelta>(lnSensor->_imuPos); | |
| 156 | |||
| 157 | // Lambda for reversing bits in a uint16_t | ||
| 158 | ✗ | auto reverseBits = [](uint16_t& word) { | |
| 159 | ✗ | word = static_cast<uint16_t>(((word & 0x5555) << 1) | ((word & 0xAAAA) >> 1)); // Swap adjacent bits | |
| 160 | ✗ | word = static_cast<uint16_t>(((word & 0x3333) << 2) | ((word & 0xCCCC) >> 2)); // Swap pairs of bits | |
| 161 | ✗ | word = static_cast<uint16_t>(((word & 0x0F0F) << 4) | ((word & 0xF0F0) >> 4)); // Swap nibbles | |
| 162 | ✗ | word = static_cast<uint16_t>(((word & 0x00FF) << 8) | ((word & 0xFF00) >> 8)); // Swap bytes | |
| 163 | ✗ | }; | |
| 164 | |||
| 165 | ✗ | const std::vector<uint8_t>& bytes = p.getRawData(); | |
| 166 | ✗ | std::array<uint16_t, 14> rawWords{}; // Temporary storage for raw words | |
| 167 | ✗ | std::array<double, 12> processedWords{}; // Storage of processed data words | |
| 168 | |||
| 169 | // Extract exactly 14 words (each 16 bits in little-endian order) | ||
| 170 | ✗ | for (uint8_t i = 0; i < 14; ++i) | |
| 171 | { | ||
| 172 | ✗ | uint8_t index = i * 2; | |
| 173 | ✗ | uint16_t word = static_cast<uint16_t>(bytes[index + 1]) | static_cast<uint16_t>(static_cast<uint16_t>(bytes[index]) << 8); | |
| 174 | ✗ | reverseBits(word); | |
| 175 | ✗ | rawWords.at(i) = word; | |
| 176 | } | ||
| 177 | |||
| 178 | // TODO: Check internal validity | ||
| 179 | // The last word is a checksum: | ||
| 180 | // uint16_t checksum = rawWords[12]; | ||
| 181 | |||
| 182 | // TODO: Process IMU status | ||
| 183 | // uint16_t imuStatus = rawWords[6]; | ||
| 184 | |||
| 185 | // TODO: Implement the conversion of the mux word based on the mux id | ||
| 186 | // The 8th word (index 7) is the multiplexer ID and the 9th word (index 8) is mux data | ||
| 187 | // uint16_t muxId = rawWords[7]; | ||
| 188 | // uint16_t muxDataWordRaw = rawWords[8]; | ||
| 189 | |||
| 190 | // Convert the mux word based on the mux id | ||
| 191 | // double muxDataWord = 0.0; | ||
| 192 | // if (muxId == 8 || muxId == 13) // 8: Temperature conversion, 13: Voltage conversion | ||
| 193 | // { | ||
| 194 | // muxDataWord = static_cast<double>(muxDataWordRaw) * 0.1; | ||
| 195 | // } | ||
| 196 | // else // 17: IMU failure and all other cases | ||
| 197 | // { | ||
| 198 | // muxDataWord = muxDataWordRaw; | ||
| 199 | // } | ||
| 200 | |||
| 201 | // Processing velocity | ||
| 202 | ✗ | for (uint8_t i = 0; i < 3; ++i) | |
| 203 | { | ||
| 204 | ✗ | processedWords.at(i) = static_cast<int16_t>(rawWords.at(i)) * (1.0 / 16384.0); | |
| 205 | } | ||
| 206 | // Processing angles | ||
| 207 | ✗ | for (uint8_t i = 3; i < 6; ++i) | |
| 208 | { | ||
| 209 | ✗ | processedWords.at(i) = static_cast<int16_t>(rawWords.at(i)) * (1.0 / 262144.0); | |
| 210 | } | ||
| 211 | ✗ | double dVelX = processedWords[0]; | |
| 212 | ✗ | double dVelY = processedWords[1]; | |
| 213 | ✗ | double dVelZ = processedWords[2]; | |
| 214 | ✗ | double dAngleX = processedWords[3]; | |
| 215 | ✗ | double dAngleY = processedWords[4]; | |
| 216 | ✗ | double dAngleZ = processedWords[5]; | |
| 217 | ✗ | double accelX = processedWords[0] * FREQ; | |
| 218 | ✗ | double accelY = processedWords[1] * FREQ; | |
| 219 | ✗ | double accelZ = processedWords[2] * FREQ; | |
| 220 | ✗ | double angleRateX = processedWords[3] * FREQ; | |
| 221 | ✗ | double angleRateY = processedWords[4] * FREQ; | |
| 222 | ✗ | double angleRateZ = processedWords[5] * FREQ; | |
| 223 | |||
| 224 | ✗ | obs->dtime = 1.0 / FREQ; | |
| 225 | ✗ | obs->dvel = { dVelX, dVelY, dVelZ }; | |
| 226 | ✗ | obs->dtheta = { dAngleX, dAngleY, dAngleZ }; | |
| 227 | ✗ | obs->p_acceleration = { accelX, accelY, accelZ }; | |
| 228 | ✗ | obs->p_angularRate = { angleRateX, angleRateY, angleRateZ }; | |
| 229 | |||
| 230 | LOG_DATA("DATA({}): {}", lnSensor->name, obs->temperature.value()); | ||
| 231 | |||
| 232 | // Calls all the callbacks | ||
| 233 | ✗ | if (InsTime currentTime = util::time::GetCurrentInsTime(); | |
| 234 | ✗ | !currentTime.empty()) | |
| 235 | { | ||
| 236 | ✗ | obs->insTime = currentTime; | |
| 237 | } | ||
| 238 | ✗ | lnSensor->invokeCallbacks(OUTPUT_PORT_INDEX_LN_OBS, obs); | |
| 239 | ✗ | } | |
| 240 | ✗ | else if (p.type() == uart::protocol::Packet::Type::TYPE_ASCII) | |
| 241 | { | ||
| 242 | ✗ | LOG_WARN("{}: Received an ASCII Async message: {}", lnSensor->name, p.datastr()); | |
| 243 | } | ||
| 244 | ✗ | } | |
| 245 |