| 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 "UlogFile.hpp" | ||
| 10 | |||
| 11 | #include "util/Logger.hpp" | ||
| 12 | |||
| 13 | #include <exception> | ||
| 14 | |||
| 15 | #include "internal/gui/widgets/FileDialog.hpp" | ||
| 16 | |||
| 17 | #include "internal/NodeManager.hpp" | ||
| 18 | namespace nm = NAV::NodeManager; | ||
| 19 | #include "internal/FlowManager.hpp" | ||
| 20 | |||
| 21 | #include "util/Vendor/Pixhawk/UlogFileFormat.hpp" | ||
| 22 | #include "NodeData/IMU/ImuObs.hpp" | ||
| 23 | #include "NodeData/State/PosVelAtt.hpp" | ||
| 24 | #include "Navigation/Transformations/Units.hpp" | ||
| 25 | |||
| 26 | #include <ctime> | ||
| 27 | |||
| 28 | // ----------------------------------------------------------- Basic Node Functions -------------------------------------------------------------- | ||
| 29 | |||
| 30 | 112 | NAV::UlogFile::UlogFile() | |
| 31 |
3/6✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 112 times.
✗ Branch 9 not taken.
|
112 | : Imu(typeStatic()) |
| 32 | { | ||
| 33 | LOG_TRACE("{}: called", name); | ||
| 34 | |||
| 35 | 112 | _hasConfig = true; | |
| 36 | 112 | _guiConfigDefaultWindowSize = { 589, 257 }; | |
| 37 | |||
| 38 | // All message types are polled from the first output pin, but then send out on the correct pin over invokeCallbacks | ||
| 39 |
4/8✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112 times.
✓ Branch 9 taken 112 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
336 | nm::CreateOutputPin(this, "ImuObs #1", Pin::Type::Flow, { NAV::ImuObs::type() }, &UlogFile::pollData); |
| 40 |
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, "ImuObs #2", Pin::Type::Flow, { NAV::ImuObs::type() }); |
| 41 |
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, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() }); |
| 42 | 448 | } | |
| 43 | |||
| 44 | 224 | NAV::UlogFile::~UlogFile() | |
| 45 | { | ||
| 46 | LOG_TRACE("{}: called", nameId()); | ||
| 47 | 224 | } | |
| 48 | |||
| 49 | 224 | std::string NAV::UlogFile::typeStatic() | |
| 50 | { | ||
| 51 |
1/2✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
|
448 | return "UlogFile"; |
| 52 | } | ||
| 53 | |||
| 54 | ✗ | std::string NAV::UlogFile::type() const | |
| 55 | { | ||
| 56 | ✗ | return typeStatic(); | |
| 57 | } | ||
| 58 | |||
| 59 | 112 | std::string NAV::UlogFile::category() | |
| 60 | { | ||
| 61 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Provider"; |
| 62 | } | ||
| 63 | |||
| 64 | ✗ | void NAV::UlogFile::guiConfig() | |
| 65 | { | ||
| 66 | ✗ | if (auto res = FileReader::guiConfig(".ulg,.*", { ".ulg" }, size_t(id), nameId())) | |
| 67 | { | ||
| 68 | ✗ | LOG_DEBUG("{}: Path changed to {}", nameId(), _path); | |
| 69 | ✗ | flow::ApplyChanges(); | |
| 70 | ✗ | if (res == FileReader::PATH_CHANGED) | |
| 71 | { | ||
| 72 | ✗ | doReinitialize(); | |
| 73 | } | ||
| 74 | else | ||
| 75 | { | ||
| 76 | ✗ | doDeinitialize(); | |
| 77 | } | ||
| 78 | } | ||
| 79 | |||
| 80 | ✗ | Imu::guiConfig(); | |
| 81 | ✗ | } | |
| 82 | |||
| 83 | ✗ | [[nodiscard]] json NAV::UlogFile::save() const | |
| 84 | { | ||
| 85 | LOG_TRACE("{}: called", nameId()); | ||
| 86 | |||
| 87 | ✗ | json j; | |
| 88 | |||
| 89 | ✗ | j["FileReader"] = FileReader::save(); | |
| 90 | ✗ | j["Imu"] = Imu::save(); | |
| 91 | |||
| 92 | ✗ | return j; | |
| 93 | ✗ | } | |
| 94 | |||
| 95 | ✗ | void NAV::UlogFile::restore(json const& j) | |
| 96 | { | ||
| 97 | LOG_TRACE("{}: called", nameId()); | ||
| 98 | |||
| 99 | ✗ | if (j.contains("FileReader")) | |
| 100 | { | ||
| 101 | ✗ | FileReader::restore(j.at("FileReader")); | |
| 102 | } | ||
| 103 | ✗ | if (j.contains("Imu")) | |
| 104 | { | ||
| 105 | ✗ | Imu::restore(j.at("Imu")); | |
| 106 | } | ||
| 107 | ✗ | } | |
| 108 | |||
| 109 | ✗ | bool NAV::UlogFile::initialize() | |
| 110 | { | ||
| 111 | LOG_TRACE("{}: called", nameId()); | ||
| 112 | |||
| 113 | ✗ | return FileReader::initialize(); | |
| 114 | } | ||
| 115 | |||
| 116 | ✗ | void NAV::UlogFile::deinitialize() | |
| 117 | { | ||
| 118 | LOG_TRACE("{}: called", nameId()); | ||
| 119 | |||
| 120 | ✗ | FileReader::deinitialize(); | |
| 121 | ✗ | } | |
| 122 | |||
| 123 | ✗ | bool NAV::UlogFile::resetNode() | |
| 124 | { | ||
| 125 | LOG_TRACE("{}: called", nameId()); | ||
| 126 | |||
| 127 | ✗ | FileReader::resetReader(); | |
| 128 | |||
| 129 | ✗ | lastGnssTime.timeSinceStartup = 0; | |
| 130 | ✗ | _epochData.clear(); | |
| 131 | ✗ | _subscribedMessages.clear(); | |
| 132 | |||
| 133 | ✗ | return true; | |
| 134 | } | ||
| 135 | |||
| 136 | // ------------------------------------------------------------ File Reading --------------------------------------------------------------- | ||
| 137 | |||
| 138 | ✗ | NAV::FileReader::FileType NAV::UlogFile::determineFileType() | |
| 139 | { | ||
| 140 | LOG_TRACE("{}: called", nameId()); | ||
| 141 | |||
| 142 | ✗ | std::filesystem::path filepath = getFilepath(); | |
| 143 | |||
| 144 | ✗ | auto filestream = std::ifstream(filepath); | |
| 145 | |||
| 146 | ✗ | constexpr uint16_t BUFFER_SIZE = 10; // TODO: validate size | |
| 147 | |||
| 148 | ✗ | std::array<char, BUFFER_SIZE> buffer{}; | |
| 149 | ✗ | if (filestream.good()) | |
| 150 | { | ||
| 151 | ✗ | filestream.read(buffer.data(), BUFFER_SIZE); | |
| 152 | ✗ | filestream.close(); | |
| 153 | ✗ | LOG_DEBUG("{} has the file type: CSV", nameId()); | |
| 154 | ✗ | return FileType::BINARY; | |
| 155 | } | ||
| 156 | ✗ | filestream.close(); | |
| 157 | |||
| 158 | ✗ | LOG_ERROR("{} could not open file", nameId(), filepath); | |
| 159 | ✗ | return FileType::NONE; | |
| 160 | ✗ | } | |
| 161 | |||
| 162 | ✗ | void NAV::UlogFile::readHeader() | |
| 163 | { | ||
| 164 | LOG_TRACE("{}: called", nameId()); | ||
| 165 | |||
| 166 | ✗ | if (_fileType == FileType::BINARY) | |
| 167 | { | ||
| 168 | union | ||
| 169 | { | ||
| 170 | std::array<char, 16> data{}; | ||
| 171 | vendor::pixhawk::ulog_Header_s header; | ||
| 172 | ✗ | } ulogHeader{}; | |
| 173 | |||
| 174 | ✗ | read(ulogHeader.data.data(), ulogHeader.data.size()); | |
| 175 | |||
| 176 | // Check "ULog" at beginning of file | ||
| 177 | ✗ | if ((ulogHeader.header.fileMagic[0] != 'U') || (ulogHeader.header.fileMagic[1] != 'L') || (ulogHeader.header.fileMagic[2] != 'o') || (ulogHeader.header.fileMagic[3] != 'g')) | |
| 178 | { | ||
| 179 | ✗ | LOG_WARN("{}: FileType is binary, but not ULog", nameId()); | |
| 180 | } | ||
| 181 | |||
| 182 | ✗ | LOG_DEBUG("{}: version: {}", nameId(), static_cast<int>(ulogHeader.header.version)); | |
| 183 | |||
| 184 | ✗ | LOG_DEBUG("{}: time stamp [µs]: {}", nameId(), ulogHeader.header.timeStamp); | |
| 185 | |||
| 186 | // Read message header | ||
| 187 | union | ||
| 188 | { | ||
| 189 | std::array<char, 3> data{}; | ||
| 190 | vendor::pixhawk::message_header_s msgHeader; | ||
| 191 | ✗ | } ulogMsgHeader{}; | |
| 192 | |||
| 193 | ✗ | while ((ulogMsgHeader.msgHeader.msg_type != 'A') && (ulogMsgHeader.msgHeader.msg_type != 'L')) | |
| 194 | { | ||
| 195 | ✗ | read(ulogMsgHeader.data.data(), ulogMsgHeader.data.size()); | |
| 196 | |||
| 197 | LOG_DATA("{}: msgSize: {}, msgType: {}", nameId(), ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | ||
| 198 | |||
| 199 | // Read definition message | ||
| 200 | // Flag bitset message | ||
| 201 | ✗ | if (ulogMsgHeader.msgHeader.msg_type == 'B') | |
| 202 | { | ||
| 203 | ✗ | if (ulogMsgHeader.msgHeader.msg_size > 40) | |
| 204 | { | ||
| 205 | ✗ | LOG_WARN("{}: Exceeding bytes in 'flag bitset message' are ignored. Check for ULog file format update.", nameId()); | |
| 206 | } | ||
| 207 | |||
| 208 | union | ||
| 209 | { | ||
| 210 | std::array<char, 40> data{}; | ||
| 211 | vendor::pixhawk::ulog_message_flag_bits_s ulogMsgFlagBits_s; | ||
| 212 | ✗ | } ulogMsgFlagBits{}; | |
| 213 | |||
| 214 | ✗ | read(ulogMsgFlagBits.data.data(), ulogMsgFlagBits.data.size() * sizeof(char)); // 'sizeof' is optional here, but it is the solution in general, since data types can be larger than one byte | |
| 215 | } | ||
| 216 | |||
| 217 | // Format definition for a single (composite) type that can be logged or used in another definition as a nested type | ||
| 218 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'F') | |
| 219 | { | ||
| 220 | ✗ | vendor::pixhawk::message_format_s messageFormat; | |
| 221 | ✗ | messageFormat.header = ulogMsgHeader.msgHeader; | |
| 222 | |||
| 223 | LOG_DATA("{}: messageFormat.header.msg_size: {}", nameId(), messageFormat.header.msg_size); | ||
| 224 | |||
| 225 | ✗ | messageFormat.format.resize(messageFormat.header.msg_size); | |
| 226 | ✗ | read(messageFormat.format.data(), ulogMsgHeader.msgHeader.msg_size); | |
| 227 | LOG_DATA("{}: messageFormat.format.data(): {}", nameId(), messageFormat.format.data()); | ||
| 228 | |||
| 229 | ✗ | std::string msgName = messageFormat.format.substr(0, messageFormat.format.find(':')); | |
| 230 | |||
| 231 | // Decoding data format fields | ||
| 232 | ✗ | std::stringstream lineStream(messageFormat.format.substr(messageFormat.format.find(':') + 1)); | |
| 233 | ✗ | std::string cell; | |
| 234 | |||
| 235 | ✗ | std::vector<DataField> msgDataFields; | |
| 236 | |||
| 237 | ✗ | while (std::getline(lineStream, cell, ';')) | |
| 238 | { | ||
| 239 | ✗ | DataField data_field{ .type = cell.substr(0, cell.find(' ')), .name = cell.substr(cell.find(' ') + 1) }; | |
| 240 | ✗ | msgDataFields.push_back(data_field); | |
| 241 | ✗ | } | |
| 242 | |||
| 243 | ✗ | if (msgName == "sensor_accel") | |
| 244 | { | ||
| 245 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 246 | } | ||
| 247 | ✗ | else if (msgName == "sensor_gyro") | |
| 248 | { | ||
| 249 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 250 | } | ||
| 251 | ✗ | else if (msgName == "sensor_mag") | |
| 252 | { | ||
| 253 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 254 | } | ||
| 255 | ✗ | else if (msgName == "vehicle_gps_position") | |
| 256 | { | ||
| 257 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 258 | } | ||
| 259 | ✗ | else if (msgName == "vehicle_attitude") | |
| 260 | { | ||
| 261 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 262 | } | ||
| 263 | ✗ | else if (msgName == "vehicle_air_data") | |
| 264 | { | ||
| 265 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 266 | } | ||
| 267 | ✗ | else if (msgName == "vehicle_control_mode") | |
| 268 | { | ||
| 269 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 270 | } | ||
| 271 | ✗ | else if (msgName == "vehicle_status") | |
| 272 | { | ||
| 273 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 274 | } | ||
| 275 | ✗ | else if (msgName == "cpuload") | |
| 276 | { | ||
| 277 | ✗ | _messageFormats.insert_or_assign(msgName, msgDataFields); | |
| 278 | } | ||
| 279 | else | ||
| 280 | { | ||
| 281 | ✗ | LOG_ERROR("{}: Data format '{}' could not be decoded", nameId(), msgName); | |
| 282 | } | ||
| 283 | ✗ | } | |
| 284 | |||
| 285 | // Information message | ||
| 286 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'I') | |
| 287 | { | ||
| 288 | ✗ | readInformationMessage(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 289 | } | ||
| 290 | |||
| 291 | // Information message multi | ||
| 292 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'M') | |
| 293 | { | ||
| 294 | ✗ | readInformationMessageMulti(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 295 | } | ||
| 296 | |||
| 297 | // Parameter message | ||
| 298 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'P') | |
| 299 | { | ||
| 300 | ✗ | readParameterMessage(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 301 | } | ||
| 302 | |||
| 303 | // Parameter message default | ||
| 304 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'Q') | |
| 305 | { | ||
| 306 | ✗ | readParameterMessageDefault(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 307 | } | ||
| 308 | } | ||
| 309 | ✗ | seekg(-3, std::ios_base::cur); // 'msg_size' + 'msg_type' = 3 Byte | |
| 310 | ✗ | LOG_DEBUG("{}: Read 'Definitions Section' completed", nameId()); | |
| 311 | } | ||
| 312 | ✗ | } | |
| 313 | |||
| 314 | ✗ | std::shared_ptr<const NAV::NodeData> NAV::UlogFile::pollData() | |
| 315 | { | ||
| 316 | // Read message header | ||
| 317 | union | ||
| 318 | { | ||
| 319 | std::array<char, 3> data{}; | ||
| 320 | vendor::pixhawk::message_header_s msgHeader; | ||
| 321 | ✗ | } ulogMsgHeader{}; | |
| 322 | |||
| 323 | while (true) | ||
| 324 | { | ||
| 325 | ✗ | read(ulogMsgHeader.data.data(), ulogMsgHeader.data.size()); | |
| 326 | |||
| 327 | LOG_DATA("{}: msgSize: {}", nameId(), ulogMsgHeader.msgHeader.msg_size); | ||
| 328 | LOG_DATA("{}: msgType: {}", nameId(), ulogMsgHeader.msgHeader.msg_type); | ||
| 329 | |||
| 330 | ✗ | if (ulogMsgHeader.msgHeader.msg_type == 'A') | |
| 331 | { | ||
| 332 | ✗ | vendor::pixhawk::message_add_logged_s messageAddLog; | |
| 333 | ✗ | messageAddLog.header = ulogMsgHeader.msgHeader; | |
| 334 | ✗ | read(reinterpret_cast<char*>(&messageAddLog.multi_id), sizeof(messageAddLog.multi_id)); | |
| 335 | LOG_DATA("{}: multi_id: {}", nameId(), messageAddLog.multi_id); | ||
| 336 | ✗ | read(reinterpret_cast<char*>(&messageAddLog.msg_id), sizeof(messageAddLog.msg_id)); | |
| 337 | LOG_DATA("{}: msg_id: {}", nameId(), messageAddLog.msg_id); | ||
| 338 | |||
| 339 | ✗ | messageAddLog.msg_name.resize(messageAddLog.header.msg_size - 3); | |
| 340 | ✗ | read(messageAddLog.msg_name.data(), messageAddLog.header.msg_size - 3); | |
| 341 | LOG_DATA("{}: messageAddLog.msg_name: {}", nameId(), messageAddLog.msg_name); | ||
| 342 | |||
| 343 | /// Combines (sensor-)message name with an ID that indicates a possible multiple of a sensor | ||
| 344 | ✗ | _subscribedMessages.insert_or_assign(messageAddLog.msg_id, SubscriptionData{ .multi_id = messageAddLog.multi_id, .message_name = messageAddLog.msg_name }); | |
| 345 | ✗ | } | |
| 346 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'R') | |
| 347 | { | ||
| 348 | ✗ | vendor::pixhawk::message_remove_logged_s messageRemoveLog; | |
| 349 | ✗ | messageRemoveLog.header = ulogMsgHeader.msgHeader; | |
| 350 | ✗ | read(reinterpret_cast<char*>(&messageRemoveLog.msg_id), sizeof(messageRemoveLog.msg_id)); | |
| 351 | LOG_DATA("{}: Removed message with 'msg_id': {}", nameId(), messageRemoveLog.msg_id); | ||
| 352 | |||
| 353 | ✗ | _subscribedMessages.erase(messageRemoveLog.msg_id); | |
| 354 | } | ||
| 355 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'D') | |
| 356 | { | ||
| 357 | ✗ | vendor::pixhawk::message_data_s messageData; | |
| 358 | ✗ | messageData.header = ulogMsgHeader.msgHeader; | |
| 359 | ✗ | read(reinterpret_cast<char*>(&messageData.msg_id), sizeof(messageData.msg_id)); | |
| 360 | LOG_DATA("{}: msg_id: {}", nameId(), messageData.msg_id); | ||
| 361 | |||
| 362 | ✗ | messageData.data.resize(messageData.header.msg_size - 2); | |
| 363 | ✗ | read(messageData.data.data(), messageData.header.msg_size - 2); | |
| 364 | LOG_DATA("{}: messageData.header.msg_size: {}", nameId(), messageData.header.msg_size); | ||
| 365 | |||
| 366 | ✗ | const auto& messageFormat = _messageFormats.at(_subscribedMessages.at(messageData.msg_id).message_name); | |
| 367 | |||
| 368 | ✗ | size_t currentExtractLocation = 0; | |
| 369 | ✗ | if (_subscribedMessages.at(messageData.msg_id).message_name == "sensor_accel") | |
| 370 | { | ||
| 371 | ✗ | SensorAccel sensorAccel{}; | |
| 372 | ✗ | for (const auto& dataField : messageFormat) | |
| 373 | { | ||
| 374 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 375 | ✗ | if (dataField.name == "timestamp") | |
| 376 | { | ||
| 377 | ✗ | std::memcpy(&sensorAccel.timestamp, currentData, sizeof(sensorAccel.timestamp)); | |
| 378 | LOG_DATA("{}: sensorAccel.timestamp: {}", nameId(), sensorAccel.timestamp); | ||
| 379 | ✗ | currentExtractLocation += sizeof(sensorAccel.timestamp); | |
| 380 | } | ||
| 381 | ✗ | else if (dataField.name == "timestamp_sample") | |
| 382 | { | ||
| 383 | ✗ | std::memcpy(&sensorAccel.timestamp_sample, currentData, sizeof(sensorAccel.timestamp_sample)); | |
| 384 | LOG_DATA("{}: sensorAccel.timestamp_sample: {}", nameId(), sensorAccel.timestamp_sample); | ||
| 385 | ✗ | currentExtractLocation += sizeof(sensorAccel.timestamp_sample); | |
| 386 | } | ||
| 387 | ✗ | else if (dataField.name == "device_id") | |
| 388 | { | ||
| 389 | ✗ | std::memcpy(&sensorAccel.device_id, currentData, sizeof(sensorAccel.device_id)); | |
| 390 | LOG_DATA("{}: sensorAccel.device_id: {}", nameId(), sensorAccel.device_id); | ||
| 391 | ✗ | currentExtractLocation += sizeof(sensorAccel.device_id); | |
| 392 | } | ||
| 393 | ✗ | else if (dataField.name == "error_count") | |
| 394 | { | ||
| 395 | ✗ | std::memcpy(&sensorAccel.error_count, currentData, sizeof(sensorAccel.error_count)); | |
| 396 | LOG_DATA("{}: sensorAccel.error_count: {}", nameId(), sensorAccel.error_count); | ||
| 397 | ✗ | currentExtractLocation += sizeof(sensorAccel.error_count); | |
| 398 | } | ||
| 399 | ✗ | else if (dataField.name == "x") | |
| 400 | { | ||
| 401 | ✗ | std::memcpy(&sensorAccel.x, currentData, sizeof(sensorAccel.x)); | |
| 402 | LOG_DATA("{}: sensorAccel.x: {}", nameId(), sensorAccel.x); | ||
| 403 | ✗ | currentExtractLocation += sizeof(sensorAccel.x); | |
| 404 | } | ||
| 405 | ✗ | else if (dataField.name == "y") | |
| 406 | { | ||
| 407 | ✗ | std::memcpy(&sensorAccel.y, currentData, sizeof(sensorAccel.y)); | |
| 408 | LOG_DATA("{}: sensorAccel.y: {}", nameId(), sensorAccel.y); | ||
| 409 | ✗ | currentExtractLocation += sizeof(sensorAccel.y); | |
| 410 | } | ||
| 411 | ✗ | else if (dataField.name == "z") | |
| 412 | { | ||
| 413 | ✗ | std::memcpy(&sensorAccel.z, currentData, sizeof(sensorAccel.z)); | |
| 414 | LOG_DATA("{}: sensorAccel.z: {}", nameId(), sensorAccel.z); | ||
| 415 | ✗ | currentExtractLocation += sizeof(sensorAccel.z); | |
| 416 | } | ||
| 417 | ✗ | else if (dataField.name == "temperature") | |
| 418 | { | ||
| 419 | ✗ | std::memcpy(&sensorAccel.temperature, currentData, sizeof(sensorAccel.temperature)); | |
| 420 | LOG_DATA("{}: sensorAccel.temperature: {}", nameId(), sensorAccel.temperature); | ||
| 421 | ✗ | currentExtractLocation += sizeof(sensorAccel.temperature); | |
| 422 | } | ||
| 423 | ✗ | else if (dataField.name == "clip_counter") | |
| 424 | { | ||
| 425 | ✗ | std::memcpy(sensorAccel.clip_counter.data(), currentData, sensorAccel.clip_counter.size()); | |
| 426 | LOG_DATA("{}: sensorAccel.clip_counter: {}", nameId(), fmt::join(sensorAccel.clip_counter, ", ")); | ||
| 427 | ✗ | currentExtractLocation += sensorAccel.clip_counter.size(); | |
| 428 | } | ||
| 429 | ✗ | else if (dataField.name.compare(0, 7, "_padding")) // e.g. '_padding0', '_padding1' | |
| 430 | { | ||
| 431 | ✗ | currentExtractLocation += SensorAccel::padding; // Extraction Location should be moved to account for multiple padding | |
| 432 | LOG_DATA("{}: sensorAccel: padding", nameId()); | ||
| 433 | } | ||
| 434 | else | ||
| 435 | { | ||
| 436 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 437 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 438 | } | ||
| 439 | } | ||
| 440 | |||
| 441 | LOG_DATA("{}: Inserting [{}] {}: {}", nameId(), sensorAccel.timestamp, | ||
| 442 | _subscribedMessages.at(messageData.msg_id).multi_id, | ||
| 443 | _subscribedMessages.at(messageData.msg_id).message_name); | ||
| 444 | |||
| 445 | ✗ | _epochData.insert(std::make_pair(sensorAccel.timestamp, | |
| 446 | ✗ | MeasurementData{ .multi_id = _subscribedMessages.at(messageData.msg_id).multi_id, | |
| 447 | ✗ | .message_name = _subscribedMessages.at(messageData.msg_id).message_name, | |
| 448 | .data = sensorAccel })); | ||
| 449 | } | ||
| 450 | ✗ | else if (_subscribedMessages.at(messageData.msg_id).message_name == "sensor_gyro") | |
| 451 | { | ||
| 452 | ✗ | SensorGyro sensorGyro{}; | |
| 453 | |||
| 454 | ✗ | for (const auto& dataField : messageFormat) | |
| 455 | { | ||
| 456 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 457 | ✗ | if (dataField.name == "timestamp") | |
| 458 | { | ||
| 459 | ✗ | std::memcpy(&sensorGyro.timestamp, currentData, sizeof(sensorGyro.timestamp)); | |
| 460 | LOG_DATA("{}: sensorGyro.timestamp: {}", nameId(), sensorGyro.timestamp); | ||
| 461 | ✗ | currentExtractLocation += sizeof(sensorGyro.timestamp); | |
| 462 | } | ||
| 463 | ✗ | else if (dataField.name == "timestamp_sample") | |
| 464 | { | ||
| 465 | ✗ | std::memcpy(&sensorGyro.timestamp_sample, currentData, sizeof(sensorGyro.timestamp_sample)); | |
| 466 | LOG_DATA("{}: sensorGyro.timestamp_sample: {}", nameId(), sensorGyro.timestamp_sample); | ||
| 467 | ✗ | currentExtractLocation += sizeof(sensorGyro.timestamp_sample); | |
| 468 | } | ||
| 469 | ✗ | else if (dataField.name == "device_id") | |
| 470 | { | ||
| 471 | ✗ | std::memcpy(&sensorGyro.device_id, currentData, sizeof(sensorGyro.device_id)); | |
| 472 | LOG_DATA("{}: sensorGyro.device_id: {}", nameId(), sensorGyro.device_id); | ||
| 473 | ✗ | currentExtractLocation += sizeof(sensorGyro.device_id); | |
| 474 | } | ||
| 475 | ✗ | else if (dataField.name == "x") | |
| 476 | { | ||
| 477 | ✗ | std::memcpy(&sensorGyro.x, currentData, sizeof(sensorGyro.x)); | |
| 478 | LOG_DATA("{}: sensorGyro.x: {}", nameId(), sensorGyro.x); | ||
| 479 | ✗ | currentExtractLocation += sizeof(sensorGyro.x); | |
| 480 | } | ||
| 481 | ✗ | else if (dataField.name == "y") | |
| 482 | { | ||
| 483 | ✗ | std::memcpy(&sensorGyro.y, currentData, sizeof(sensorGyro.y)); | |
| 484 | LOG_DATA("{}: sensorGyro.y: {}", nameId(), sensorGyro.y); | ||
| 485 | ✗ | currentExtractLocation += sizeof(sensorGyro.y); | |
| 486 | } | ||
| 487 | ✗ | else if (dataField.name == "z") | |
| 488 | { | ||
| 489 | ✗ | std::memcpy(&sensorGyro.z, currentData, sizeof(sensorGyro.z)); | |
| 490 | LOG_DATA("{}: sensorGyro.z: {}", nameId(), sensorGyro.z); | ||
| 491 | ✗ | currentExtractLocation += sizeof(sensorGyro.z); | |
| 492 | } | ||
| 493 | ✗ | else if (dataField.name == "temperature") | |
| 494 | { | ||
| 495 | ✗ | std::memcpy(&sensorGyro.temperature, currentData, sizeof(sensorGyro.temperature)); | |
| 496 | LOG_DATA("{}: sensorGyro.temperature: {}", nameId(), sensorGyro.temperature); | ||
| 497 | ✗ | currentExtractLocation += sizeof(sensorGyro.temperature); | |
| 498 | } | ||
| 499 | ✗ | else if (dataField.name == "error_count") | |
| 500 | { | ||
| 501 | ✗ | std::memcpy(&sensorGyro.error_count, currentData, sizeof(sensorGyro.error_count)); | |
| 502 | LOG_DATA("{}: sensorGyro.error_count: {}", nameId(), sensorGyro.error_count); | ||
| 503 | ✗ | currentExtractLocation += sizeof(sensorGyro.error_count); | |
| 504 | } | ||
| 505 | else | ||
| 506 | { | ||
| 507 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 508 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 509 | } | ||
| 510 | } | ||
| 511 | |||
| 512 | LOG_DATA("{}: Inserting [{}] {}: {}", nameId(), sensorGyro.timestamp, | ||
| 513 | _subscribedMessages.at(messageData.msg_id).multi_id, | ||
| 514 | _subscribedMessages.at(messageData.msg_id).message_name); | ||
| 515 | |||
| 516 | ✗ | _epochData.insert(std::make_pair(sensorGyro.timestamp, | |
| 517 | ✗ | MeasurementData{ .multi_id = _subscribedMessages.at(messageData.msg_id).multi_id, | |
| 518 | ✗ | .message_name = _subscribedMessages.at(messageData.msg_id).message_name, | |
| 519 | .data = sensorGyro })); | ||
| 520 | } | ||
| 521 | ✗ | else if (_subscribedMessages.at(messageData.msg_id).message_name == "sensor_mag") | |
| 522 | { | ||
| 523 | ✗ | SensorMag sensorMag{}; | |
| 524 | ✗ | for (const auto& dataField : messageFormat) | |
| 525 | { | ||
| 526 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 527 | ✗ | if (dataField.name == "timestamp") | |
| 528 | { | ||
| 529 | ✗ | std::memcpy(&sensorMag.timestamp, currentData, sizeof(sensorMag.timestamp)); | |
| 530 | LOG_DATA("{}: sensorMag.timestamp: {}", nameId(), sensorMag.timestamp); | ||
| 531 | ✗ | currentExtractLocation += sizeof(sensorMag.timestamp); | |
| 532 | } | ||
| 533 | ✗ | else if (dataField.name == "timestamp_sample") | |
| 534 | { | ||
| 535 | ✗ | std::memcpy(&sensorMag.timestamp_sample, currentData, sizeof(sensorMag.timestamp_sample)); | |
| 536 | LOG_DATA("{}: sensorMag.timestamp_sample: {}", nameId(), sensorMag.timestamp_sample); | ||
| 537 | ✗ | currentExtractLocation += sizeof(sensorMag.timestamp_sample); | |
| 538 | } | ||
| 539 | ✗ | else if (dataField.name == "device_id") | |
| 540 | { | ||
| 541 | ✗ | std::memcpy(&sensorMag.device_id, currentData, sizeof(sensorMag.device_id)); | |
| 542 | LOG_DATA("{}: sensorMag.device_id: {}", nameId(), sensorMag.device_id); | ||
| 543 | ✗ | currentExtractLocation += sizeof(sensorMag.device_id); | |
| 544 | } | ||
| 545 | ✗ | else if (dataField.name == "x") | |
| 546 | { | ||
| 547 | ✗ | std::memcpy(&sensorMag.x, currentData, sizeof(sensorMag.x)); | |
| 548 | LOG_DATA("{}: sensorMag.x: {}", nameId(), sensorMag.x); | ||
| 549 | ✗ | currentExtractLocation += sizeof(sensorMag.x); | |
| 550 | } | ||
| 551 | ✗ | else if (dataField.name == "y") | |
| 552 | { | ||
| 553 | ✗ | std::memcpy(&sensorMag.y, currentData, sizeof(sensorMag.y)); | |
| 554 | LOG_DATA("{}: sensorMag.y: {}", nameId(), sensorMag.y); | ||
| 555 | ✗ | currentExtractLocation += sizeof(sensorMag.y); | |
| 556 | } | ||
| 557 | ✗ | else if (dataField.name == "z") | |
| 558 | { | ||
| 559 | ✗ | std::memcpy(&sensorMag.z, currentData, sizeof(sensorMag.z)); | |
| 560 | LOG_DATA("{}: sensorMag.z: {}", nameId(), sensorMag.z); | ||
| 561 | ✗ | currentExtractLocation += sizeof(sensorMag.z); | |
| 562 | } | ||
| 563 | ✗ | else if (dataField.name == "temperature") | |
| 564 | { | ||
| 565 | ✗ | std::memcpy(&sensorMag.temperature, currentData, sizeof(sensorMag.temperature)); | |
| 566 | LOG_DATA("{}: sensorMag.temperature: {}", nameId(), sensorMag.temperature); | ||
| 567 | ✗ | currentExtractLocation += sizeof(sensorMag.temperature); | |
| 568 | } | ||
| 569 | ✗ | else if (dataField.name == "error_count") | |
| 570 | { | ||
| 571 | ✗ | std::memcpy(&sensorMag.error_count, currentData, sizeof(sensorMag.error_count)); | |
| 572 | LOG_DATA("{}: sensorMag.error_count: {}", nameId(), sensorMag.error_count); | ||
| 573 | ✗ | currentExtractLocation += sizeof(sensorMag.error_count); | |
| 574 | } | ||
| 575 | ✗ | else if (dataField.name == "is_external") | |
| 576 | { | ||
| 577 | ✗ | std::memcpy(&sensorMag.is_external, currentData, sizeof(sensorMag.is_external)); | |
| 578 | LOG_DATA("{}: sensorMag.is_external: {}", nameId(), sensorMag.is_external); | ||
| 579 | ✗ | currentExtractLocation += sizeof(sensorMag.is_external); | |
| 580 | } | ||
| 581 | ✗ | else if (dataField.name.compare(0, 7, "_padding")) // e.g. '_padding0', '_padding1' | |
| 582 | { | ||
| 583 | ✗ | currentExtractLocation += SensorMag::padding; // Extraction Location should be moved to account for multiple padding | |
| 584 | LOG_DATA("{}: sensorMag: padding", nameId()); | ||
| 585 | } | ||
| 586 | else | ||
| 587 | { | ||
| 588 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 589 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 590 | } | ||
| 591 | } | ||
| 592 | |||
| 593 | LOG_DATA("{}: Inserting [{}] {}: {}", nameId(), sensorMag.timestamp, | ||
| 594 | _subscribedMessages.at(messageData.msg_id).multi_id, | ||
| 595 | _subscribedMessages.at(messageData.msg_id).message_name); | ||
| 596 | |||
| 597 | ✗ | _epochData.insert(std::make_pair(sensorMag.timestamp, | |
| 598 | ✗ | MeasurementData{ .multi_id = _subscribedMessages.at(messageData.msg_id).multi_id, | |
| 599 | ✗ | .message_name = _subscribedMessages.at(messageData.msg_id).message_name, | |
| 600 | .data = sensorMag })); | ||
| 601 | } | ||
| 602 | ✗ | else if (_subscribedMessages.at(messageData.msg_id).message_name == "vehicle_gps_position") | |
| 603 | { | ||
| 604 | ✗ | VehicleGpsPosition vehicleGpsPosition{}; | |
| 605 | ✗ | for (const auto& dataField : messageFormat) | |
| 606 | { | ||
| 607 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 608 | ✗ | if (dataField.name == "timestamp") | |
| 609 | { | ||
| 610 | ✗ | std::memcpy(&vehicleGpsPosition.timestamp, currentData, sizeof(vehicleGpsPosition.timestamp)); | |
| 611 | LOG_DATA("{}: vehicleGpsPosition.timestamp: {}", nameId(), vehicleGpsPosition.timestamp); | ||
| 612 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.timestamp); | |
| 613 | } | ||
| 614 | ✗ | else if (dataField.name == "time_utc_usec") | |
| 615 | { | ||
| 616 | ✗ | std::memcpy(&vehicleGpsPosition.time_utc_usec, currentData, sizeof(vehicleGpsPosition.time_utc_usec)); | |
| 617 | LOG_DATA("{}: vehicleGpsPosition.time_utc_usec: {}", nameId(), vehicleGpsPosition.time_utc_usec); | ||
| 618 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.time_utc_usec); | |
| 619 | } | ||
| 620 | ✗ | else if (dataField.name == "lat") | |
| 621 | { | ||
| 622 | ✗ | std::memcpy(&vehicleGpsPosition.lat, currentData, sizeof(vehicleGpsPosition.lat)); | |
| 623 | LOG_DATA("{}: vehicleGpsPosition.lat: {}", nameId(), vehicleGpsPosition.lat); | ||
| 624 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.lat); | |
| 625 | } | ||
| 626 | ✗ | else if (dataField.name == "lon") | |
| 627 | { | ||
| 628 | ✗ | std::memcpy(&vehicleGpsPosition.lon, currentData, sizeof(vehicleGpsPosition.lon)); | |
| 629 | LOG_DATA("{}: vehicleGpsPosition.lon: {}", nameId(), vehicleGpsPosition.lon); | ||
| 630 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.lon); | |
| 631 | } | ||
| 632 | ✗ | else if (dataField.name == "alt") | |
| 633 | { | ||
| 634 | ✗ | std::memcpy(&vehicleGpsPosition.alt, currentData, sizeof(vehicleGpsPosition.alt)); | |
| 635 | LOG_DATA("{}: vehicleGpsPosition.alt: {}", nameId(), vehicleGpsPosition.alt); | ||
| 636 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.alt); | |
| 637 | } | ||
| 638 | ✗ | else if (dataField.name == "alt_ellipsoid") | |
| 639 | { | ||
| 640 | ✗ | std::memcpy(&vehicleGpsPosition.alt_ellipsoid, currentData, sizeof(vehicleGpsPosition.alt_ellipsoid)); | |
| 641 | LOG_DATA("{}: vehicleGpsPosition.alt_ellipsoid: {}", nameId(), vehicleGpsPosition.alt_ellipsoid); | ||
| 642 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.alt_ellipsoid); | |
| 643 | } | ||
| 644 | ✗ | else if (dataField.name == "s_variance_m_s") | |
| 645 | { | ||
| 646 | ✗ | std::memcpy(&vehicleGpsPosition.s_variance_m_s, currentData, sizeof(vehicleGpsPosition.s_variance_m_s)); | |
| 647 | LOG_DATA("{}: vehicleGpsPosition.s_variance_m_s: {}", nameId(), vehicleGpsPosition.s_variance_m_s); | ||
| 648 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.s_variance_m_s); | |
| 649 | } | ||
| 650 | ✗ | else if (dataField.name == "c_variance_rad") | |
| 651 | { | ||
| 652 | ✗ | std::memcpy(&vehicleGpsPosition.c_variance_rad, currentData, sizeof(vehicleGpsPosition.c_variance_rad)); | |
| 653 | LOG_DATA("{}: vehicleGpsPosition.c_variance_rad: {}", nameId(), vehicleGpsPosition.c_variance_rad); | ||
| 654 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.c_variance_rad); | |
| 655 | } | ||
| 656 | ✗ | else if (dataField.name == "eph") | |
| 657 | { | ||
| 658 | ✗ | std::memcpy(&vehicleGpsPosition.eph, currentData, sizeof(vehicleGpsPosition.eph)); | |
| 659 | LOG_DATA("{}: vehicleGpsPosition.eph: {}", nameId(), vehicleGpsPosition.eph); | ||
| 660 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.eph); | |
| 661 | } | ||
| 662 | ✗ | else if (dataField.name == "epv") | |
| 663 | { | ||
| 664 | ✗ | std::memcpy(&vehicleGpsPosition.epv, currentData, sizeof(vehicleGpsPosition.epv)); | |
| 665 | LOG_DATA("{}: vehicleGpsPosition.epv: {}", nameId(), vehicleGpsPosition.epv); | ||
| 666 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.epv); | |
| 667 | } | ||
| 668 | ✗ | else if (dataField.name == "hdop") | |
| 669 | { | ||
| 670 | ✗ | std::memcpy(&vehicleGpsPosition.hdop, currentData, sizeof(vehicleGpsPosition.hdop)); | |
| 671 | LOG_DATA("{}: vehicleGpsPosition.hdop: {}", nameId(), vehicleGpsPosition.hdop); | ||
| 672 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.hdop); | |
| 673 | } | ||
| 674 | ✗ | else if (dataField.name == "vdop") | |
| 675 | { | ||
| 676 | ✗ | std::memcpy(&vehicleGpsPosition.vdop, currentData, sizeof(vehicleGpsPosition.lat)); | |
| 677 | LOG_DATA("{}: vehicleGpsPosition.vdop: {}", nameId(), vehicleGpsPosition.vdop); | ||
| 678 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.vdop); | |
| 679 | } | ||
| 680 | ✗ | else if (dataField.name == "noise_per_ms") | |
| 681 | { | ||
| 682 | ✗ | std::memcpy(&vehicleGpsPosition.noise_per_ms, currentData, sizeof(vehicleGpsPosition.noise_per_ms)); | |
| 683 | LOG_DATA("{}: vehicleGpsPosition.noise_per_ms: {}", nameId(), vehicleGpsPosition.noise_per_ms); | ||
| 684 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.noise_per_ms); | |
| 685 | } | ||
| 686 | ✗ | else if (dataField.name == "jamming_indicator") | |
| 687 | { | ||
| 688 | ✗ | std::memcpy(&vehicleGpsPosition.jamming_indicator, currentData, sizeof(vehicleGpsPosition.jamming_indicator)); | |
| 689 | LOG_DATA("{}: vehicleGpsPosition.jamming_indicator: {}", nameId(), vehicleGpsPosition.jamming_indicator); | ||
| 690 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.jamming_indicator); | |
| 691 | } | ||
| 692 | ✗ | else if (dataField.name == "vel_m_s") | |
| 693 | { | ||
| 694 | ✗ | std::memcpy(&vehicleGpsPosition.vel_m_s, currentData, sizeof(vehicleGpsPosition.vel_m_s)); | |
| 695 | LOG_DATA("{}: vehicleGpsPosition.vel_m_s: {}", nameId(), vehicleGpsPosition.vel_m_s); | ||
| 696 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.vel_m_s); | |
| 697 | } | ||
| 698 | ✗ | else if (dataField.name == "vel_n_m_s") | |
| 699 | { | ||
| 700 | ✗ | std::memcpy(&vehicleGpsPosition.vel_n_m_s, currentData, sizeof(vehicleGpsPosition.vel_n_m_s)); | |
| 701 | LOG_DATA("{}: vehicleGpsPosition.vel_n_m_s: {}", nameId(), vehicleGpsPosition.vel_n_m_s); | ||
| 702 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.vel_n_m_s); | |
| 703 | } | ||
| 704 | ✗ | else if (dataField.name == "vel_e_m_s") | |
| 705 | { | ||
| 706 | ✗ | std::memcpy(&vehicleGpsPosition.vel_e_m_s, currentData, sizeof(vehicleGpsPosition.vel_e_m_s)); | |
| 707 | LOG_DATA("{}: vehicleGpsPosition.vel_e_m_s: {}", nameId(), vehicleGpsPosition.vel_e_m_s); | ||
| 708 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.vel_e_m_s); | |
| 709 | } | ||
| 710 | ✗ | else if (dataField.name == "vel_d_m_s") | |
| 711 | { | ||
| 712 | ✗ | std::memcpy(&vehicleGpsPosition.vel_d_m_s, currentData, sizeof(vehicleGpsPosition.vel_d_m_s)); | |
| 713 | LOG_DATA("{}: vehicleGpsPosition.vel_d_m_s: {}", nameId(), vehicleGpsPosition.vel_d_m_s); | ||
| 714 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.vel_d_m_s); | |
| 715 | } | ||
| 716 | ✗ | else if (dataField.name == "cog_rad") | |
| 717 | { | ||
| 718 | ✗ | std::memcpy(&vehicleGpsPosition.cog_rad, currentData, sizeof(vehicleGpsPosition.cog_rad)); | |
| 719 | LOG_DATA("{}: vehicleGpsPosition.cog_rad: {}", nameId(), vehicleGpsPosition.cog_rad); | ||
| 720 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.cog_rad); | |
| 721 | } | ||
| 722 | ✗ | else if (dataField.name == "timestamp_time_relative") | |
| 723 | { | ||
| 724 | ✗ | std::memcpy(&vehicleGpsPosition.timestamp_time_relative, currentData, sizeof(vehicleGpsPosition.timestamp_time_relative)); | |
| 725 | LOG_DATA("{}: vehicleGpsPosition.timestamp_time_relative: {}", nameId(), vehicleGpsPosition.timestamp_time_relative); | ||
| 726 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.timestamp_time_relative); | |
| 727 | } | ||
| 728 | ✗ | else if (dataField.name == "heading") | |
| 729 | { | ||
| 730 | ✗ | std::memcpy(&vehicleGpsPosition.heading, currentData, sizeof(vehicleGpsPosition.heading)); | |
| 731 | LOG_DATA("{}: vehicleGpsPosition.heading: {}", nameId(), vehicleGpsPosition.heading); | ||
| 732 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.heading); | |
| 733 | } | ||
| 734 | ✗ | else if (dataField.name == "heading_offset") | |
| 735 | { | ||
| 736 | ✗ | std::memcpy(&vehicleGpsPosition.heading_offset, currentData, sizeof(vehicleGpsPosition.heading_offset)); | |
| 737 | LOG_DATA("{}: vehicleGpsPosition.heading_offset: {}", nameId(), vehicleGpsPosition.heading_offset); | ||
| 738 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.heading_offset); | |
| 739 | } | ||
| 740 | ✗ | else if (dataField.name == "fix_type") | |
| 741 | { | ||
| 742 | ✗ | std::memcpy(&vehicleGpsPosition.fix_type, currentData, sizeof(vehicleGpsPosition.fix_type)); | |
| 743 | LOG_DATA("{}: vehicleGpsPosition.fix_type: {}", nameId(), vehicleGpsPosition.fix_type); | ||
| 744 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.fix_type); | |
| 745 | } | ||
| 746 | ✗ | else if (dataField.name == "vel_ned_valid") | |
| 747 | { | ||
| 748 | ✗ | std::memcpy(&vehicleGpsPosition.vel_ned_valid, currentData, sizeof(vehicleGpsPosition.vel_ned_valid)); | |
| 749 | LOG_DATA("{}: vehicleGpsPosition.vel_ned_valid: {}", nameId(), vehicleGpsPosition.vel_ned_valid); | ||
| 750 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.vel_ned_valid); | |
| 751 | } | ||
| 752 | ✗ | else if (dataField.name == "satellites_used") | |
| 753 | { | ||
| 754 | ✗ | std::memcpy(&vehicleGpsPosition.satellites_used, currentData, sizeof(vehicleGpsPosition.satellites_used)); | |
| 755 | LOG_DATA("{}: vehicleGpsPosition.satellites_used: {}", nameId(), vehicleGpsPosition.satellites_used); | ||
| 756 | ✗ | currentExtractLocation += sizeof(vehicleGpsPosition.satellites_used); | |
| 757 | } | ||
| 758 | ✗ | else if (dataField.name.compare(0, 7, "_padding")) // e.g. '_padding0', '_padding1' | |
| 759 | { | ||
| 760 | ✗ | currentExtractLocation += VehicleGpsPosition::padding; // Extraction Location should be moved to account for multiple padding | |
| 761 | LOG_DATA("{}: vehicleGpsPosition: padding", nameId()); | ||
| 762 | } | ||
| 763 | else | ||
| 764 | { | ||
| 765 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 766 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 767 | } | ||
| 768 | } | ||
| 769 | |||
| 770 | ✗ | if (lastGnssTime.timeSinceStartup) | |
| 771 | { | ||
| 772 | ✗ | [[maybe_unused]] auto newGnssTime = InsTime(1970, 1, 1, 0, 0, vehicleGpsPosition.time_utc_usec * 1e-6L); | |
| 773 | LOG_DATA("{}: Updating GnssTime from {} to {} (Diff {} sec)", nameId(), lastGnssTime.gnssTime.toYMDHMS(), newGnssTime.toYMDHMS(), static_cast<double>((newGnssTime - lastGnssTime.gnssTime).count())); | ||
| 774 | LOG_DATA("{}: Updating tStartup from {} to {} (Diff {} sec)", nameId(), lastGnssTime.timeSinceStartup, vehicleGpsPosition.timestamp, (vehicleGpsPosition.timestamp - lastGnssTime.timeSinceStartup) * 1e-6L); | ||
| 775 | } | ||
| 776 | |||
| 777 | ✗ | lastGnssTime.gnssTime = InsTime(1970, 1, 1, 0, 0, vehicleGpsPosition.time_utc_usec * 1e-6L); | |
| 778 | ✗ | lastGnssTime.timeSinceStartup = vehicleGpsPosition.timestamp; | |
| 779 | |||
| 780 | while (true) // Delete all old VehicleGpsPosition entries | ||
| 781 | { | ||
| 782 | ✗ | auto iter = std::ranges::find_if(_epochData, [](const std::pair<uint64_t, MeasurementData>& v) { | |
| 783 | ✗ | return std::holds_alternative<UlogFile::VehicleGpsPosition>(v.second.data); | |
| 784 | }); | ||
| 785 | ✗ | if (iter == _epochData.end()) | |
| 786 | { | ||
| 787 | ✗ | break; | |
| 788 | } | ||
| 789 | ✗ | _epochData.erase(iter); | |
| 790 | ✗ | } | |
| 791 | |||
| 792 | LOG_DATA("{}: Inserting [{}] {}: {}", nameId(), vehicleGpsPosition.timestamp, | ||
| 793 | _subscribedMessages.at(messageData.msg_id).multi_id, | ||
| 794 | _subscribedMessages.at(messageData.msg_id).message_name); | ||
| 795 | |||
| 796 | ✗ | _epochData.insert(std::make_pair(vehicleGpsPosition.timestamp, | |
| 797 | ✗ | MeasurementData{ .multi_id = _subscribedMessages.at(messageData.msg_id).multi_id, | |
| 798 | ✗ | .message_name = _subscribedMessages.at(messageData.msg_id).message_name, | |
| 799 | .data = vehicleGpsPosition })); | ||
| 800 | } | ||
| 801 | ✗ | else if (_subscribedMessages.at(messageData.msg_id).message_name == "vehicle_attitude") | |
| 802 | { | ||
| 803 | ✗ | VehicleAttitude vehicleAttitude{}; | |
| 804 | ✗ | for (const auto& dataField : messageFormat) | |
| 805 | { | ||
| 806 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 807 | ✗ | if (dataField.name == "timestamp") | |
| 808 | { | ||
| 809 | ✗ | std::memcpy(&vehicleAttitude.timestamp, currentData, sizeof(vehicleAttitude.timestamp)); | |
| 810 | LOG_DATA("{}: vehicleAttitude.timestamp: {}", nameId(), vehicleAttitude.timestamp); | ||
| 811 | ✗ | currentExtractLocation += sizeof(vehicleAttitude.timestamp); | |
| 812 | } | ||
| 813 | ✗ | else if (dataField.name == "q") | |
| 814 | { | ||
| 815 | ✗ | std::memcpy(vehicleAttitude.q.data(), currentData, vehicleAttitude.q.size()); | |
| 816 | LOG_DATA("{}: vehicleAttitude.q: {}", nameId(), fmt::join(vehicleAttitude.q, ", ")); | ||
| 817 | ✗ | currentExtractLocation += vehicleAttitude.q.size(); | |
| 818 | } | ||
| 819 | ✗ | else if (dataField.name == "delta_q_reset") | |
| 820 | { | ||
| 821 | ✗ | std::memcpy(vehicleAttitude.delta_q_reset.data(), currentData, vehicleAttitude.delta_q_reset.size()); | |
| 822 | LOG_DATA("{}: vehicleAttitude.delta_q_reset: {}", nameId(), fmt::join(vehicleAttitude.delta_q_reset, ", ")); | ||
| 823 | ✗ | currentExtractLocation += vehicleAttitude.delta_q_reset.size(); | |
| 824 | } | ||
| 825 | ✗ | if (dataField.name == "quat_reset_counter") | |
| 826 | { | ||
| 827 | ✗ | std::memcpy(&vehicleAttitude.quat_reset_counter, currentData, sizeof(vehicleAttitude.quat_reset_counter)); | |
| 828 | LOG_DATA("{}: vehicleAttitude.quat_reset_counter: {}", nameId(), vehicleAttitude.quat_reset_counter); | ||
| 829 | ✗ | currentExtractLocation += sizeof(vehicleAttitude.quat_reset_counter); | |
| 830 | } | ||
| 831 | ✗ | else if (dataField.name.compare(0, 7, "_padding")) // e.g. '_padding0', '_padding1' | |
| 832 | { | ||
| 833 | ✗ | currentExtractLocation += VehicleAttitude::padding; // Extraction Location should be moved to account for multiple padding | |
| 834 | LOG_DATA("{}: VehicleAttitude: padding", nameId()); | ||
| 835 | } | ||
| 836 | else | ||
| 837 | { | ||
| 838 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 839 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 840 | } | ||
| 841 | } | ||
| 842 | |||
| 843 | while (true) // Delete all old VehicleAttitude entries | ||
| 844 | { | ||
| 845 | ✗ | auto iter = std::ranges::find_if(_epochData, [](const std::pair<uint64_t, MeasurementData>& v) { | |
| 846 | ✗ | return std::holds_alternative<UlogFile::VehicleAttitude>(v.second.data); | |
| 847 | }); | ||
| 848 | ✗ | if (iter == _epochData.end()) | |
| 849 | { | ||
| 850 | ✗ | break; | |
| 851 | } | ||
| 852 | ✗ | _epochData.erase(iter); | |
| 853 | ✗ | } | |
| 854 | |||
| 855 | LOG_DATA("{}: Inserting [{}] {}: {}", nameId(), vehicleAttitude.timestamp, | ||
| 856 | _subscribedMessages.at(messageData.msg_id).multi_id, | ||
| 857 | _subscribedMessages.at(messageData.msg_id).message_name); | ||
| 858 | |||
| 859 | ✗ | _epochData.insert(std::make_pair(vehicleAttitude.timestamp, | |
| 860 | ✗ | MeasurementData{ .multi_id = _subscribedMessages.at(messageData.msg_id).multi_id, | |
| 861 | ✗ | .message_name = _subscribedMessages.at(messageData.msg_id).message_name, | |
| 862 | .data = vehicleAttitude })); | ||
| 863 | } | ||
| 864 | ✗ | else if (_subscribedMessages.at(messageData.msg_id).message_name == "vehicle_control_mode") | |
| 865 | { | ||
| 866 | ✗ | VehicleControlMode vehicleControlMode{}; | |
| 867 | ✗ | for (const auto& dataField : messageFormat) | |
| 868 | { | ||
| 869 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 870 | ✗ | if (dataField.name == "timestamp") | |
| 871 | { | ||
| 872 | ✗ | std::memcpy(&vehicleControlMode.timestamp, currentData, sizeof(vehicleControlMode.timestamp)); | |
| 873 | LOG_DATA("{}: vehicleControlMode.timestamp: {}", nameId(), vehicleControlMode.timestamp); | ||
| 874 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.timestamp); | |
| 875 | } | ||
| 876 | ✗ | else if (dataField.name == "flag_armed") | |
| 877 | { | ||
| 878 | ✗ | std::memcpy(&vehicleControlMode.flag_armed, currentData, sizeof(vehicleControlMode.flag_armed)); | |
| 879 | LOG_DATA("{}: vehicleControlMode.flag_armed: {}", nameId(), vehicleControlMode.flag_armed); | ||
| 880 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_armed); | |
| 881 | } | ||
| 882 | ✗ | else if (dataField.name == "flag_external_manual_override_ok") | |
| 883 | { | ||
| 884 | ✗ | std::memcpy(&vehicleControlMode.flag_external_manual_override_ok, currentData, sizeof(vehicleControlMode.flag_external_manual_override_ok)); | |
| 885 | LOG_DATA("{}: vehicleControlMode.flag_external_manual_override_ok: {}", nameId(), vehicleControlMode.flag_external_manual_override_ok); | ||
| 886 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_external_manual_override_ok); | |
| 887 | } | ||
| 888 | ✗ | else if (dataField.name == "flag_control_manual_enabled") | |
| 889 | { | ||
| 890 | ✗ | std::memcpy(&vehicleControlMode.flag_control_manual_enabled, currentData, sizeof(vehicleControlMode.flag_control_manual_enabled)); | |
| 891 | LOG_DATA("{}: vehicleControlMode.flag_control_manual_enabled: {}", nameId(), vehicleControlMode.flag_control_manual_enabled); | ||
| 892 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_manual_enabled); | |
| 893 | } | ||
| 894 | ✗ | else if (dataField.name == "flag_control_auto_enabled") | |
| 895 | { | ||
| 896 | ✗ | std::memcpy(&vehicleControlMode.flag_control_auto_enabled, currentData, sizeof(vehicleControlMode.flag_control_auto_enabled)); | |
| 897 | LOG_DATA("{}: vehicleControlMode.flag_control_auto_enabled: {}", nameId(), vehicleControlMode.flag_control_auto_enabled); | ||
| 898 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_auto_enabled); | |
| 899 | } | ||
| 900 | ✗ | else if (dataField.name == "flag_control_offboard_enabled") | |
| 901 | { | ||
| 902 | ✗ | std::memcpy(&vehicleControlMode.flag_control_offboard_enabled, currentData, sizeof(vehicleControlMode.flag_control_offboard_enabled)); | |
| 903 | LOG_DATA("{}: vehicleControlMode.flag_control_offboard_enabled: {}", nameId(), vehicleControlMode.flag_control_offboard_enabled); | ||
| 904 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_offboard_enabled); | |
| 905 | } | ||
| 906 | ✗ | else if (dataField.name == "flag_control_rates_enabled") | |
| 907 | { | ||
| 908 | ✗ | std::memcpy(&vehicleControlMode.flag_control_rates_enabled, currentData, sizeof(vehicleControlMode.flag_control_rates_enabled)); | |
| 909 | LOG_DATA("{}: vehicleControlMode.flag_control_rates_enabled: {}", nameId(), vehicleControlMode.flag_control_rates_enabled); | ||
| 910 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_rates_enabled); | |
| 911 | } | ||
| 912 | ✗ | else if (dataField.name == "flag_control_attitude_enabled") | |
| 913 | { | ||
| 914 | ✗ | std::memcpy(&vehicleControlMode.flag_control_attitude_enabled, currentData, sizeof(vehicleControlMode.flag_control_attitude_enabled)); | |
| 915 | LOG_DATA("{}: vehicleControlMode.flag_control_attitude_enabled: {}", nameId(), vehicleControlMode.flag_control_attitude_enabled); | ||
| 916 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_attitude_enabled); | |
| 917 | } | ||
| 918 | ✗ | else if (dataField.name == "flag_control_yawrate_override_enabled") | |
| 919 | { | ||
| 920 | ✗ | std::memcpy(&vehicleControlMode.flag_control_yawrate_override_enabled, currentData, sizeof(vehicleControlMode.flag_control_yawrate_override_enabled)); | |
| 921 | LOG_DATA("{}: vehicleControlMode.flag_control_yawrate_override_enabled: {}", nameId(), vehicleControlMode.flag_control_yawrate_override_enabled); | ||
| 922 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_yawrate_override_enabled); | |
| 923 | } | ||
| 924 | ✗ | else if (dataField.name == "flag_control_rattitude_enabled") | |
| 925 | { | ||
| 926 | ✗ | std::memcpy(&vehicleControlMode.flag_control_rattitude_enabled, currentData, sizeof(vehicleControlMode.flag_control_rattitude_enabled)); | |
| 927 | LOG_DATA("{}: vehicleControlMode.flag_control_rattitude_enabled: {}", nameId(), vehicleControlMode.flag_control_rattitude_enabled); | ||
| 928 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_rattitude_enabled); | |
| 929 | } | ||
| 930 | ✗ | else if (dataField.name == "flag_control_force_enabled") | |
| 931 | { | ||
| 932 | ✗ | std::memcpy(&vehicleControlMode.flag_control_force_enabled, currentData, sizeof(vehicleControlMode.flag_control_force_enabled)); | |
| 933 | LOG_DATA("{}: vehicleControlMode.flag_control_force_enabled: {}", nameId(), vehicleControlMode.flag_control_force_enabled); | ||
| 934 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_force_enabled); | |
| 935 | } | ||
| 936 | ✗ | else if (dataField.name == "flag_control_acceleration_enabled") | |
| 937 | { | ||
| 938 | ✗ | std::memcpy(&vehicleControlMode.flag_control_acceleration_enabled, currentData, sizeof(vehicleControlMode.flag_control_acceleration_enabled)); | |
| 939 | LOG_DATA("{}: vehicleControlMode.flag_control_acceleration_enabled: {}", nameId(), vehicleControlMode.flag_control_acceleration_enabled); | ||
| 940 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_acceleration_enabled); | |
| 941 | } | ||
| 942 | ✗ | else if (dataField.name == "flag_control_velocity_enabled") | |
| 943 | { | ||
| 944 | ✗ | std::memcpy(&vehicleControlMode.flag_control_velocity_enabled, currentData, sizeof(vehicleControlMode.flag_control_velocity_enabled)); | |
| 945 | LOG_DATA("{}: vehicleControlMode.flag_control_velocity_enabled: {}", nameId(), vehicleControlMode.flag_control_velocity_enabled); | ||
| 946 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_velocity_enabled); | |
| 947 | } | ||
| 948 | ✗ | else if (dataField.name == "flag_control_position_enabled") | |
| 949 | { | ||
| 950 | ✗ | std::memcpy(&vehicleControlMode.flag_control_position_enabled, currentData, sizeof(vehicleControlMode.flag_control_position_enabled)); | |
| 951 | LOG_DATA("{}: vehicleControlMode.flag_control_position_enabled: {}", nameId(), vehicleControlMode.flag_control_position_enabled); | ||
| 952 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_position_enabled); | |
| 953 | } | ||
| 954 | ✗ | else if (dataField.name == "flag_control_altitude_enabled") | |
| 955 | { | ||
| 956 | ✗ | std::memcpy(&vehicleControlMode.flag_control_altitude_enabled, currentData, sizeof(vehicleControlMode.flag_control_altitude_enabled)); | |
| 957 | LOG_DATA("{}: vehicleControlMode.flag_control_altitude_enabled: {}", nameId(), vehicleControlMode.flag_control_altitude_enabled); | ||
| 958 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_altitude_enabled); | |
| 959 | } | ||
| 960 | ✗ | else if (dataField.name == "flag_control_climb_rate_enabled") | |
| 961 | { | ||
| 962 | ✗ | std::memcpy(&vehicleControlMode.flag_control_climb_rate_enabled, currentData, sizeof(vehicleControlMode.flag_control_climb_rate_enabled)); | |
| 963 | LOG_DATA("{}: vehicleControlMode.flag_control_climb_rate_enabled: {}", nameId(), vehicleControlMode.flag_control_climb_rate_enabled); | ||
| 964 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_climb_rate_enabled); | |
| 965 | } | ||
| 966 | ✗ | else if (dataField.name == "flag_control_termination_enabled") | |
| 967 | { | ||
| 968 | ✗ | std::memcpy(&vehicleControlMode.flag_control_termination_enabled, currentData, sizeof(vehicleControlMode.flag_control_termination_enabled)); | |
| 969 | LOG_DATA("{}: vehicleControlMode.flag_control_termination_enabled: {}", nameId(), vehicleControlMode.flag_control_termination_enabled); | ||
| 970 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_termination_enabled); | |
| 971 | } | ||
| 972 | ✗ | else if (dataField.name == "flag_control_fixed_hdg_enabled") | |
| 973 | { | ||
| 974 | ✗ | std::memcpy(&vehicleControlMode.flag_control_fixed_hdg_enabled, currentData, sizeof(vehicleControlMode.flag_control_fixed_hdg_enabled)); | |
| 975 | LOG_DATA("{}: vehicleControlMode.flag_control_fixed_hdg_enabled: {}", nameId(), vehicleControlMode.flag_control_fixed_hdg_enabled); | ||
| 976 | ✗ | currentExtractLocation += sizeof(vehicleControlMode.flag_control_fixed_hdg_enabled); | |
| 977 | } | ||
| 978 | ✗ | else if (dataField.name.compare(0, 7, "_padding")) // e.g. '_padding0', '_padding1' | |
| 979 | { | ||
| 980 | ✗ | currentExtractLocation += VehicleControlMode::padding; // Extraction Location should be moved to account for multiple padding | |
| 981 | LOG_DATA("{}: VehicleControlMode: padding", nameId()); | ||
| 982 | } | ||
| 983 | else | ||
| 984 | { | ||
| 985 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 986 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 987 | } | ||
| 988 | } | ||
| 989 | // TODO: insert to _epochData necessary for 'vehicleControlMode'? | ||
| 990 | } | ||
| 991 | ✗ | else if (_subscribedMessages.at(messageData.msg_id).message_name == "vehicle_air_data") | |
| 992 | { | ||
| 993 | ✗ | VehicleAirData vehicleAirData{}; | |
| 994 | ✗ | for (const auto& dataField : messageFormat) | |
| 995 | { | ||
| 996 | ✗ | char* currentData = messageData.data.data() + currentExtractLocation; | |
| 997 | ✗ | if (dataField.name == "timestamp") | |
| 998 | { | ||
| 999 | ✗ | std::memcpy(&vehicleAirData.timestamp, currentData, sizeof(vehicleAirData.timestamp)); | |
| 1000 | LOG_DATA("{}: vehicleAirData.timestamp: {}", nameId(), vehicleAirData.timestamp); | ||
| 1001 | ✗ | currentExtractLocation += sizeof(vehicleAirData.timestamp); | |
| 1002 | } | ||
| 1003 | ✗ | else if (dataField.name == "timestamp_sample") | |
| 1004 | { | ||
| 1005 | ✗ | std::memcpy(&vehicleAirData.timestamp_sample, currentData, sizeof(vehicleAirData.timestamp_sample)); | |
| 1006 | LOG_DATA("{}: vehicleAirData.timestamp_sample: {}", nameId(), vehicleAirData.timestamp_sample); | ||
| 1007 | ✗ | currentExtractLocation += sizeof(vehicleAirData.timestamp_sample); | |
| 1008 | } | ||
| 1009 | ✗ | else if (dataField.name == "baro_device_id") | |
| 1010 | { | ||
| 1011 | ✗ | std::memcpy(&vehicleAirData.baro_device_id, currentData, sizeof(vehicleAirData.baro_device_id)); | |
| 1012 | LOG_DATA("{}: vehicleAirData.baro_device_id: {}", nameId(), vehicleAirData.baro_device_id); | ||
| 1013 | ✗ | currentExtractLocation += sizeof(vehicleAirData.baro_device_id); | |
| 1014 | } | ||
| 1015 | ✗ | else if (dataField.name == "baro_alt_meter") | |
| 1016 | { | ||
| 1017 | ✗ | std::memcpy(&vehicleAirData.baro_alt_meter, currentData, sizeof(vehicleAirData.baro_alt_meter)); | |
| 1018 | LOG_DATA("{}: vehicleAirData.baro_alt_meter: {}", nameId(), vehicleAirData.baro_alt_meter); | ||
| 1019 | ✗ | currentExtractLocation += sizeof(vehicleAirData.baro_alt_meter); | |
| 1020 | } | ||
| 1021 | ✗ | else if (dataField.name == "baro_temp_celcius") | |
| 1022 | { | ||
| 1023 | ✗ | std::memcpy(&vehicleAirData.baro_temp_celcius, currentData, sizeof(vehicleAirData.baro_temp_celcius)); | |
| 1024 | LOG_DATA("{}: vehicleAirData.baro_temp_celcius: {}", nameId(), vehicleAirData.baro_temp_celcius); | ||
| 1025 | ✗ | currentExtractLocation += sizeof(vehicleAirData.baro_temp_celcius); | |
| 1026 | } | ||
| 1027 | ✗ | else if (dataField.name == "baro_pressure_pa") | |
| 1028 | { | ||
| 1029 | ✗ | std::memcpy(&vehicleAirData.baro_pressure_pa, currentData, sizeof(vehicleAirData.baro_pressure_pa)); | |
| 1030 | LOG_DATA("{}: vehicleAirData.baro_pressure_pa: {}", nameId(), vehicleAirData.baro_pressure_pa); | ||
| 1031 | ✗ | currentExtractLocation += sizeof(vehicleAirData.baro_pressure_pa); | |
| 1032 | } | ||
| 1033 | ✗ | else if (dataField.name == "rho") | |
| 1034 | { | ||
| 1035 | ✗ | std::memcpy(&vehicleAirData.rho, currentData, sizeof(vehicleAirData.rho)); | |
| 1036 | LOG_DATA("{}: vehicleAirData.rho: {}", nameId(), vehicleAirData.rho); | ||
| 1037 | ✗ | currentExtractLocation += sizeof(vehicleAirData.rho); | |
| 1038 | } | ||
| 1039 | ✗ | else if (dataField.name.compare(0, 7, "_padding")) // e.g. '_padding0', '_padding1' | |
| 1040 | { | ||
| 1041 | ✗ | currentExtractLocation += VehicleAirData::padding; // Extraction Location should be moved to account for multiple padding | |
| 1042 | LOG_DATA("{}: VehicleControlMode: padding", nameId()); | ||
| 1043 | } | ||
| 1044 | else | ||
| 1045 | { | ||
| 1046 | // FIXME: move 'currentExtractLocation', if yes, how far? | ||
| 1047 | ✗ | LOG_WARN("{}: dataField.name = '{}' or dataField.type = '{}' is unknown", nameId(), dataField.name, dataField.type); | |
| 1048 | } | ||
| 1049 | } | ||
| 1050 | // TODO: insert to _epochData necessary for 'vehicleAirData'? | ||
| 1051 | } | ||
| 1052 | else | ||
| 1053 | { | ||
| 1054 | LOG_DATA("{}: UKNOWN: _subscribedMessages.at(messageData.msg_id).message_name = {}", nameId(), _subscribedMessages.at(messageData.msg_id).message_name); | ||
| 1055 | } | ||
| 1056 | |||
| 1057 | // ######################################################################################################################################### | ||
| 1058 | // Callbacks | ||
| 1059 | // ######################################################################################################################################### | ||
| 1060 | // This is the hasEnoughData check for an ImuObs | ||
| 1061 | ✗ | if (auto multi_id = enoughImuDataAvailable(); | |
| 1062 | ✗ | multi_id >= 0) | |
| 1063 | { | ||
| 1064 | LOG_DATA("{}: Construct ImuObs and invoke callback", nameId()); | ||
| 1065 | |||
| 1066 | ✗ | auto obs = std::make_shared<ImuObs>(this->_imuPos); | |
| 1067 | |||
| 1068 | ✗ | uint64_t timeSinceStartupNew{}; | |
| 1069 | |||
| 1070 | ✗ | for ([[maybe_unused]] const auto& [timestamp, measurement] : _epochData) | |
| 1071 | { | ||
| 1072 | LOG_DATA("{}: [{}] {}: {}", nameId(), timestamp, measurement.multi_id, measurement.message_name); | ||
| 1073 | } | ||
| 1074 | |||
| 1075 | ✗ | bool accelFound = false; | |
| 1076 | ✗ | bool angularRateFound = false; | |
| 1077 | // Construct ImuObs | ||
| 1078 | ✗ | for (auto it = _epochData.begin(); it != _epochData.end();) | |
| 1079 | { | ||
| 1080 | // Add accel data to ImuObs | ||
| 1081 | ✗ | if (std::holds_alternative<SensorAccel>(it->second.data) && (it->second.multi_id == static_cast<uint8_t>(multi_id)) | |
| 1082 | ✗ | && !accelFound) | |
| 1083 | { | ||
| 1084 | ✗ | accelFound = true; | |
| 1085 | ✗ | timeSinceStartupNew = it->first; | |
| 1086 | ✗ | float accelX = std::get<SensorAccel>(it->second.data).x; | |
| 1087 | ✗ | float accelY = std::get<SensorAccel>(it->second.data).y; | |
| 1088 | ✗ | float accelZ = std::get<SensorAccel>(it->second.data).z; | |
| 1089 | ✗ | obs->p_acceleration = { accelX, accelY, accelZ }; | |
| 1090 | ✗ | auto delIt = it; | |
| 1091 | ✗ | ++it; | |
| 1092 | ✗ | _epochData.erase(delIt); | |
| 1093 | LOG_DATA("{}: accelX = {}, accelY = {}, accelZ = {}", nameId(), accelX, accelY, accelZ); | ||
| 1094 | } | ||
| 1095 | // Add gyro data to ImuObs | ||
| 1096 | ✗ | else if (std::holds_alternative<SensorGyro>(it->second.data) && (it->second.multi_id == static_cast<uint8_t>(multi_id)) | |
| 1097 | ✗ | && !angularRateFound) | |
| 1098 | { | ||
| 1099 | ✗ | angularRateFound = true; | |
| 1100 | ✗ | timeSinceStartupNew = it->first; | |
| 1101 | ✗ | float gyroX = std::get<SensorGyro>(it->second.data).x; | |
| 1102 | ✗ | float gyroY = std::get<SensorGyro>(it->second.data).y; | |
| 1103 | ✗ | float gyroZ = std::get<SensorGyro>(it->second.data).z; | |
| 1104 | ✗ | obs->p_angularRate = { gyroX, gyroY, gyroZ }; | |
| 1105 | ✗ | auto delIt = it; | |
| 1106 | ✗ | ++it; | |
| 1107 | ✗ | _epochData.erase(delIt); | |
| 1108 | LOG_DATA("{}: gyroX = {}, gyroY = {}, gyroZ = {}", nameId(), gyroX, gyroY, gyroZ); | ||
| 1109 | } | ||
| 1110 | // Add mag data to ImuObs | ||
| 1111 | ✗ | else if (std::holds_alternative<SensorMag>(it->second.data) && (it->second.multi_id == static_cast<uint8_t>(multi_id)) | |
| 1112 | ✗ | && !obs->p_magneticField.has_value()) // TODO: Find out what is multi_id = 1. Px4 Mini is supposed to have only one magnetometer | |
| 1113 | { | ||
| 1114 | ✗ | float magX = std::get<SensorMag>(it->second.data).x; | |
| 1115 | ✗ | float magY = std::get<SensorMag>(it->second.data).y; | |
| 1116 | ✗ | float magZ = std::get<SensorMag>(it->second.data).z; | |
| 1117 | ✗ | obs->p_magneticField.emplace(magX, magY, magZ); | |
| 1118 | ✗ | auto delIt = it; | |
| 1119 | ✗ | ++it; | |
| 1120 | ✗ | _epochData.erase(delIt); | |
| 1121 | LOG_DATA("{}: magX = {}, magY = {}, magZ = {}", nameId(), magX, magY, magZ); | ||
| 1122 | } | ||
| 1123 | else | ||
| 1124 | { | ||
| 1125 | ✗ | ++it; | |
| 1126 | } | ||
| 1127 | } | ||
| 1128 | |||
| 1129 | ✗ | obs->insTime = lastGnssTime.gnssTime + std::chrono::microseconds(static_cast<int64_t>(timeSinceStartupNew) - static_cast<int64_t>(lastGnssTime.timeSinceStartup)); | |
| 1130 | |||
| 1131 | ✗ | obs->timeSinceStartup = 1000 * timeSinceStartupNew; // latest timestamp in [ns] | |
| 1132 | LOG_DATA("{}: *obs->timeSinceStartup = {} s", nameId(), static_cast<double>(*obs->timeSinceStartup) * 1e-9); | ||
| 1133 | |||
| 1134 | LOG_DATA("{}: Sending out ImuObs {}: {} - {}", nameId(), multi_id, obs->insTime.toYMDHMS(), obs->timeSinceStartup.value()); | ||
| 1135 | ✗ | if (multi_id == 0) | |
| 1136 | { | ||
| 1137 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_IMUOBS_1, obs); | |
| 1138 | } | ||
| 1139 | ✗ | else if (multi_id == 1) | |
| 1140 | { | ||
| 1141 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_IMUOBS_2, obs); | |
| 1142 | } | ||
| 1143 | else | ||
| 1144 | { | ||
| 1145 | ✗ | LOG_ERROR("{}: multi_id = {} is invalid", nameId(), multi_id); | |
| 1146 | } | ||
| 1147 | ✗ | return obs; | |
| 1148 | ✗ | } | |
| 1149 | ✗ | if (auto [gpsIter, attIter] = findPosVelAttData(); | |
| 1150 | ✗ | gpsIter != _epochData.end() && attIter != _epochData.end()) | |
| 1151 | { | ||
| 1152 | LOG_DATA("{}: Construct PosVelAtt and invoke callback", nameId()); | ||
| 1153 | |||
| 1154 | ✗ | auto obs = std::make_shared<NAV::PosVelAtt>(); | |
| 1155 | |||
| 1156 | ✗ | const auto& vehicleGpsPosition = std::get<VehicleGpsPosition>(gpsIter->second.data); | |
| 1157 | ✗ | const auto& vehicleAttitude = std::get<VehicleAttitude>(attIter->second.data); | |
| 1158 | |||
| 1159 | ✗ | obs->insTime = InsTime(0, 0, 0, 0, 0, vehicleGpsPosition.time_utc_usec * 1e-6L); | |
| 1160 | ✗ | obs->setState_n(Eigen::Vector3d{ deg2rad(1e-7 * static_cast<double>(vehicleGpsPosition.lat)), deg2rad(1e-7 * static_cast<double>(vehicleGpsPosition.lon)), 1e-3 * (static_cast<double>(vehicleGpsPosition.alt_ellipsoid)) }, | |
| 1161 | ✗ | Eigen::Vector3d{ vehicleGpsPosition.vel_n_m_s, vehicleGpsPosition.vel_e_m_s, vehicleGpsPosition.vel_d_m_s }, | |
| 1162 | ✗ | Eigen::Quaterniond{ vehicleAttitude.q.at(0), vehicleAttitude.q.at(1), vehicleAttitude.q.at(2), vehicleAttitude.q.at(3) }); | |
| 1163 | // TODO: Check order of w,x,y,z | ||
| 1164 | // TODO: Check if this is quaternion_nb | ||
| 1165 | |||
| 1166 | // Above it is ensured that only one gps and att element is present. | ||
| 1167 | // Here we still need to delete the used elements, otherwise the next iteration would find the elements again. | ||
| 1168 | ✗ | _epochData.erase(gpsIter); | |
| 1169 | ✗ | _epochData.erase(attIter); | |
| 1170 | |||
| 1171 | LOG_DATA("{}: Sending out PosVelAtt: {}", nameId(), obs->insTime.toYMDHMS()); | ||
| 1172 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_POSVELATT, obs); | |
| 1173 | ✗ | return obs; | |
| 1174 | ✗ | } | |
| 1175 | ✗ | } | |
| 1176 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'L') | |
| 1177 | { | ||
| 1178 | ✗ | vendor::pixhawk::message_logging_s messageLog; | |
| 1179 | ✗ | messageLog.header = ulogMsgHeader.msgHeader; | |
| 1180 | ✗ | read(reinterpret_cast<char*>(&messageLog.log_level), sizeof(messageLog.log_level)); | |
| 1181 | |||
| 1182 | ✗ | if (messageLog.log_level == 48) // '0' | |
| 1183 | { | ||
| 1184 | LOG_DATA("{}: Log-level: EMERG - System is unusable", nameId()); | ||
| 1185 | } | ||
| 1186 | ✗ | else if (messageLog.log_level == 49) // '1' | |
| 1187 | { | ||
| 1188 | LOG_DATA("{}: Log-level: ALERT - Action must be taken immediately", nameId()); | ||
| 1189 | } | ||
| 1190 | ✗ | else if (messageLog.log_level == 50) // '2' | |
| 1191 | { | ||
| 1192 | LOG_DATA("{}: Log-level: CRIT - Critical conditions", nameId()); | ||
| 1193 | } | ||
| 1194 | ✗ | else if (messageLog.log_level == 51) // '3' | |
| 1195 | { | ||
| 1196 | LOG_DATA("{}: Log-level: ERR - Error conditions", nameId()); | ||
| 1197 | } | ||
| 1198 | ✗ | else if (messageLog.log_level == 52) // '4' | |
| 1199 | { | ||
| 1200 | LOG_DATA("{}: Log-level: WARNING - Warning conditions", nameId()); | ||
| 1201 | } | ||
| 1202 | ✗ | else if (messageLog.log_level == 53) // '5' | |
| 1203 | { | ||
| 1204 | LOG_DATA("{}: Log-level: NOTICE - Normal but significant condition", nameId()); | ||
| 1205 | } | ||
| 1206 | ✗ | else if (messageLog.log_level == 54) // '6' | |
| 1207 | { | ||
| 1208 | LOG_DATA("{}: Log-level: INFO - Informational", nameId()); | ||
| 1209 | } | ||
| 1210 | ✗ | else if (messageLog.log_level == 55) // '7' | |
| 1211 | { | ||
| 1212 | LOG_DATA("{}: Log-level: DEBUG - Debug-level messages", nameId()); | ||
| 1213 | } | ||
| 1214 | else | ||
| 1215 | { | ||
| 1216 | ✗ | LOG_WARN("{}: Log-level is out of scope ({}) - possible data loss", nameId(), messageLog.log_level); | |
| 1217 | } | ||
| 1218 | |||
| 1219 | ✗ | read(reinterpret_cast<char*>(&messageLog.timestamp), sizeof(messageLog.timestamp)); | |
| 1220 | LOG_DATA("{}: messageLog.timestamp [µs]: {}", nameId(), messageLog.timestamp); | ||
| 1221 | |||
| 1222 | ✗ | messageLog.message.resize(messageLog.header.msg_size - 9); | |
| 1223 | ✗ | read(messageLog.message.data(), messageLog.header.msg_size - 9); | |
| 1224 | LOG_DATA("{}: messageLog.message: {}", nameId(), messageLog.message); | ||
| 1225 | ✗ | } | |
| 1226 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'C') | |
| 1227 | { | ||
| 1228 | ✗ | vendor::pixhawk::message_logging_tagged_s messageLogTagged; | |
| 1229 | ✗ | messageLogTagged.header = ulogMsgHeader.msgHeader; | |
| 1230 | ✗ | read(reinterpret_cast<char*>(&messageLogTagged.log_level), sizeof(messageLogTagged.log_level)); | |
| 1231 | |||
| 1232 | ✗ | if (messageLogTagged.log_level == 48) // '0' | |
| 1233 | { | ||
| 1234 | LOG_DATA("{}: Log-level: EMERG - System is unusable", nameId()); | ||
| 1235 | } | ||
| 1236 | ✗ | else if (messageLogTagged.log_level == 49) // '1' | |
| 1237 | { | ||
| 1238 | LOG_DATA("{}: Log-level: ALERT - Action must be taken immediately", nameId()); | ||
| 1239 | } | ||
| 1240 | ✗ | else if (messageLogTagged.log_level == 50) // '2' | |
| 1241 | { | ||
| 1242 | LOG_DATA("{}: Log-level: CRIT - Critical conditions", nameId()); | ||
| 1243 | } | ||
| 1244 | ✗ | else if (messageLogTagged.log_level == 51) // '3' | |
| 1245 | { | ||
| 1246 | LOG_DATA("{}: Log-level: ERR - Error conditions", nameId()); | ||
| 1247 | } | ||
| 1248 | ✗ | else if (messageLogTagged.log_level == 52) // '4' | |
| 1249 | { | ||
| 1250 | LOG_DATA("{}: Log-level: WARNING - Warning conditions", nameId()); | ||
| 1251 | } | ||
| 1252 | ✗ | else if (messageLogTagged.log_level == 53) // '5' | |
| 1253 | { | ||
| 1254 | LOG_DATA("{}: Log-level: NOTICE - Normal but significant condition", nameId()); | ||
| 1255 | } | ||
| 1256 | ✗ | else if (messageLogTagged.log_level == 54) // '6' | |
| 1257 | { | ||
| 1258 | LOG_DATA("{}: Log-level: INFO - Informational", nameId()); | ||
| 1259 | } | ||
| 1260 | ✗ | else if (messageLogTagged.log_level == 55) // '7' | |
| 1261 | { | ||
| 1262 | LOG_DATA("{}: Log-level: DEBUG - Debug-level messages", nameId()); | ||
| 1263 | } | ||
| 1264 | else | ||
| 1265 | { | ||
| 1266 | ✗ | LOG_WARN("{}: Log-level is out of scope ({}) - possible data loss", nameId(), messageLogTagged.log_level); | |
| 1267 | } | ||
| 1268 | |||
| 1269 | ✗ | read(reinterpret_cast<char*>(&messageLogTagged.tag), sizeof(messageLogTagged.tag)); | |
| 1270 | LOG_DATA("{}: messageLogTagged.tag: {}", nameId(), messageLogTagged.tag); | ||
| 1271 | ✗ | read(reinterpret_cast<char*>(&messageLogTagged.timestamp), sizeof(messageLogTagged.timestamp)); | |
| 1272 | LOG_DATA("{}: messageLogTagged.timestamp [µs]: {}", nameId(), messageLogTagged.timestamp); | ||
| 1273 | |||
| 1274 | ✗ | messageLogTagged.message.resize(messageLogTagged.header.msg_size - 11); | |
| 1275 | ✗ | read(messageLogTagged.message.data(), messageLogTagged.header.msg_size - 11); | |
| 1276 | LOG_DATA("{}: messageLogTagged.header.msg_size: {}", nameId(), messageLogTagged.header.msg_size); | ||
| 1277 | ✗ | } | |
| 1278 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'S') | |
| 1279 | { | ||
| 1280 | ✗ | vendor::pixhawk::message_sync_s messageSync; | |
| 1281 | ✗ | messageSync.header = ulogMsgHeader.msgHeader; | |
| 1282 | ✗ | read(reinterpret_cast<char*>(messageSync.syncMsg.data()), sizeof(messageSync.syncMsg)); | |
| 1283 | LOG_DATA("{}: messageSync.syncMsg[0]: {}", nameId(), messageSync.syncMsg[0]); | ||
| 1284 | } | ||
| 1285 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'O') | |
| 1286 | { | ||
| 1287 | ✗ | vendor::pixhawk::message_dropout_s messageDropout; | |
| 1288 | ✗ | messageDropout.header = ulogMsgHeader.msgHeader; | |
| 1289 | ✗ | read(reinterpret_cast<char*>(&messageDropout.duration), sizeof(messageDropout.duration)); | |
| 1290 | ✗ | LOG_WARN("{}: Dropout of duration: {} ms", nameId(), messageDropout.duration); | |
| 1291 | } | ||
| 1292 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'I') | |
| 1293 | { | ||
| 1294 | ✗ | readInformationMessage(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 1295 | } | ||
| 1296 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'M') | |
| 1297 | { | ||
| 1298 | ✗ | readInformationMessageMulti(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 1299 | } | ||
| 1300 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'P') | |
| 1301 | { | ||
| 1302 | ✗ | readParameterMessage(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 1303 | } | ||
| 1304 | ✗ | else if (ulogMsgHeader.msgHeader.msg_type == 'Q') | |
| 1305 | { | ||
| 1306 | ✗ | readParameterMessageDefault(ulogMsgHeader.msgHeader.msg_size, ulogMsgHeader.msgHeader.msg_type); | |
| 1307 | } | ||
| 1308 | else | ||
| 1309 | { | ||
| 1310 | ✗ | std::string nextChars; | |
| 1311 | ✗ | [[maybe_unused]] auto unidentifiedPos = static_cast<uint64_t>(tellg()); | |
| 1312 | ✗ | nextChars.resize(100); | |
| 1313 | ✗ | read(nextChars.data(), 100); | |
| 1314 | ✗ | LOG_WARN("{}: Message type not identified. Position: {}, The next 100 chars: {}", nameId(), unidentifiedPos, nextChars); | |
| 1315 | |||
| 1316 | // Reset read cursor | ||
| 1317 | ✗ | seekg(-100, std::ios_base::cur); | |
| 1318 | ✗ | } | |
| 1319 | |||
| 1320 | ✗ | if (!good() || eof()) | |
| 1321 | { | ||
| 1322 | ✗ | break; | |
| 1323 | } | ||
| 1324 | ✗ | } | |
| 1325 | ✗ | return nullptr; | |
| 1326 | } | ||
| 1327 | |||
| 1328 | ✗ | void NAV::UlogFile::readInformationMessage(uint16_t msgSize, char msgType) | |
| 1329 | { | ||
| 1330 | // Read msg size (2B) and type (1B) | ||
| 1331 | ✗ | vendor::pixhawk::message_info_s messageInfo; | |
| 1332 | ✗ | messageInfo.header.msg_size = msgSize; | |
| 1333 | ✗ | messageInfo.header.msg_type = msgType; | |
| 1334 | ✗ | read(reinterpret_cast<char*>(&messageInfo.key_len), sizeof(messageInfo.key_len)); | |
| 1335 | |||
| 1336 | // Read 'key' identifier ('keylength' byte) and its associated 'value' | ||
| 1337 | ✗ | messageInfo.key.resize(messageInfo.key_len); | |
| 1338 | ✗ | read(messageInfo.key.data(), messageInfo.key_len); | |
| 1339 | // if (!good() || eof()) | ||
| 1340 | // { | ||
| 1341 | // return false; | ||
| 1342 | // } | ||
| 1343 | |||
| 1344 | ✗ | messageInfo.value.resize(static_cast<size_t>(messageInfo.header.msg_size - 1 - messageInfo.key_len)); // 'msg_size' contains key and value, but not header | |
| 1345 | ✗ | read(messageInfo.value.data(), messageInfo.header.msg_size - 1 - messageInfo.key_len); | |
| 1346 | LOG_DATA("{}: Information message - key: {}", nameId(), messageInfo.key); | ||
| 1347 | LOG_DATA("{}: Information message - value: {}", nameId(), messageInfo.value); | ||
| 1348 | ✗ | } | |
| 1349 | |||
| 1350 | ✗ | void NAV::UlogFile::readInformationMessageMulti(uint16_t msgSize, char msgType) | |
| 1351 | { | ||
| 1352 | // Read msg size (2B) and type (1B) | ||
| 1353 | ✗ | vendor::pixhawk::ulog_message_info_multiple_header_s messageInfoMulti; | |
| 1354 | ✗ | messageInfoMulti.header.msg_size = msgSize; | |
| 1355 | ✗ | messageInfoMulti.header.msg_type = msgType; | |
| 1356 | ✗ | read(reinterpret_cast<char*>(&messageInfoMulti.is_continued), sizeof(messageInfoMulti.is_continued)); | |
| 1357 | ✗ | read(reinterpret_cast<char*>(&messageInfoMulti.key_len), sizeof(messageInfoMulti.key_len)); | |
| 1358 | |||
| 1359 | // Read 'key' identifier ('keylength' byte) and its associated 'value' | ||
| 1360 | ✗ | messageInfoMulti.key.resize(messageInfoMulti.key_len); | |
| 1361 | ✗ | read(messageInfoMulti.key.data(), messageInfoMulti.key_len); | |
| 1362 | ✗ | messageInfoMulti.value.resize(static_cast<size_t>(messageInfoMulti.header.msg_size - 2 - messageInfoMulti.key_len)); // contains 'is_continued' flag in contrast to information message | |
| 1363 | ✗ | read(messageInfoMulti.value.data(), messageInfoMulti.header.msg_size - 2 - messageInfoMulti.key_len); | |
| 1364 | LOG_DATA("{}: Information message multi - key_len: {}", nameId(), messageInfoMulti.key_len); | ||
| 1365 | LOG_DATA("{}: Information message multi - key: {}", nameId(), messageInfoMulti.key); | ||
| 1366 | LOG_DATA("{}: Information message multi - value: {}", nameId(), messageInfoMulti.value); | ||
| 1367 | |||
| 1368 | // TODO: Use 'is_continued' to generate a list of values with the same key | ||
| 1369 | ✗ | } | |
| 1370 | |||
| 1371 | ✗ | void NAV::UlogFile::readParameterMessage(uint16_t msgSize, char msgType) | |
| 1372 | { | ||
| 1373 | // Read msg size (2B) and type (1B) | ||
| 1374 | ✗ | vendor::pixhawk::message_info_s messageParam; | |
| 1375 | ✗ | messageParam.header.msg_size = msgSize; | |
| 1376 | ✗ | messageParam.header.msg_type = msgType; | |
| 1377 | ✗ | read(reinterpret_cast<char*>(&messageParam.key_len), sizeof(messageParam.key_len)); | |
| 1378 | |||
| 1379 | // Read 'key' identifier ('keylength' byte) and its associated 'value' | ||
| 1380 | ✗ | messageParam.key.resize(messageParam.key_len); | |
| 1381 | ✗ | read(messageParam.key.data(), messageParam.key_len); | |
| 1382 | |||
| 1383 | ✗ | if (!(messageParam.key.find("int32_t")) && !(messageParam.key.find("float"))) | |
| 1384 | { | ||
| 1385 | ✗ | LOG_WARN("{}: Parameter message contains invalid data type. It is neither 'int32_t', nor 'float', instead: {}", nameId(), messageParam.key); | |
| 1386 | } | ||
| 1387 | |||
| 1388 | ✗ | if (messageParam.header.msg_size - 1 - messageParam.key_len < 0) | |
| 1389 | { | ||
| 1390 | ✗ | LOG_WARN("{}: Parameter msg has key_len: {}", nameId(), messageParam.key_len); | |
| 1391 | } | ||
| 1392 | else | ||
| 1393 | { | ||
| 1394 | ✗ | messageParam.value.resize(static_cast<size_t>(messageParam.header.msg_size - 1 - messageParam.key_len)); // 'msg_size' contains key and value, but not header | |
| 1395 | ✗ | read(messageParam.value.data(), messageParam.header.msg_size - 1 - messageParam.key_len); | |
| 1396 | LOG_DATA("{}: Parameter message - key: {}", nameId(), messageParam.key); | ||
| 1397 | LOG_DATA("{}: Parameter message - value: {}", nameId(), messageParam.value); | ||
| 1398 | } | ||
| 1399 | ✗ | } | |
| 1400 | |||
| 1401 | ✗ | void NAV::UlogFile::readParameterMessageDefault(uint16_t msgSize, char msgType) | |
| 1402 | { | ||
| 1403 | ✗ | vendor::pixhawk::ulog_message_parameter_default_header_s messageParamDefault; | |
| 1404 | ✗ | messageParamDefault.header.msg_size = msgSize; | |
| 1405 | ✗ | messageParamDefault.header.msg_type = msgType; | |
| 1406 | ✗ | read(reinterpret_cast<char*>(&messageParamDefault.default_types), sizeof(messageParamDefault.default_types)); | |
| 1407 | ✗ | read(reinterpret_cast<char*>(&messageParamDefault.key_len), sizeof(messageParamDefault.key_len)); | |
| 1408 | |||
| 1409 | ✗ | messageParamDefault.key.resize(messageParamDefault.key_len); | |
| 1410 | ✗ | read(messageParamDefault.key.data(), messageParamDefault.key_len); | |
| 1411 | ✗ | messageParamDefault.value.resize(static_cast<size_t>(messageParamDefault.header.msg_size - 2 - messageParamDefault.key_len)); | |
| 1412 | ✗ | read(messageParamDefault.value.data(), messageParamDefault.header.msg_size - 2 - messageParamDefault.key_len); | |
| 1413 | ✗ | LOG_DEBUG("{}: Parameter default message - key: {}", nameId(), messageParamDefault.key); | |
| 1414 | ✗ | LOG_DEBUG("{}: Parameter default message - value: {}", nameId(), messageParamDefault.value); | |
| 1415 | |||
| 1416 | // TODO: Restriction on '1<<0' and '1<<1' | ||
| 1417 | ✗ | } | |
| 1418 | |||
| 1419 | ✗ | int8_t NAV::UlogFile::enoughImuDataAvailable() | |
| 1420 | { | ||
| 1421 | ✗ | std::array<bool, 2> accelHasData{}; | |
| 1422 | ✗ | std::array<bool, 2> gyroHasData{}; | |
| 1423 | |||
| 1424 | ✗ | for ([[maybe_unused]] const auto& [timestamp, measurement] : _epochData) | |
| 1425 | { | ||
| 1426 | ✗ | if (std::holds_alternative<SensorAccel>(measurement.data)) | |
| 1427 | { | ||
| 1428 | ✗ | accelHasData.at(measurement.multi_id) = true; | |
| 1429 | } | ||
| 1430 | ✗ | else if (std::holds_alternative<SensorGyro>(measurement.data)) | |
| 1431 | { | ||
| 1432 | ✗ | gyroHasData.at(measurement.multi_id) = true; | |
| 1433 | } | ||
| 1434 | } | ||
| 1435 | |||
| 1436 | ✗ | if (lastGnssTime.timeSinceStartup) | |
| 1437 | { | ||
| 1438 | ✗ | for (size_t i = 0; i < 2; ++i) | |
| 1439 | { | ||
| 1440 | ✗ | if (accelHasData.at(i) && gyroHasData.at(i)) | |
| 1441 | { | ||
| 1442 | ✗ | return static_cast<int8_t>(i); | |
| 1443 | } | ||
| 1444 | } | ||
| 1445 | } | ||
| 1446 | |||
| 1447 | ✗ | return -1; | |
| 1448 | } | ||
| 1449 | |||
| 1450 | ✗ | std::array<std::multimap<uint64_t, NAV::UlogFile::MeasurementData>::iterator, 2> NAV::UlogFile::findPosVelAttData() | |
| 1451 | { | ||
| 1452 | ✗ | auto gpsIter = _epochData.end(); | |
| 1453 | ✗ | auto attIter = _epochData.end(); | |
| 1454 | |||
| 1455 | ✗ | for (auto iter = _epochData.begin(); iter != _epochData.end(); ++iter) | |
| 1456 | { | ||
| 1457 | ✗ | if (std::holds_alternative<VehicleGpsPosition>(iter->second.data)) | |
| 1458 | { | ||
| 1459 | ✗ | gpsIter = iter; | |
| 1460 | } | ||
| 1461 | ✗ | else if (std::holds_alternative<VehicleAttitude>(iter->second.data)) | |
| 1462 | { | ||
| 1463 | ✗ | attIter = iter; | |
| 1464 | } | ||
| 1465 | } | ||
| 1466 | |||
| 1467 | ✗ | return { gpsIter, attIter }; | |
| 1468 | } | ||
| 1469 |