From 4de33807698da77adb6843680541ae6429054538 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Sun, 7 Jan 2024 15:07:55 +0100 Subject: [PATCH 01/27] EPS32 Messages are sent together --- .vscode/launch.json | 4 +- .vscode/settings.json | 2 +- CMakeLists.txt | 3 + INSTINCT.code-workspace | 58 ++ cmake/modules/FindLibSSH.cmake | 68 +++ src/CMakeLists.txt | 1 + src/NodeData/WiFi/ArubaObs.hpp | 48 ++ src/NodeData/WiFi/EspressifObs.hpp | 50 ++ src/NodeRegistry.cpp | 11 + .../Converter/GNSS/UartPacketConverter.cpp | 35 +- .../Converter/GNSS/UartPacketConverter.hpp | 6 +- .../DataProcessor/WiFi/WiFiPositioning.cpp | 495 ++++++++++++++++++ .../DataProcessor/WiFi/WiFiPositioning.hpp | 131 +++++ .../DataProvider/WiFi/Sensors/ArubaSensor.cpp | 332 ++++++++++++ .../DataProvider/WiFi/Sensors/ArubaSensor.hpp | 93 ++++ .../WiFi/Sensors/EspressifSensor.cpp | 155 ++++++ .../WiFi/Sensors/EspressifSensor.hpp | 81 +++ .../Vendor/Espressif/EspressifUartSensor.cpp | 168 ++++++ .../Vendor/Espressif/EspressifUartSensor.hpp | 120 +++++ .../Vendor/Espressif/EspressifUtilities.cpp | 52 ++ .../Vendor/Espressif/EspressifUtilities.hpp | 36 ++ test/CMakeLists.txt | 1 + 22 files changed, 1943 insertions(+), 7 deletions(-) create mode 100644 INSTINCT.code-workspace create mode 100644 cmake/modules/FindLibSSH.cmake create mode 100644 src/NodeData/WiFi/ArubaObs.hpp create mode 100644 src/NodeData/WiFi/EspressifObs.hpp create mode 100644 src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp create mode 100644 src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp create mode 100644 src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp create mode 100644 src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp create mode 100644 src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp create mode 100644 src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp create mode 100644 src/util/Vendor/Espressif/EspressifUartSensor.cpp create mode 100644 src/util/Vendor/Espressif/EspressifUartSensor.hpp create mode 100644 src/util/Vendor/Espressif/EspressifUtilities.cpp create mode 100644 src/util/Vendor/Espressif/EspressifUtilities.hpp diff --git a/.vscode/launch.json b/.vscode/launch.json index c2ac2e087..a92c2474c 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -2,7 +2,7 @@ "version": "0.2.0", "configurations": [ { - "name": "Linux Debug", + "name": "Mac Debug", "type": "cppdbg", "request": "launch", "preLaunchTask": "MAIN: Build project", @@ -25,7 +25,7 @@ "cwd": "${workspaceFolder}", "environment": [], "externalConsole": false, - "MIMode": "gdb", + "MIMode": "lldb", "setupCommands": [ { "description": "Enable pretty-printing for gdb", diff --git a/.vscode/settings.json b/.vscode/settings.json index 858feb2db..166077973 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,6 @@ { "[cpp]": { - "editor.defaultFormatter": "xaver.clang-format" + "editor.defaultFormatter": "ms-vscode.cpptools" }, "[c]": { "editor.defaultFormatter": "ms-vscode.cpptools" diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d949912e..263d60574 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,6 +200,9 @@ include_directories(SYSTEM lib/vnproglib/cpp/include) set_target_properties(libvncxx PROPERTIES CXX_CLANG_TIDY "") set_target_properties(libvncxx PROPERTIES CXX_CPPCHECK "") +# ----------------------------------------------- libssh ------------------------------------------------- */ +find_package(libssh REQUIRED) + # ------------------------------------------------ Navio2 ------------------------------------------------ */ if(NOT APPLE AND NOT WIN32) diff --git a/INSTINCT.code-workspace b/INSTINCT.code-workspace new file mode 100644 index 000000000..d1da74e32 --- /dev/null +++ b/INSTINCT.code-workspace @@ -0,0 +1,58 @@ +{ + "folders": [ + { + "path": "." + } + ], + "settings": { + "C_Cpp.autoAddFileAssociations": false, + "doxdocgen.generic.paramTemplate": "@param[in, out] {param} ", + "doxdocgen.file.fileOrder": [ + "custom", + "file", + "brief", + "author", + "date" + ], + "doxdocgen.file.customTag": [ + "This file is part of INSTINCT, the INS Toolkit for Integrated", + "Navigation Concepts and Training by the Institute of Navigation of", + "the University of Stuttgart, Germany.", + "", + "This Source Code Form is subject to the terms of the Mozilla Public", + "License, v. 2.0. If a copy of the MPL was not distributed with this", + "file, You can obtain one at https://mozilla.org/MPL/2.0/.", + "", + "<-- KEEP ONE EMPTY LINE HERE AND MAKE LICENSE COMMENTS only 2 slashes '//'", + "" + ], + "doxdocgen.generic.order": [ + "brief", + "tparam", + "param", + "return" + ], + "doxdocgen.c.commentPrefix": "/// ", + "doxdocgen.c.firstLine": "", + "doxdocgen.c.lastLine": "", + "doxdocgen.c.triggerSequence": "///", + "doxdocgen.generic.boolReturnsTrueFalse": false, + "doxdocgen.generic.returnTemplate": "@return ", + "doxdocgen.cpp.ctorText": "Constructor", + "doxdocgen.cpp.dtorText": "Destructor", + "files.associations": { + "**/C++/**": "cpp", + "**/tbb/**": "cpp", + "**/boost/**": "cpp", + "*.tpp": "cpp", + "*.log.*": "log", + "*.flow": "json", + "*.dox": "latex" + }, + "mathover.matchRegex": "\\/\\/\\sMath:\\s(.+)", + "mathover.forwardSkip": 9, + "todo-tree.filtering.excludeGlobs": [ + "**/lib" + ] + } +} \ No newline at end of file diff --git a/cmake/modules/FindLibSSH.cmake b/cmake/modules/FindLibSSH.cmake new file mode 100644 index 000000000..c04aedb48 --- /dev/null +++ b/cmake/modules/FindLibSSH.cmake @@ -0,0 +1,68 @@ +# - Try to find LibSSH +# Once done this will define +# +# LIBSSH_FOUND - system has LibSSH +# LIBSSH_INCLUDE_DIRS - the LibSSH include directory +# LIBSSH_LIBRARIES - Link these to use LibSSH +# LIBSSH_DEFINITIONS - Compiler switches required for using LibSSH +# +# Copyright (c) 2009 Andreas Schneider +# +# Redistribution and use is allowed according to the terms of the New +# BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. +# + +if(NOT (LIBSSH_INCLUDE_DIR AND LIBSSH_LIBRARY)) + find_package(PkgConfig) + pkg_check_modules(PC_LIBSSH QUIET libssh) + + find_path(LIBSSH_INCLUDE_DIR + NAMES + libssh/libssh.h + PATHS + /usr/include + /usr/local/include + /opt/local/include + /sw/include + ${CMAKE_INCLUDE_PATH} + ${CMAKE_INSTALL_PREFIX}/include + ) + + find_library(LIBSSH_LIBRARY + NAMES + ssh + libssh + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ${CMAKE_LIBRARY_PATH} + ${CMAKE_INSTALL_PREFIX}/lib + ) + + mark_as_advanced(LIBSSH_INCLUDE_DIR LIBSSH_LIBRARY) +endif(NOT (LIBSSH_INCLUDE_DIR AND LIBSSH_LIBRARY)) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(LIBSSH + FOUND_VAR LIBSSH_FOUND + REQUIRED_VARS LIBSSH_LIBRARY LIBSSH_INCLUDE_DIR) + +if(LIBSSH_FOUND) + set(LIBSSH_LIBRARIES ${LIBSSH_LIBRARY}) + set(LIBSSH_INCLUDE_DIRS ${LIBSSH_INCLUDE_DIR}) + set(LIBSSH_DEFINITIONS ${PC_LIBSSH_CFLAGS_OTHER}) +endif() + +if(LIBSSH_FOUND AND NOT TARGET LIBSSH::LIBSSH) + add_library(LIBSSH::LIBSSH UNKNOWN IMPORTED) + set_target_properties( + LIBSSH::LIBSSH PROPERTIES + IMPORTED_LOCATION "${LIBSSH_LIBRARY}" + INTERFACE_COMPILE_OPTIONS "${PC_LIBSSH_CFLAGS_OTHER}" + INTERFACE_INCLUDE_DIRECTORIES "${LIBSSH_INCLUDE_DIR}") +endif() + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c61971c49..1de70bc0b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -37,6 +37,7 @@ if(ENABLE_MAIN) ImGuiFileDialog implot application + ssh libvncxx libUartSensor) diff --git a/src/NodeData/WiFi/ArubaObs.hpp b/src/NodeData/WiFi/ArubaObs.hpp new file mode 100644 index 000000000..eedd13379 --- /dev/null +++ b/src/NodeData/WiFi/ArubaObs.hpp @@ -0,0 +1,48 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file ArubaObs.hpp +/// @brief Aruba Observation Class +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date TODO + +#pragma once + +#include "NodeData/NodeData.hpp" + +namespace NAV +{ +/// Aruba Observation Class +class ArubaObs : public NodeData +{ + public: + /// @brief Returns the type of the data class + /// @return The data type + [[nodiscard]] static std::string type() + { + return "ArubaObs"; + } + + /// @brief Returns the parent types of the data class + /// @return The parent data types + [[nodiscard]] static std::vector parentTypes() + { + return { NodeData::type() }; + } + + struct FtmObs + { + std::string macAddress; + InsTime time; + double measuredDistance; + }; + + std::vector data; +}; + +} // namespace NAV diff --git a/src/NodeData/WiFi/EspressifObs.hpp b/src/NodeData/WiFi/EspressifObs.hpp new file mode 100644 index 000000000..933093e9f --- /dev/null +++ b/src/NodeData/WiFi/EspressifObs.hpp @@ -0,0 +1,50 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file EspressifObs.hpp +/// @brief Espressif Observation Class +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date TODO + +#pragma once + +#include "NodeData/NodeData.hpp" + +namespace NAV +{ +/// Espressif Observation Class +class EspressifObs : public NodeData +{ + public: + /// @brief Returns the type of the data class + /// @return The data type + [[nodiscard]] static std::string type() + { + return "EspressifObs"; + } + + /// @brief Returns the parent types of the data class + /// @return The parent data types + [[nodiscard]] static std::vector parentTypes() + { + return { NodeData::type() }; + } + + /// Payload length in bytes + uint16_t payloadLength = 0; + + struct FtmObs + { + std::array macAddress; + double measuredDistance; + }; + + std::vector data; +}; + +} // namespace NAV diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index 90d92eb1d..6fb2df975 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -197,6 +197,7 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.hpp" #include "Nodes/DataProcessor/KalmanFilter/TightlyCoupledKF.hpp" #include "Nodes/DataProcessor/SensorCombiner/ImuFusion.hpp" +#include "Nodes/DataProcessor/WiFi/WiFiPositioning.hpp" // Data Provider #include "Nodes/DataProvider/CSV/CsvFile.hpp" #include "Nodes/DataProvider/GNSS/FileReader/RinexNavFile.hpp" @@ -216,6 +217,8 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataProvider/IMU/FileReader/UlogFile.hpp" #include "Nodes/DataProvider/State/PosVelAttFile.hpp" #include "Nodes/DataProvider/IMU/FileReader/MultiImuFile.hpp" +#include "Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp" +#include "Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp" // Data Simulator #include "Nodes/DataProvider/IMU/Simulators/ImuSimulator.hpp" // Plotting @@ -262,6 +265,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); // Data Provider registerNodeType(); registerNodeType(); @@ -281,6 +285,8 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); + registerNodeType(); // Data Simulator registerNodeType(); // Experimental @@ -312,6 +318,8 @@ void NAV::NodeRegistry::RegisterNodeTypes() #include "NodeData/State/Pos.hpp" #include "NodeData/State/PosVel.hpp" #include "NodeData/State/PosVelAtt.hpp" +#include "NodeData/WiFi/ArubaObs.hpp" +#include "NodeData/WiFi/EspressifObs.hpp" void NAV::NodeRegistry::RegisterNodeDataTypes() { @@ -337,4 +345,7 @@ void NAV::NodeRegistry::RegisterNodeDataTypes() registerNodeDataType(); registerNodeDataType(); registerNodeDataType(); + // WiFi + registerNodeDataType(); + registerNodeDataType(); } \ No newline at end of file diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index 5319da774..55403bd9c 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -19,6 +19,7 @@ namespace nm = NAV::NodeManager; #include "util/Vendor/Ublox/UbloxUtilities.hpp" #include "util/Vendor/Emlid/EmlidUtilities.hpp" +#include "util/Vendor/Espressif/EspressifUtilities.hpp" NAV::UartPacketConverter::UartPacketConverter() : Node(typeStatic()) @@ -54,9 +55,22 @@ std::string NAV::UartPacketConverter::category() void NAV::UartPacketConverter::guiConfig() { - if (ImGui::Combo(fmt::format("Output Type##{}", size_t(id)).c_str(), reinterpret_cast(&_outputType), "UbloxObs\0EmlidObs\0\0")) + if (ImGui::Combo(fmt::format("Output Type ##{}", size_t(id)).c_str(), reinterpret_cast(&_outputType), "UbloxObs\0EmlidObs\0EspressifObs\0\0")) { - LOG_DEBUG("{}: Output Type changed to {}", nameId(), _outputType == OutputType_UbloxObs ? "UbloxObs" : "EmlidObs"); + std::string outputTypeString; + switch (_outputType) + { + case OutputType_UbloxObs: + outputTypeString = "UbloxObs"; + break; + case OutputType_EmlidObs: + outputTypeString = "EmlidObs"; + break; + default: + outputTypeString = "EspressifObs"; + } + + LOG_DEBUG("{}: Output Type changed to {}", nameId(), outputTypeString); if (_outputType == OutputType_UbloxObs) { @@ -68,6 +82,11 @@ void NAV::UartPacketConverter::guiConfig() outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EmlidObs::type() }; outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EmlidObs::type(); } + else if (_outputType == OutputType_EspressifObs) + { + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EspressifObs::type() }; + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EspressifObs::type(); + } for (auto& link : outputPins.front().links) { @@ -112,6 +131,11 @@ void NAV::UartPacketConverter::restore(json const& j) outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EmlidObs::type() }; outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EmlidObs::type(); } + else if (_outputType == OutputType_EspressifObs) + { + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EspressifObs::type() }; + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EspressifObs::type(); + } } } } @@ -136,6 +160,13 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, s vendor::ublox::decryptUbloxObs(obs, packet); convertedData = obs; } + else if (_outputType == OutputType_EspressifObs) + { + auto obs = std::make_shared(); + auto packet = uartPacket->raw; + vendor::espressif::decryptEspressifObs(obs, packet); + convertedData = obs; + } else /* if (_outputType == OutputType_EmlidObs) */ { auto obs = std::make_shared(); diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp index 53cbdf178..b723f1e15 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp @@ -18,6 +18,7 @@ #include "NodeData/General/UartPacket.hpp" #include "NodeData/GNSS/UbloxObs.hpp" #include "NodeData/GNSS/EmlidObs.hpp" +#include "NodeData/WiFi/EspressifObs.hpp" #include #include @@ -68,8 +69,9 @@ class UartPacketConverter : public Node /// Enum specifying the type of the output message enum OutputType { - OutputType_UbloxObs, ///< Extract UbloxObs data - OutputType_EmlidObs, ///< Extract EmlidObs data + OutputType_UbloxObs, ///< Extract UbloxObs data + OutputType_EmlidObs, ///< Extract EmlidObs data + OutputType_EspressifObs, ///< Extract EspressifObs data }; /// The selected output type in the GUI diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp new file mode 100644 index 000000000..c1079857e --- /dev/null +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -0,0 +1,495 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "WiFiPositioning.hpp" + +#include +#include +#include + +#include "util/Logger.hpp" +#include "util/Container/Vector.hpp" + +#include "Navigation/Constants.hpp" + +#include "internal/gui/NodeEditorApplication.hpp" +#include "internal/gui/widgets/HelpMarker.hpp" +#include "internal/gui/widgets/imgui_ex.hpp" + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +#include "NodeData/WiFi/EspressifObs.hpp" +#include "NodeData/WiFi/ArubaObs.hpp" +#include "NodeData/State/PosVelAtt.hpp" +#include "Navigation/GNSS/Functions.hpp" +#include "Navigation/Transformations/CoordinateFrames.hpp" + +#include "Navigation/Math/LeastSquares.hpp" + +NAV::WiFiPositioning::WiFiPositioning() + : Node(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _hasConfig = true; + _guiConfigDefaultWindowSize = { 575, 300 }; + _outputInterval = 3000; + + nm::CreateInputPin(this, NAV::EspressifObs::type().c_str(), Pin::Type::Flow, { NAV::EspressifObs::type() }, &WiFiPositioning::recvEspressifObs); + nm::CreateInputPin(this, NAV::ArubaObs::type().c_str(), Pin::Type::Flow, { NAV::ArubaObs::type() }, &WiFiPositioning::recvArubaObs); + + nm::CreateOutputPin(this, NAV::Pos::type().c_str(), Pin::Type::Flow, { NAV::Pos::type() }); +} + +NAV::WiFiPositioning::~WiFiPositioning() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::WiFiPositioning::typeStatic() +{ + return "WiFiPositioning"; +} + +std::string NAV::WiFiPositioning::type() const +{ + return typeStatic(); +} + +std::string NAV::WiFiPositioning::category() +{ + return "Data Processor"; +} + +void NAV::WiFiPositioning::guiConfig() +{ + float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio(); + + ImGui::SetNextItemWidth(250 * gui::NodeEditorApplication::windowFontRatio()); + + if (_numOfDevices == 0) + { + if (ImGui::Combo(fmt::format("Frame##{}", size_t(id)).c_str(), reinterpret_cast(&_frame), "ECEF\0LLA\0NED\0\0")) + { + switch (_frame) + { + case Frame::ECEF: + LOG_DEBUG("{}: Frame changed to ECEF", nameId()); + break; + case Frame::LLA: + LOG_DEBUG("{}: Frame changed to LLA", nameId()); + break; + case Frame::NED: + LOG_DEBUG("{}: Frame changed to NED", nameId()); + break; + } + } + + flow::ApplyChanges(); + } + + if (ImGui::BeginTable("RouterInput", 4, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) + { + // Column headers + ImGui::TableSetupColumn("MAC address", ImGuiTableColumnFlags_WidthFixed, columnWidth); + if (_frame == Frame::ECEF) + { + ImGui::TableSetupColumn("X", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Y", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Z", ImGuiTableColumnFlags_WidthFixed, columnWidth); + } + else if (_frame == Frame::LLA) + { + ImGui::TableSetupColumn("Latitude", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Longitude", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Altitude", ImGuiTableColumnFlags_WidthFixed, columnWidth); + } + else if (_frame == Frame::NED) + { + ImGui::TableSetupColumn("North", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("East", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Down", ImGuiTableColumnFlags_WidthFixed, columnWidth); + } + + // Automatic header row + ImGui::TableHeadersRow(); + + for (size_t rowIndex = 0; rowIndex < _numOfDevices; rowIndex++) + { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + + // MAC address validation + std::regex macRegex("^([0-9A-Fa-f]{2}[:-]){5}([0-9A-Fa-f]{2})$"); + + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputText(fmt::format("##Mac{}", size_t(rowIndex)).c_str(), &_deviceMacAddresses.at(rowIndex), ImGuiInputTextFlags_None)) + { + std::transform(_deviceMacAddresses.at(rowIndex).begin(), _deviceMacAddresses.at(rowIndex).end(), _deviceMacAddresses.at(rowIndex).begin(), ::toupper); // Convert to uppercase + if (!std::regex_match(_deviceMacAddresses.at(rowIndex), macRegex)) + { + _deviceMacAddresses.at(rowIndex) = "00:00:00:00:00:00"; + LOG_DEBUG("{}: Invalid MAC address", nameId()); + } + else + { + flow::ApplyChanges(); + } + } + + if (_frame == Frame::ECEF) + { + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputX{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputY{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputZ{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + } + else if (_frame == Frame::LLA) + { + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDoubleL(fmt::format("##InputLat{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], -180, 180, 0.0, 0.0, "%.8f°")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDoubleL(fmt::format("##InputLon{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], -180, 180, 0.0, 0.0, "%.8f°")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputHeight{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + } + else if (_frame == Frame::NED) + { + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputNorth{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputEast{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputDown{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + } + } + + ImGui::EndTable(); + } + if (ImGui::Button(fmt::format("Add Device##{}", size_t(id)).c_str(), ImVec2(columnWidth * 2.1f, 0))) + { + _numOfDevices++; + _deviceMacAddresses.push_back("00:00:00:00:00:00"); + _devicePositions.push_back(Eigen::Vector3d::Zero()); + flow::ApplyChanges(); + } + ImGui::SameLine(); + if (ImGui::Button(fmt::format("Delete Device##{}", size_t(id)).c_str(), ImVec2(columnWidth * 2.1f, 0))) + { + if (_numOfDevices > 0) + { + _numOfDevices--; + _deviceMacAddresses.pop_back(); + _devicePositions.pop_back(); + flow::ApplyChanges(); + } + } + ImGui::Separator(); + if (ImGui::Combo(fmt::format("Solution##{}", size_t(id)).c_str(), reinterpret_cast(&_solutionMode), "Least squares 2D\0Least squares 3D\0Kalman Filter\0\0")) + { + switch (_solutionMode) + { + case SolutionMode::LSQ2D: + LOG_DEBUG("{}: Solution changed to Least squares 2D", nameId()); + break; + case SolutionMode::LSQ3D: + LOG_DEBUG("{}: Solution changed to Least squares 3D", nameId()); + break; + case SolutionMode::KF: + LOG_DEBUG("{}: Solution changed to Kalman Filter", nameId()); + break; + } + } + flow::ApplyChanges(); + if (_solutionMode == SolutionMode::LSQ2D || _solutionMode == SolutionMode::LSQ3D) + { + ImGui::SameLine(); + gui::widgets::HelpMarker("The third coordinate of the devices is not utilized in a two-dimensional solution."); + flow::ApplyChanges(); + if (ImGui::InputInt("Output Interval in ms", &_outputInterval)) + { + LOG_DEBUG("{}: output interval changed to {}", nameId(), _outputInterval); + } + ImGui::SameLine(); + gui::widgets::HelpMarker("The output interval should be at least as large as that of the sensors."); + flow::ApplyChanges(); + } +} + +[[nodiscard]] json NAV::WiFiPositioning::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["frame"] = static_cast(_frame); + j["deviceMacAddresses"] = _deviceMacAddresses; + j["devicePositions"] = _devicePositions; + j["numOfDevices"] = _numOfDevices; + j["solutionMode"] = static_cast(_solutionMode); + j["outputInterval"] = _outputInterval; + + return j; +} + +void NAV::WiFiPositioning::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("frame")) + { + j.at("frame").get_to(_frame); + } + if (j.contains("deviceMacAddresses")) + { + j.at("deviceMacAddresses").get_to(_deviceMacAddresses); + } + if (j.contains("devicePositions")) + { + j.at("devicePositions").get_to(_devicePositions); + } + if (j.contains("numOfDevices")) + { + j.at("numOfDevices").get_to(_numOfDevices); + } + if (j.contains("solutionMode")) + { + j.at("solutionMode").get_to(_solutionMode); + } + if (j.contains("outputInterval")) + { + j.at("outputInterval").get_to(_outputInterval); + } +} + +bool NAV::WiFiPositioning::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + _e_position = Eigen::Vector3d::Zero(); + + LOG_DEBUG("WiFiPositioning initialized"); + + return true; +} + +void NAV::WiFiPositioning::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); +} + +void NAV::WiFiPositioning::recvEspressifObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +{ + _devices.clear(); + auto espressifObs = std::static_pointer_cast(queue.extract_front()); + + for (auto const& obs : espressifObs->data) + { + std::string macAddressString = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", obs.macAddress[0], obs.macAddress[1], obs.macAddress[2], obs.macAddress[3], obs.macAddress[4], obs.macAddress[5]); + auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), macAddressString); + if (it != _deviceMacAddresses.end()) // Device already exists + { + // Get the index of the found element + size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); + if (_frame == Frame::LLA) + { + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.measuredDistance }); + } + else if (_frame == Frame::ECEF) + { + _devices.push_back({ _devicePositions.at(index), obs.measuredDistance }); + } + } + } +} + +void NAV::WiFiPositioning::recvArubaObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +{ + _devices.clear(); + auto arubaObs = std::static_pointer_cast(queue.extract_front()); + for (auto const& obs : arubaObs->data) + { + auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs.macAddress); + if (it != _deviceMacAddresses.end()) // Device already exists + { + // Get the index of the found element + size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); + if (_frame == Frame::LLA) + { + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.measuredDistance }); + } + else if (_frame == Frame::ECEF) + { + _devices.push_back({ _devicePositions.at(index), obs.measuredDistance }); + } + } + } +} + +// void NAV::WiFiPositioning::lsqSolution2d() +// { +// LOG_DEBUG("{}: Received {} observations", nameId(), _devices.size()); + +// if (_devices.size() < 3) +// { +// LOG_WARN("{}: Received less than 3 observations. Can't compute position", nameId()); +// return; +// } +// else if (_devices.size() == 3 && _calc2dPosition) +// { +// LOG_WARN("{}: Received 3 observations. Only 2D position is possible", nameId()); +// } +// else +// { +// LOG_DEBUG("{}: Received {} observations", nameId(), espressifObs->data.size()); +// } +// _e_position = Eigen::Vector3d::Zero(); + +// std::vector routerPositions; +// std::vector distMeas; +// routerPositions.reserve(espressifObs->data.size()); + +// for (auto const& obs : espressifObs->data) +// { +// routerPositions.emplace_back(static_cast(obs.routerPosition[0]), static_cast(obs.routerPosition[1])); // Remove [2] +// distMeas.emplace_back(static_cast(obs.measuredDistance)); +// } + +// Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(distMeas.size()), 2); +// Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(distMeas.size())); +// LeastSquaresResult lsq; +// for (size_t o = 0; o < 10; o++) +// { +// LOG_DATA("{}: Iteration {}", nameId(), o); +// size_t ix = 0; +// for (auto const& routerPosition : routerPositions) +// { +// double distEst = (routerPosition - _e_position.head<2>()).norm(); +// Eigen::Vector2d e_lineOfSightUnitVector = (_e_position.head<2>() - routerPosition) / (_e_position.head<2>() - routerPosition).norm(); +// e_H.block<1, 2>(static_cast(ix), 0) = -e_lineOfSightUnitVector; +// ddist(static_cast(ix)) = distMeas.at(ix) - distEst; + +// lsq = solveLinearLeastSquaresUncertainties(e_H, ddist); +// LOG_DATA("{}: [{}] dx (lsq) {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1)); +// LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); + +// _e_position.head<2>() += lsq.solution.head<2>(); +// ix++; +// } +// bool solInaccurate = lsq.solution.norm() > 1e-4; + +// if (!solInaccurate) +// { +// break; +// } +// } + +// auto pos = std::make_shared(); +// pos->setPosition_e(_e_position); +// LOG_DEBUG("{}: Position: {}", nameId(), _e_position.transpose()); +// invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); +// } + +// void NAV::WiFiPositioning::lsqSolution3d() +// { + +// LOG_DEBUG("{}: Received {} observations", nameId(), _devices.size()); + +// if (_devices.size() < 4) +// { +// LOG_WARN("{}: Received less than 4 observations. Can't compute position", nameId()); +// return; +// } +// else +// { +// LOG_DEBUG("{}: Received {} observations", nameId(), espressifObs->data.size()); +// } +// _e_position = Eigen::Vector3d::Zero(); + +// Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(_numOfMeasurements), static_cast(3)); +// Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_numOfMeasurements)); +// LeastSquaresResult lsq; +// for (size_t o = 0; o < 10; o++) +// { +// LOG_DATA("{}: Iteration {}", nameId(), o); +// size_t ix = 0; +// for (size_t i = 0; i < _numOfMeasurements; i++) +// { +// double distEst = (_devices.at(i).position - _e_position).norm(); +// Eigen::Vector3d e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(_e_position, _devices.at(i).position); +// e_H.block<1, 3>(static_cast(ix), 0) = -e_lineOfSightUnitVector; +// ddist(static_cast(ix)) = distMeas.at(ix) - distEst; + +// lsq = solveLinearLeastSquaresUncertainties(e_H, ddist); +// LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); +// LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); + +// _e_position += lsq.solution.head<3>(); +// ix++; +// } +// bool solInaccurate = lsq.solution.norm() > 1e-4; + +// if (!solInaccurate) +// { +// break; +// } +// } + +// auto pos = std::make_shared(); +// pos->setPosition_e(_e_position); +// LOG_DEBUG("{}: Position: {}", nameId(), _e_position.transpose()); +// invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); +// } + +// void NAV::WiFiPositioning::kfSolution() +// { +// auto pos = std::make_shared(); +// pos->setPosition_e(_e_position); +// LOG_DEBUG("{}: Position: {}", nameId(), _e_position.transpose()); +// invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); +// } \ No newline at end of file diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp new file mode 100644 index 000000000..894ab8d53 --- /dev/null +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp @@ -0,0 +1,131 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file WiFiPositioning.hpp +/// @brief Single Point Positioning (SPP) / Code Phase Positioning +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2022-04-29 + +#pragma once + +#include "util/Eigen.hpp" +#include + +#include "internal/Node/Node.hpp" +#include "NodeData/WiFi/EspressifObs.hpp" +#include "NodeData/WiFi/ArubaObs.hpp" +#include "NodeData/State/Pos.hpp" + +namespace NAV +{ +/// @brief Numerically integrates Imu data +class WiFiPositioning : public Node +{ + public: + /// @brief Default constructor + WiFiPositioning(); + /// @brief Destructor + ~WiFiPositioning() override; + /// @brief Copy constructor + WiFiPositioning(const WiFiPositioning&) = delete; + /// @brief Move constructor + WiFiPositioning(WiFiPositioning&&) = delete; + /// @brief Copy assignment operator + WiFiPositioning& operator=(const WiFiPositioning&) = delete; + /// @brief Move assignment operator + WiFiPositioning& operator=(WiFiPositioning&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + private: + constexpr static size_t INPUT_PORT_INDEX_ESPRESSIF_OBS = 0; ///< @brief EspressifObs + constexpr static size_t INPUT_PORT_INDEX_ARUBA_OBS = 0; ///< @brief ArubaObs + constexpr static size_t OUTPUT_PORT_INDEX_POS = 0; ///< @brief Pos + + // --------------------------------------------------------------- Gui ----------------------------------------------------------------- + + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Available Frames + enum class Frame : int + { + ECEF, ///< Earth-Centered Earth-Fixed frame + LLA, ///< Latitude-Longitude-Altitude frame + NED, ///< North-East-Down frame + }; + /// Frame to calculate the position in + Frame _frame = Frame::ECEF; + + /// @brief Available Solution Modes + enum class SolutionMode : int + { + LSQ2D, ///< Least Squares in 2D + LSQ3D, ///< Least Squares in 3D + KF, ///< Kalman Filter + }; + /// Solution Mode + SolutionMode _solutionMode = SolutionMode::LSQ3D; + + /// Output interval in ms + int _outputInterval; + // ------------------------------------------------------------ Algorithm -------------------------------------------------------------- + + /// Estimated position in local frame [m] + Eigen::Vector3d _e_position = Eigen::Vector3d::Zero(); + + std::vector _deviceMacAddresses; + std::vector _devicePositions; + size_t _numOfDevices; + + struct Device + { + Eigen::Vector3d position; + double distance; + }; + std::vector _devices; + + /// @brief Receive Function for the Espressif Observations + /// @param[in] queue Queue with all the received data messages + /// @param[in] pinIdx Index of the pin the data is received on + void recvEspressifObs(InputPin::NodeDataQueue& queue, size_t pinIdx); + + /// @brief Receive Function for the Aruba Observations + /// @param[in] queue Queue with all the received data messages + /// @param[in] pinIdx Index of the pin the data is received on + void recvArubaObs(InputPin::NodeDataQueue& queue, size_t pinIdx); + + /// @brief Calculate the position + void lsqSolution2d(); + + /// @brief Calculate the position + void lsqSolution3d(); +}; + +} // namespace NAV \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp new file mode 100644 index 000000000..03c095dc9 --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -0,0 +1,332 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "ArubaSensor.hpp" + +#include "util/Time/TimeBase.hpp" +#include "util/Logger.hpp" +#include +#include +#include + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +#include "NodeData/WiFi/ArubaObs.hpp" + +/// Speed of light in air [m/s] +constexpr double cAir = 299702547.0; + +NAV::ArubaSensor::ArubaSensor() + : Node(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _onlyRealTime = true; + _hasConfig = true; + _guiConfigDefaultWindowSize = { 710, 220 }; + + _sshHost = "192.168.178.45"; + _sshUser = "admin"; + _sshHostkeys = "ssh-rsa"; + _sshKeyExchange = "ecdh-sha2-nistp256"; + _sshPublickeyAcceptedTypes = "ssh-rsa"; + _outputInterval = 3000; + + nm::CreateOutputPin(this, "ArubaObs", Pin::Type::Flow, { NAV::ArubaObs::type() }); +} + +NAV::ArubaSensor::~ArubaSensor() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::ArubaSensor::typeStatic() +{ + return "ArubaSensor"; +} + +std::string NAV::ArubaSensor::type() const +{ + return typeStatic(); +} + +std::string NAV::ArubaSensor::category() +{ + return "Data Provider"; +} + +void NAV::ArubaSensor::guiConfig() +{ + if (ImGui::InputTextWithHint("SSH Host", "192.168.0.0", &_sshHost)) + { + LOG_DEBUG("{}: ssh host changed to {}", nameId(), _sshHost); + flow::ApplyChanges(); + doDeinitialize(); + } + if (ImGui::InputTextWithHint("SSH User", "admin", &_sshUser)) + { + LOG_DEBUG("{}: ssh admin changed to {}", nameId(), _sshUser); + flow::ApplyChanges(); + doDeinitialize(); + } + if (ImGui::InputText("SSH Host Keys", &_sshHostkeys)) + { + LOG_DEBUG("{}: ssh host keys changed to {}", nameId(), _sshHostkeys); + flow::ApplyChanges(); + doDeinitialize(); + } + if (ImGui::InputText("SSH Key Exchange", &_sshKeyExchange)) + { + LOG_DEBUG("{}: ssh key exchange changed to {}", nameId(), _sshKeyExchange); + flow::ApplyChanges(); + doDeinitialize(); + } + if (ImGui::InputText("SSH Publickey Accepted Types", &_sshPublickeyAcceptedTypes)) + { + LOG_DEBUG("{}: ssh publickey accepted types changed to {}", nameId(), _sshPublickeyAcceptedTypes); + flow::ApplyChanges(); + doDeinitialize(); + } + ImGui::Spacing(); // Add spacing here + ImGui::Separator(); // Add a horizontal line + if (ImGui::InputInt("Output Interval in ms", &_outputInterval)) + { + LOG_DEBUG("{}: output interval changed to {}", nameId(), _outputInterval); + flow::ApplyChanges(); + } +} + +[[nodiscard]] json NAV::ArubaSensor::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["sshHost"] = _sshHost; + j["sshUser"] = _sshUser; + j["sshHostkeys"] = _sshHostkeys; + j["sshKeyExchange"] = _sshKeyExchange; + j["sshPublickeyAcceptedTypes"] = _sshPublickeyAcceptedTypes; + j["outputInterval"] = _outputInterval; + + return j; +} + +void NAV::ArubaSensor::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("sshHost")) + { + j.at("sshHost").get_to(_sshHost); + } + if (j.contains("sshUser")) + { + j.at("sshUser").get_to(_sshUser); + } + if (j.contains("sshHostkeys")) + { + j.at("sshHostkeys").get_to(_sshHostkeys); + } + if (j.contains("sshKeyExchange")) + { + j.at("sshKeyExchange").get_to(_sshKeyExchange); + } + if (j.contains("sshPublickeyAcceptedTypes")) + { + j.at("sshPublickeyAcceptedTypes").get_to(_sshPublickeyAcceptedTypes); + } + if (j.contains("outputInterval")) + { + j.at("outputInterval").get_to(_outputInterval); + } +} + +bool NAV::ArubaSensor::resetNode() +{ + return true; +} + +bool NAV::ArubaSensor::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + // SSH session + _session = ssh_new(); + if (_session == NULL) + { + LOG_INFO("{}: Failed to create SSH _session", nameId()); + return false; + } + LOG_DEBUG("{}: Successfully created SSH _session", nameId()); + + ssh_options_set(_session, SSH_OPTIONS_HOST, "192.168.178.45"); + ssh_options_set(_session, SSH_OPTIONS_USER, "admin"); + ssh_options_set(_session, SSH_OPTIONS_HOSTKEYS, "ssh-rsa"); + ssh_options_set(_session, SSH_OPTIONS_KEY_EXCHANGE, "ecdh-sha2-nistp256"); + ssh_options_set(_session, SSH_OPTIONS_PUBLICKEY_ACCEPTED_TYPES, "ssh-rsa"); + + // connect + if (ssh_connect(_session) != SSH_OK) + { + LOG_INFO("{}: Failed to connect to the router", nameId()); + ssh_free(_session); + return false; + } + LOG_DEBUG("{}: Successfully connected to the router", nameId()); + + // authenticate + if (ssh_userauth_password(_session, NULL, "CNNZKYJ51F") != SSH_AUTH_SUCCESS) + { + LOG_INFO("{}: Authentication failed", nameId()); + ssh_disconnect(_session); + ssh_free(_session); + return false; + } + LOG_DEBUG("{}: Authentication succeeded", nameId()); + + // channel + _channel = ssh_channel_new(_session); + if (_channel == NULL) + { + LOG_INFO("{}: Failed to create SSH channel", nameId()); + ssh_disconnect(_session); + ssh_free(_session); + return false; + } + LOG_DEBUG("{}: Successfully created SSH channel", nameId()); + + // open _session + if (ssh_channel_open_session(_channel) != SSH_OK) + { + LOG_INFO("{}: Failed to open an SSH _session", nameId()); + ssh_channel_free(_channel); + ssh_disconnect(_session); + ssh_free(_session); + return false; + } + LOG_DEBUG("{}: Successfully opened an SSH _session", nameId()); + + // pty + if (ssh_channel_request_pty(_channel) != SSH_OK) + { + LOG_INFO("{}: Failed to open pty", nameId()); + ssh_channel_free(_channel); + ssh_disconnect(_session); + ssh_free(_session); + return false; + } + LOG_DEBUG("{}: Successfully opened pty", nameId()); + + // shell + if (ssh_channel_request_shell(_channel) != SSH_OK) + { + LOG_INFO("{}: Failed to open shell", nameId()); + ssh_channel_free(_channel); + ssh_disconnect(_session); + ssh_free(_session); + return false; + } + LOG_DEBUG("{}: Successfully opened shell", nameId()); + + _timer.start(_outputInterval, readSensorDataThread, this); + + return true; +} + +void NAV::ArubaSensor::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); + + if (!isInitialized()) + { + return; + } + + ssh_channel_write(_channel, "exit\n", strlen("exit\n")); + ssh_channel_free(_channel); + ssh_disconnect(_session); + ssh_free(_session); + + if (_timer.is_running()) + { + _timer.stop(); + } + + // To Show the Deinitialization in the GUI // TODO Wieso? + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); +} + +void NAV::ArubaSensor::readSensorDataThread(void* userData) +{ + auto* node = static_cast(userData); + auto obs = std::make_shared(); + + obs->insTime = util::time::GetCurrentInsTime(); + + char buffer[1024]; + std::string receivedData; + size_t bytesRead; + // Send command to access point + ssh_channel_write(node->_channel, "show ap range scanning-results\n", strlen("show ap range scanning-results\n")); + // Read output + do { + bytesRead = static_cast(ssh_channel_read_timeout(node->_channel, buffer, sizeof(buffer), 0, 10)); // timeout in ms + if (bytesRead > 0) + { + receivedData.append(buffer, static_cast(bytesRead)); + } + } while (bytesRead > 0); + // Send command to clear output + ssh_channel_write(node->_channel, "clear range-scanning-result\n", strlen("clear range-scanning-result\n")); + + // Parse the received data + std::istringstream iss(receivedData); + std::string line; + + while (std::getline(iss, line) && line.find("Peer-bssid") == std::string::npos) + ; // Skip lines until the header "Peer-bssid" is found + + // Skip the header lines + std::getline(iss, line); + std::getline(iss, line); + + // MAC address validation + std::regex macRegex("^([0-9A-Fa-f]{2}[:-]){5}([0-9A-Fa-f]{2})$"); + + while (std::getline(iss, line) && !line.empty()) + { + std::istringstream lineStream(line); + std::string dummy; + std::string macAddress; + lineStream >> macAddress; + + int rtt, rssi, stdValue; + lineStream >> rtt >> rssi >> stdValue; + for (int i = 0; i < 17; ++i) + { + lineStream >> dummy; + } + std::string timeStamp1, timeStamp2; + lineStream >> timeStamp1; + lineStream >> timeStamp2; + + double measuredDistance = static_cast(rtt) * 1e-9 * cAir; + if (std::regex_match(macAddress, macRegex)) // Check if the MAC address is valid + { + InsTime_YMDHMS yearMonthDayHMS(std::stoi(timeStamp1.substr(0, 4)), std::stoi(timeStamp1.substr(5, 2)), std::stoi(timeStamp1.substr(8, 2)), std::stoi(timeStamp2.substr(0, 2)), std::stoi(timeStamp2.substr(3, 2)), std::stoi(timeStamp2.substr(6, 2))); + InsTime timeOfMeasurement(yearMonthDayHMS, UTC); + std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase + obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); + } + } + + node->invokeCallbacks(OUTPUT_PORT_INDEX_Aruba_OBS, obs); +} \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp new file mode 100644 index 000000000..2e219c39b --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp @@ -0,0 +1,93 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file ArubaSensor.hpp +/// @brief Aruba Sensor Class +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2020-03-19 + +#pragma once + +#include "internal/Node/Node.hpp" + +#include "util/CallbackTimer.hpp" +#include + +namespace NAV +{ +/// Aruba Sensor Class +class ArubaSensor : public Node +{ + public: + /// @brief Default constructor + ArubaSensor(); + /// @brief Destructor + ~ArubaSensor() override; + /// @brief Copy constructor + ArubaSensor(const ArubaSensor&) = delete; + /// @brief Move constructor + ArubaSensor(ArubaSensor&&) = delete; + /// @brief Copy assignment operator + ArubaSensor& operator=(const ArubaSensor&) = delete; + /// @brief Move assignment operator + ArubaSensor& operator=(ArubaSensor&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + /// @brief Resets the node. It is guaranteed that the node is initialized when this is called. + bool resetNode() override; + + private: + constexpr static size_t OUTPUT_PORT_INDEX_Aruba_OBS = 0; ///< @brief Flow (ArubaObs) + + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Function which performs the async data reading + /// @param[in] userData Pointer to the object + static void readSensorDataThread(void* userData); + + ssh_channel _channel; ///< @brief SSH channel + ssh_session _session; ///< @brief SSH session + + /// Timer object to handle async data requests + CallbackTimer _timer; + + /// Ssh options + std::string _sshHost; + std::string _sshUser; + std::string _sshHostkeys; + std::string _sshKeyExchange; + std::string _sshPublickeyAcceptedTypes; + + /// Output interval in ms + int _outputInterval; +}; + +} // namespace NAV \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp new file mode 100644 index 000000000..bfc9ed6b5 --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -0,0 +1,155 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "EspressifSensor.hpp" + +#include "util/Logger.hpp" + +#include "util/Time/TimeBase.hpp" + +#include "internal/gui/widgets/HelpMarker.hpp" + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +#include "NodeData/General/UartPacket.hpp" + +NAV::EspressifSensor::EspressifSensor() + : Node(typeStatic()), _sensor(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _onlyRealTime = true; + _hasConfig = true; + _guiConfigDefaultWindowSize = { 360, 70 }; + + // TODO: Update the library to handle different baudrates + _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_9600); + _sensorPort = "/dev/ttyACM0"; + + nm::CreateOutputPin(this, "UartPacket", Pin::Type::Flow, { NAV::UartPacket::type() }); +} + +NAV::EspressifSensor::~EspressifSensor() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::EspressifSensor::typeStatic() +{ + return "EspressifSensor"; +} + +std::string NAV::EspressifSensor::type() const +{ + return typeStatic(); +} + +std::string NAV::EspressifSensor::category() +{ + return "Data Provider"; +} + +void NAV::EspressifSensor::guiConfig() +{ + if (ImGui::InputTextWithHint("SensorPort", "/dev/ttyACM0", &_sensorPort)) + { + LOG_DEBUG("{}: SensorPort changed to {}", nameId(), _sensorPort); + flow::ApplyChanges(); + doDeinitialize(); + } + ImGui::SameLine(); + gui::widgets::HelpMarker("COM port where the sensor is attached to\n" + "- \"COM1\" (Windows format for physical and virtual (USB) serial port)\n" + "- \"/dev/ttyS1\" (Linux format for physical serial port)\n" + "- \"/dev/ttyUSB0\" (Linux format for virtual (USB) serial port)\n" + "- \"/dev/tty.usbserial-FTXXXXXX\" (Mac OS X format for virtual (USB) serial port)\n" + "- \"/dev/ttyS0\" (CYGWIN format. Usually the Windows COM port number minus 1. This would connect to COM1)"); +} + +[[nodiscard]] json NAV::EspressifSensor::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["UartSensor"] = UartSensor::save(); + + return j; +} + +void NAV::EspressifSensor::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("UartSensor")) + { + UartSensor::restore(j.at("UartSensor")); + } +} + +bool NAV::EspressifSensor::resetNode() +{ + return true; +} + +bool NAV::EspressifSensor::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + // connect to the sensor + try + { + _sensor->connect(_sensorPort, sensorBaudrate()); + + LOG_DEBUG("{} connected on port {} with baudrate {}", nameId(), _sensorPort, sensorBaudrate()); + } + catch (...) + { + LOG_ERROR("{} could not connect", nameId()); + return false; + } + + _sensor->registerAsyncPacketReceivedHandler(this, binaryAsyncMessageReceived); + + return true; +} + +void NAV::EspressifSensor::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); + + if (!isInitialized()) + { + return; + } + + if (_sensor->isConnected()) + { + try + { + _sensor->unregisterAsyncPacketReceivedHandler(); + } + catch (...) + {} + + _sensor->disconnect(); + } +} + +void NAV::EspressifSensor::binaryAsyncMessageReceived(void* userData, uart::protocol::Packet& p, [[maybe_unused]] size_t index) +{ + LOG_INFO("binaryAsyncMessageReceived"); + auto* ubSensor = static_cast(userData); + + auto obs = std::make_shared(p); + obs->insTime = util::time::GetCurrentInsTime(); + + ubSensor->invokeCallbacks(OUTPUT_PORT_INDEX_Espressif_OBS, obs); +} diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp new file mode 100644 index 000000000..63b964b47 --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp @@ -0,0 +1,81 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file EspressifSensor.hpp +/// @brief Espressif Sensor Class +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2020-03-19 + +#pragma once + +#include "internal/Node/Node.hpp" +#include "Nodes/DataProvider/Protocol/UartSensor.hpp" +#include "util/Vendor/Espressif/EspressifUartSensor.hpp" + +namespace NAV +{ +/// Espressif Sensor Class +class EspressifSensor : public Node, public UartSensor +{ + public: + /// @brief Default constructor + EspressifSensor(); + /// @brief Destructor + ~EspressifSensor() override; + /// @brief Copy constructor + EspressifSensor(const EspressifSensor&) = delete; + /// @brief Move constructor + EspressifSensor(EspressifSensor&&) = delete; + /// @brief Copy assignment operator + EspressifSensor& operator=(const EspressifSensor&) = delete; + /// @brief Move assignment operator + EspressifSensor& operator=(EspressifSensor&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + /// @brief Resets the node. It is guaranteed that the node is initialized when this is called. + bool resetNode() override; + + private: + constexpr static size_t OUTPUT_PORT_INDEX_Espressif_OBS = 0; ///< @brief Flow (EspressifObs) + + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Callback handler for notifications of new asynchronous data packets received + /// @param[in, out] userData Pointer to the data we supplied when we called registerAsyncPacketReceivedHandler + /// @param[in] p Encapsulation of the data packet. At this state, it has already been validated and identified as an asynchronous data message + /// @param[in] index Advanced usage item and can be safely ignored for now + static void binaryAsyncMessageReceived(void* userData, uart::protocol::Packet& p, size_t index); + + /// Sensor Object + vendor::espressif::EspressifUartSensor _sensor; +}; + +} // namespace NAV \ No newline at end of file diff --git a/src/util/Vendor/Espressif/EspressifUartSensor.cpp b/src/util/Vendor/Espressif/EspressifUartSensor.cpp new file mode 100644 index 000000000..375feb5d6 --- /dev/null +++ b/src/util/Vendor/Espressif/EspressifUartSensor.cpp @@ -0,0 +1,168 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "EspressifUartSensor.hpp" + +#include "EspressifUtilities.hpp" +#include "util/Logger.hpp" + +NAV::vendor::espressif::EspressifUartSensor::EspressifUartSensor(std::string name) + : _name(std::move(name)), _buffer(uart::sensors::UartSensor::DefaultReadBufferSize) +{ + resetTracking(); +} + +void NAV::vendor::espressif::EspressifUartSensor::resetTracking() +{ + _currentlyBuildingBinaryPacket = false; + + _binarySyncChar2Found = false; + _binaryPayloadLength1Found = false; + _binaryPayloadLength2Found = false; + +#if (defined(__GNUC__) || defined(__GNUG__)) && !defined(__clang__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wstringop-overflow" +#endif + _binaryPayloadLength = 0; +#if (defined(__GNUC__) || defined(__GNUG__)) && !defined(__clang__) + #pragma GCC diagnostic pop +#endif + + _buffer.resize(0); + _numOfBytesRemainingForCompletePacket = 0; +} + +std::unique_ptr NAV::vendor::espressif::EspressifUartSensor::findPacket(uint8_t dataByte) +{ + if (_buffer.size() == _buffer.capacity()) + { + // Buffer is full + resetTracking(); + LOG_ERROR("{}: Discarding current packet, because buffer is full.", _name); + } + + if (!_currentlyBuildingBinaryPacket) + { + // This byte must be the start char + if (dataByte == BINARY_SYNC_CHAR_1) + { + resetTracking(); + _currentlyBuildingBinaryPacket = true; + _buffer.push_back(dataByte); + } + } + else if (_currentlyBuildingBinaryPacket) + { + _buffer.push_back(dataByte); + + if (!_binarySyncChar2Found) + { + // This byte must be the second sync char + if (dataByte == BINARY_SYNC_CHAR_2) + { + _binarySyncChar2Found = true; + } + else + { + resetTracking(); + } + } + else if (!_binaryPayloadLength1Found) + { + _binaryPayloadLength1Found = true; + _binaryPayloadLength = static_cast(dataByte); + } + else if (!_binaryPayloadLength2Found) + { + _binaryPayloadLength2Found = true; + _binaryPayloadLength |= static_cast(static_cast(dataByte) << 8U); + _binaryPayloadLength = uart::stoh(_binaryPayloadLength, ENDIANNESS); + _numOfBytesRemainingForCompletePacket = _binaryPayloadLength + 2U; + LOG_DEBUG("{}: Binary packet: payload length={}", _name, _binaryPayloadLength); + } + else + { + // We are currently collecting data for our packet. + _numOfBytesRemainingForCompletePacket--; + + if (_numOfBytesRemainingForCompletePacket == 0) + { + // We have a possible binary packet! + auto p = std::make_unique(_buffer, &_sensor); + + if (p->isValid()) + { + // We have a valid binary packet!!!. + resetTracking(); + return p; + } + // Invalid packet! + LOG_ERROR("{}: Invalid binary packet: payload length={}", _name, _binaryPayloadLength); + resetTracking(); + } + } + } + + return nullptr; +} + +void NAV::vendor::espressif::EspressifUartSensor::packetFinderFunction(const std::vector& data, const uart::xplat::TimeStamp& timestamp, uart::sensors::UartSensor::ValidPacketFoundHandler dispatchPacket, void* dispatchPacketUserData, void* userData) +{ + auto* sensor = static_cast(userData); + + for (size_t i = 0; i < data.size(); i++, sensor->_runningDataIndex++) + { + auto packetPointer = sensor->findPacket(data.at(i)); + + if (packetPointer != nullptr) + { + uart::protocol::Packet packet = *packetPointer; + dispatchPacket(dispatchPacketUserData, packet, sensor->_runningDataIndex, timestamp); + } + } +} + +uart::protocol::Packet::Type NAV::vendor::espressif::EspressifUartSensor::packetTypeFunction(const uart::protocol::Packet& packet) +{ + if (packet.getRawData().at(0) == BINARY_SYNC_CHAR_1) + { + if (packet.getRawData().at(1) == BINARY_SYNC_CHAR_2) + { + return uart::protocol::Packet::Type::TYPE_BINARY; + } + } + + return uart::protocol::Packet::Type::TYPE_UNKNOWN; +} + +bool NAV::vendor::espressif::EspressifUartSensor::checksumFunction(const uart::protocol::Packet& packet) +{ + if (packet.getRawDataLength() <= 8) + { + return false; + } + + std::pair checksum = espressif::checksumUBX(packet.getRawData()); + + return packet.getRawData().at(packet.getRawDataLength() - 2) == checksum.first + && packet.getRawData().at(packet.getRawDataLength() - 1) == checksum.second; + + LOG_CRITICAL("Can't calculate checksum of packet with unknown type"); + return false; +} + +bool NAV::vendor::espressif::EspressifUartSensor::isErrorFunction([[maybe_unused]] const uart::protocol::Packet& packet) +{ + return false; +} + +bool NAV::vendor::espressif::EspressifUartSensor::isResponseFunction([[maybe_unused]] const uart::protocol::Packet& packet) +{ + return false; +} \ No newline at end of file diff --git a/src/util/Vendor/Espressif/EspressifUartSensor.hpp b/src/util/Vendor/Espressif/EspressifUartSensor.hpp new file mode 100644 index 000000000..74fff7f6f --- /dev/null +++ b/src/util/Vendor/Espressif/EspressifUartSensor.hpp @@ -0,0 +1,120 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file EspressifUartSensor.hpp +/// @brief Class to read out Espressif Sensors +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2020-07-22 + +#pragma once + +#include + +#include "uart/sensors/sensors.hpp" + +namespace NAV::vendor::espressif +{ +/// @brief Class to read out Espressif Sensors +class EspressifUartSensor +{ + public: + /// @brief Constructor + /// @param[in] name Name of the Parent Node + explicit EspressifUartSensor(std::string name); + + /// @brief Default constructor + EspressifUartSensor() = default; + /// @brief Destructor + ~EspressifUartSensor() = default; + /// @brief Copy constructor + EspressifUartSensor(const EspressifUartSensor&) = delete; + /// @brief Move constructor + EspressifUartSensor(EspressifUartSensor&&) = delete; + /// @brief Copy assignment operator + EspressifUartSensor& operator=(const EspressifUartSensor&) = delete; + /// @brief Move assignment operator + EspressifUartSensor& operator=(EspressifUartSensor&&) = delete; + /// @brief Arrow operator overload + uart::sensors::UartSensor* operator->() { return &_sensor; }; + + /// @brief Collects data bytes and searches for packages inside of them + /// @param[in] dataByte The next data byte + /// @return nullptr if no packet found yet, otherwise a pointer to the packet + std::unique_ptr findPacket(uint8_t dataByte); + + static constexpr uint8_t BINARY_SYNC_CHAR_1 = 0xB5; ///< µ - First sync character which begins a new binary message + static constexpr uint8_t BINARY_SYNC_CHAR_2 = 0x62; ///< b - Second sync character which begins a new binary message + + private: + /// Name of the Parent Node + const std::string _name; + + /// UartSensor object which handles the UART interface + uart::sensors::UartSensor _sensor{ ENDIANNESS, + packetFinderFunction, + this, + packetTypeFunction, + checksumFunction, + isErrorFunction, + isResponseFunction, + PACKET_HEADER_LENGTH }; + + /// @brief Function which is called to find packets in the provided data buffer + /// @param[in] data Raw data buffer which has potential packets inside + /// @param[in] timestamp Timestamp then the data in the buffer was received + /// @param[in] dispatchPacket Function to call when a complete packet was found + /// @param[in] dispatchPacketUserData User data to forward to the dispatchPacket function + /// @param[in] userData User data provided when regisering this function. Should contain the sensor object + static void packetFinderFunction(const std::vector& data, + const uart::xplat::TimeStamp& timestamp, + uart::sensors::UartSensor::ValidPacketFoundHandler dispatchPacket, void* dispatchPacketUserData, + void* userData); + + /// @brief Function which is called to determine the packet type (ascii/binary) + /// @param[in] packet Packet to check the type of + /// @return The type of the packet + static uart::protocol::Packet::Type packetTypeFunction(const uart::protocol::Packet& packet); + + /// @brief Function which is called to verify packet integrity + /// @param[in] packet Packet to calculate the checksum for + /// @return True if the packet is fault free + static bool checksumFunction(const uart::protocol::Packet& packet); + + /// @brief Function which determines, if the packet is an Error Packet + /// @param[in] packet The packet to check + static bool isErrorFunction(const uart::protocol::Packet& packet); + + /// @brief Function which determines, if the packet is a Response + /// @param[in] packet The packet to check + static bool isResponseFunction(const uart::protocol::Packet& packet); + + static constexpr uart::Endianness ENDIANNESS = uart::Endianness::ENDIAN_LITTLE; ///< Endianess of the sensor + static constexpr size_t PACKET_HEADER_LENGTH = 2; ///< Length of the header of each packet + + bool _currentlyBuildingBinaryPacket{ false }; ///< Flag if currently a binary packet is built + + bool _binarySyncChar2Found{ false }; ///< Flag if the second binary end character was found + bool _binaryPayloadLength1Found{ false }; ///< Flag if the first byte of the payload length was found + bool _binaryPayloadLength2Found{ false }; ///< Flag if the second byte of the payload length was found + + /// Payload length of the current packet + uint16_t _binaryPayloadLength{ 0 }; + + /// Buffer to collect messages till they are complete + std::vector _buffer; + + /// Used for correlating raw data with where the packet was found for the end user. + size_t _runningDataIndex{ 0 }; + /// Amount of bytes remaining for a complete packet + size_t _numOfBytesRemainingForCompletePacket{ 0 }; + + /// @brief Resets the current message tracking + void resetTracking(); +}; + +} // namespace NAV::vendor::espressif diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp new file mode 100644 index 000000000..117b00272 --- /dev/null +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -0,0 +1,52 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "EspressifUtilities.hpp" + +#include "util/Eigen.hpp" +#include "Navigation/Transformations/CoordinateFrames.hpp" +#include "util/Logger.hpp" + +#include "util/Time/TimeBase.hpp" + +void NAV::vendor::espressif::decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet) +{ + obs->insTime = util::time::GetCurrentInsTime(); + if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) + { + obs->payloadLength = packet.extractUint16(); // TODO remove + size_t measurementLength = 14; // TODO irgendwo anders definieren + size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U + for (size_t i = 0; i < numOfMeasurement; i++) + { + std::array macAdress{ packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8() }; + double measuredDistance = packet.extractDouble(); + obs->data.push_back({ macAdress, measuredDistance }); + // Log the measurement details + LOG_DATA("EspressifObs Measurement {}: RouterID {}, Position ({}, {}, {}), Distance {}", + i + 1, routerId, routerPosition[0], routerPosition[1], routerPosition[2], measuredDistance); + } + } + else + { + LOG_DEBUG("Received non-binary packet. Ignoring."); + } +} + +std::pair NAV::vendor::espressif::checksumUBX(const std::vector& data) +{ + uint8_t cka = 0; + uint8_t ckb = 0; + + for (size_t i = 2; i < data.size() - 2; i++) + { + cka += data.at(i); + ckb += cka; + } + return std::make_pair(cka, ckb); +} \ No newline at end of file diff --git a/src/util/Vendor/Espressif/EspressifUtilities.hpp b/src/util/Vendor/Espressif/EspressifUtilities.hpp new file mode 100644 index 000000000..68f572537 --- /dev/null +++ b/src/util/Vendor/Espressif/EspressifUtilities.hpp @@ -0,0 +1,36 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file EspressifUtilities.hpp +/// @brief Helper Functions to work with Espressif Sensors +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2020-07-22 + +#pragma once + +#include +#include +#include + +#include "uart/protocol/packet.hpp" + +#include "NodeData/WiFi/EspressifObs.hpp" + +namespace NAV::vendor::espressif +{ +/// @brief Decrypts the provided Espressif observation +/// @param[in] obs Espressif Observation to decrypt +/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) +void decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet); + +/// @brief Calculates the two UBX checksums for the provided data vector +/// @param[in] data Data Vector for which the checksum should be calculated +/// @return The checksums CK_A and CK_B +std::pair checksumUBX(const std::vector& data); + +} // namespace NAV::vendor::espressif \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c92d23abb..b173b727b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -50,6 +50,7 @@ target_link_libraries( implot application libvncxx + ssh libUartSensor) if(NOT APPLE AND NOT WIN32) From f7147b4493f8a10b2121427b67b983dc8e2404ee Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Sun, 7 Jan 2024 20:38:07 +0100 Subject: [PATCH 02/27] Some small changes --- src/NodeData/WiFi/EspressifObs.hpp | 3 +- .../DataProcessor/WiFi/WiFiPositioning.cpp | 11 +++-- .../DataProcessor/WiFi/WiFiPositioning.hpp | 1 + .../Vendor/Espressif/EspressifUtilities.cpp | 44 ++++++++++++++----- 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/src/NodeData/WiFi/EspressifObs.hpp b/src/NodeData/WiFi/EspressifObs.hpp index 933093e9f..14517b8d3 100644 --- a/src/NodeData/WiFi/EspressifObs.hpp +++ b/src/NodeData/WiFi/EspressifObs.hpp @@ -40,7 +40,8 @@ class EspressifObs : public NodeData struct FtmObs { - std::array macAddress; + std::string macAddress; + InsTime time; double measuredDistance; }; diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index c1079857e..9a68af65e 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -330,19 +330,18 @@ void NAV::WiFiPositioning::recvEspressifObs(NAV::InputPin::NodeDataQueue& queue, for (auto const& obs : espressifObs->data) { - std::string macAddressString = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", obs.macAddress[0], obs.macAddress[1], obs.macAddress[2], obs.macAddress[3], obs.macAddress[4], obs.macAddress[5]); - auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), macAddressString); + auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs.macAddress); if (it != _deviceMacAddresses.end()) // Device already exists { // Get the index of the found element size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); if (_frame == Frame::LLA) { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.measuredDistance }); + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.measuredDistance }); } else if (_frame == Frame::ECEF) { - _devices.push_back({ _devicePositions.at(index), obs.measuredDistance }); + _devices.push_back({ _devicePositions.at(index), obs.time, obs.measuredDistance }); } } } @@ -361,11 +360,11 @@ void NAV::WiFiPositioning::recvArubaObs(NAV::InputPin::NodeDataQueue& queue, siz size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); if (_frame == Frame::LLA) { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.measuredDistance }); + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.measuredDistance }); } else if (_frame == Frame::ECEF) { - _devices.push_back({ _devicePositions.at(index), obs.measuredDistance }); + _devices.push_back({ _devicePositions.at(index), obs.time, obs.measuredDistance }); } } } diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp index 894ab8d53..523bfcaaf 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp @@ -107,6 +107,7 @@ class WiFiPositioning : public Node struct Device { Eigen::Vector3d position; + InsTime time; double distance; }; std::vector _devices; diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 117b00272..4598bc5da 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -11,26 +11,46 @@ #include "util/Eigen.hpp" #include "Navigation/Transformations/CoordinateFrames.hpp" #include "util/Logger.hpp" +#include #include "util/Time/TimeBase.hpp" +// void NAV::vendor::espressif::decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet) +// { +// obs->insTime = util::time::GetCurrentInsTime(); +// if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) +// { +// obs->payloadLength = packet.extractUint16(); // TODO remove +// size_t measurementLength = 14; // TODO irgendwo anders definieren +// size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U +// for (size_t i = 0; i < numOfMeasurement; i++) +// { +// std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); +// std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase +// double measuredDistance = packet.extractDouble(); +// obs->data.push_back({ macAddress, measuredDistance }); +// // Log the measurement details +// LOG_DATA("EspressifObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); +// } +// } +// else +// { +// LOG_DEBUG("Received non-binary packet. Ignoring."); +// } +// } + void NAV::vendor::espressif::decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet) { obs->insTime = util::time::GetCurrentInsTime(); if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) { - obs->payloadLength = packet.extractUint16(); // TODO remove - size_t measurementLength = 14; // TODO irgendwo anders definieren - size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U - for (size_t i = 0; i < numOfMeasurement; i++) - { - std::array macAdress{ packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8() }; - double measuredDistance = packet.extractDouble(); - obs->data.push_back({ macAdress, measuredDistance }); - // Log the measurement details - LOG_DATA("EspressifObs Measurement {}: RouterID {}, Position ({}, {}, {}), Distance {}", - i + 1, routerId, routerPosition[0], routerPosition[1], routerPosition[2], measuredDistance); - } + obs->payloadLength = packet.extractUint16(); // TODO remove + std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); + std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase + double measuredDistance = packet.extractDouble(); + obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); + // Log the measurement details + LOG_DATA("EspressifObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); } else { From 0965c3a81afc1d4e2d32f29f9e3eac22b53be890 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Sun, 3 Mar 2024 21:24:18 +0100 Subject: [PATCH 03/27] =?UTF-8?q?Nur=20noch=20WifiObs=20f=C3=BCr=20beide?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/NodeData/WiFi/ArubaObs.hpp | 48 --- .../WiFi/{EspressifObs.hpp => WiFiObs.hpp} | 8 +- src/NodeRegistry.cpp | 6 +- .../Converter/GNSS/UartPacketConverter.cpp | 66 +++- .../Converter/GNSS/UartPacketConverter.hpp | 26 +- .../DataProcessor/WiFi/WiFiPositioning.cpp | 289 +++++++++--------- .../DataProcessor/WiFi/WiFiPositioning.hpp | 31 +- .../DataProvider/WiFi/Sensors/ArubaSensor.cpp | 26 +- .../DataProvider/WiFi/Sensors/ArubaSensor.hpp | 3 +- .../WiFi/Sensors/EspressifSensor.cpp | 2 +- .../WiFi/Sensors/EspressifSensor.hpp | 2 +- .../Vendor/Espressif/EspressifUtilities.cpp | 115 +++++-- .../Vendor/Espressif/EspressifUtilities.hpp | 21 +- 13 files changed, 380 insertions(+), 263 deletions(-) delete mode 100644 src/NodeData/WiFi/ArubaObs.hpp rename src/NodeData/WiFi/{EspressifObs.hpp => WiFiObs.hpp} (89%) diff --git a/src/NodeData/WiFi/ArubaObs.hpp b/src/NodeData/WiFi/ArubaObs.hpp deleted file mode 100644 index eedd13379..000000000 --- a/src/NodeData/WiFi/ArubaObs.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// This file is part of INSTINCT, the INS Toolkit for Integrated -// Navigation Concepts and Training by the Institute of Navigation of -// the University of Stuttgart, Germany. -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License, v. 2.0. If a copy of the MPL was not distributed with this -// file, You can obtain one at https://mozilla.org/MPL/2.0/. - -/// @file ArubaObs.hpp -/// @brief Aruba Observation Class -/// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date TODO - -#pragma once - -#include "NodeData/NodeData.hpp" - -namespace NAV -{ -/// Aruba Observation Class -class ArubaObs : public NodeData -{ - public: - /// @brief Returns the type of the data class - /// @return The data type - [[nodiscard]] static std::string type() - { - return "ArubaObs"; - } - - /// @brief Returns the parent types of the data class - /// @return The parent data types - [[nodiscard]] static std::vector parentTypes() - { - return { NodeData::type() }; - } - - struct FtmObs - { - std::string macAddress; - InsTime time; - double measuredDistance; - }; - - std::vector data; -}; - -} // namespace NAV diff --git a/src/NodeData/WiFi/EspressifObs.hpp b/src/NodeData/WiFi/WiFiObs.hpp similarity index 89% rename from src/NodeData/WiFi/EspressifObs.hpp rename to src/NodeData/WiFi/WiFiObs.hpp index 14517b8d3..1cd3113a9 100644 --- a/src/NodeData/WiFi/EspressifObs.hpp +++ b/src/NodeData/WiFi/WiFiObs.hpp @@ -6,7 +6,7 @@ // License, v. 2.0. If a copy of the MPL was not distributed with this // file, You can obtain one at https://mozilla.org/MPL/2.0/. -/// @file EspressifObs.hpp +/// @file WiFiObs.hpp /// @brief Espressif Observation Class /// @author T. Topp (topp@ins.uni-stuttgart.de) /// @date TODO @@ -18,14 +18,14 @@ namespace NAV { /// Espressif Observation Class -class EspressifObs : public NodeData +class WiFiObs : public NodeData { public: /// @brief Returns the type of the data class /// @return The data type [[nodiscard]] static std::string type() { - return "EspressifObs"; + return "WiFiObs"; } /// @brief Returns the parent types of the data class @@ -42,7 +42,7 @@ class EspressifObs : public NodeData { std::string macAddress; InsTime time; - double measuredDistance; + double distance; }; std::vector data; diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index 6fb2df975..c2f65ea2f 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -318,8 +318,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() #include "NodeData/State/Pos.hpp" #include "NodeData/State/PosVel.hpp" #include "NodeData/State/PosVelAtt.hpp" -#include "NodeData/WiFi/ArubaObs.hpp" -#include "NodeData/WiFi/EspressifObs.hpp" +#include "NodeData/WiFi/WiFiObs.hpp" void NAV::NodeRegistry::RegisterNodeDataTypes() { @@ -346,6 +345,5 @@ void NAV::NodeRegistry::RegisterNodeDataTypes() registerNodeDataType(); registerNodeDataType(); // WiFi - registerNodeDataType(); - registerNodeDataType(); + registerNodeDataType(); } \ No newline at end of file diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index 55403bd9c..35b586abe 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -55,7 +55,7 @@ std::string NAV::UartPacketConverter::category() void NAV::UartPacketConverter::guiConfig() { - if (ImGui::Combo(fmt::format("Output Type ##{}", size_t(id)).c_str(), reinterpret_cast(&_outputType), "UbloxObs\0EmlidObs\0EspressifObs\0\0")) + if (ImGui::Combo(fmt::format("Output Type ##{}", size_t(id)).c_str(), reinterpret_cast(&_outputType), "UbloxObs\0EmlidObs\0WiFiObs\0\0")) { std::string outputTypeString; switch (_outputType) @@ -67,7 +67,7 @@ void NAV::UartPacketConverter::guiConfig() outputTypeString = "EmlidObs"; break; default: - outputTypeString = "EspressifObs"; + outputTypeString = "WiFiObs"; } LOG_DEBUG("{}: Output Type changed to {}", nameId(), outputTypeString); @@ -82,10 +82,10 @@ void NAV::UartPacketConverter::guiConfig() outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EmlidObs::type() }; outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EmlidObs::type(); } - else if (_outputType == OutputType_EspressifObs) + else if (_outputType == OutputType_WiFiObs) { - outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EspressifObs::type() }; - outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EspressifObs::type(); + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::WiFiObs::type() }; + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::WiFiObs::type(); } for (auto& link : outputPins.front().links) @@ -98,6 +98,19 @@ void NAV::UartPacketConverter::guiConfig() flow::ApplyChanges(); } + if (_outputType == OutputType_WiFiObs) + { + if (ImGui::Combo(fmt::format("Number of measurements##{}", size_t(id)).c_str(), reinterpret_cast(&_numberOfMeasurements), "Single\0Multiple\0\0")) + { + LOG_DEBUG("{}: Number of measurements changed to {}", nameId(), _numberOfMeasurements == NumberOfMeasurements::SINGLE ? "single" : "multiple"); + flow::ApplyChanges(); + } + if (ImGui::Combo(fmt::format("Time system##{}", size_t(id)).c_str(), reinterpret_cast(&_timeSystem), "INSTINCT\0WiFi device\0\0")) + { + LOG_DEBUG("{}: Frame changed to {}", nameId(), _timeSystem == WifiTimeSystem::INSTINCT ? "INSTINCT" : "WiFi device"); + flow::ApplyChanges(); + } + } } [[nodiscard]] json NAV::UartPacketConverter::save() const @@ -107,6 +120,8 @@ void NAV::UartPacketConverter::guiConfig() json j; j["outputType"] = _outputType; + j["numberOfMeasurements"] = _numberOfMeasurements; + j["timeSystem"] = _timeSystem; return j; } @@ -131,13 +146,21 @@ void NAV::UartPacketConverter::restore(json const& j) outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EmlidObs::type() }; outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EmlidObs::type(); } - else if (_outputType == OutputType_EspressifObs) + else if (_outputType == OutputType_WiFiObs) { - outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::EspressifObs::type() }; - outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::EspressifObs::type(); + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).dataIdentifier = { NAV::WiFiObs::type() }; + outputPins.at(OUTPUT_PORT_INDEX_CONVERTED).name = NAV::WiFiObs::type(); } } } + if (j.contains("numberOfMeasurements")) + { + j.at("numberOfMeasurements").get_to(_numberOfMeasurements); + } + if (j.contains("timeSystem")) + { + j.at("timeSystem").get_to(_timeSystem); + } } bool NAV::UartPacketConverter::initialize() @@ -160,11 +183,32 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, s vendor::ublox::decryptUbloxObs(obs, packet); convertedData = obs; } - else if (_outputType == OutputType_EspressifObs) + else if (_outputType == OutputType_WiFiObs) { - auto obs = std::make_shared(); + auto obs = std::make_shared(); auto packet = uartPacket->raw; - vendor::espressif::decryptEspressifObs(obs, packet); + if (_numberOfMeasurements == NumberOfMeasurements::SINGLE) + { + if (_timeSystem == WifiTimeSystem::INSTINCT) + { + vendor::espressif::decryptSingleWiFiObsInstinctTime(obs, packet); + } + else + { + vendor::espressif::decryptSingleWiFiObsDeviceTime(obs, packet); + } + } + else + { + if (_timeSystem == WifiTimeSystem::INSTINCT) + { + vendor::espressif::decryptMultipleWiFiObsInstinctTime(obs, packet); + } + else + { + vendor::espressif::decryptMultipleWiFiObsDeviceTime(obs, packet); + } + } convertedData = obs; } else /* if (_outputType == OutputType_EmlidObs) */ diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp index b723f1e15..aa5fa3ec4 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp @@ -18,7 +18,7 @@ #include "NodeData/General/UartPacket.hpp" #include "NodeData/GNSS/UbloxObs.hpp" #include "NodeData/GNSS/EmlidObs.hpp" -#include "NodeData/WiFi/EspressifObs.hpp" +#include "NodeData/WiFi/WiFiObs.hpp" #include #include @@ -69,14 +69,32 @@ class UartPacketConverter : public Node /// Enum specifying the type of the output message enum OutputType { - OutputType_UbloxObs, ///< Extract UbloxObs data - OutputType_EmlidObs, ///< Extract EmlidObs data - OutputType_EspressifObs, ///< Extract EspressifObs data + OutputType_UbloxObs, ///< Extract UbloxObs data + OutputType_EmlidObs, ///< Extract EmlidObs data + OutputType_WiFiObs, ///< Extract WiFiObs data }; /// The selected output type in the GUI OutputType _outputType = OutputType_UbloxObs; + /// @brief Number of measurements in one message + enum class NumberOfMeasurements : int + { + SINGLE, ///< Single measurement + MULTIPLE, ///< Multiple measurements + }; + /// Number of measurements in one message + NumberOfMeasurements _numberOfMeasurements = NumberOfMeasurements::SINGLE; + + /// @brief Specifies the time system in which the timestamps of the FTM measurements are. + enum class WifiTimeSystem : int + { + INSTINCT, ///< time system of INSTINCT + WIFIDEVICE, ///< time system of the WiFi device + }; + /// Time system of the FTM measurements + WifiTimeSystem _timeSystem = WifiTimeSystem::INSTINCT; + /// @brief Initialize the node bool initialize() override; diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index 9a68af65e..b5d15d4bc 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -25,8 +25,7 @@ namespace nm = NAV::NodeManager; #include "internal/FlowManager.hpp" -#include "NodeData/WiFi/EspressifObs.hpp" -#include "NodeData/WiFi/ArubaObs.hpp" +#include "NodeData/WiFi/WiFiObs.hpp" #include "NodeData/State/PosVelAtt.hpp" #include "Navigation/GNSS/Functions.hpp" #include "Navigation/Transformations/CoordinateFrames.hpp" @@ -42,8 +41,7 @@ NAV::WiFiPositioning::WiFiPositioning() _guiConfigDefaultWindowSize = { 575, 300 }; _outputInterval = 3000; - nm::CreateInputPin(this, NAV::EspressifObs::type().c_str(), Pin::Type::Flow, { NAV::EspressifObs::type() }, &WiFiPositioning::recvEspressifObs); - nm::CreateInputPin(this, NAV::ArubaObs::type().c_str(), Pin::Type::Flow, { NAV::ArubaObs::type() }, &WiFiPositioning::recvArubaObs); + updateNumberOfInputPins(); nm::CreateOutputPin(this, NAV::Pos::type().c_str(), Pin::Type::Flow, { NAV::Pos::type() }); } @@ -72,6 +70,22 @@ void NAV::WiFiPositioning::guiConfig() { float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio(); + if (ImGui::Button(fmt::format("Add Input Pin##{}", size_t(id)).c_str())) + { + _nWifiInputPins++; + LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nWifiInputPins); + flow::ApplyChanges(); + updateNumberOfInputPins(); + } + ImGui::SameLine(); + if (ImGui::Button(fmt::format("Delete Input Pin##{}", size_t(id)).c_str())) + { + _nWifiInputPins--; + LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nWifiInputPins); + flow::ApplyChanges(); + updateNumberOfInputPins(); + } + ImGui::SetNextItemWidth(250 * gui::NodeEditorApplication::windowFontRatio()); if (_numOfDevices == 0) @@ -251,12 +265,10 @@ void NAV::WiFiPositioning::guiConfig() ImGui::SameLine(); gui::widgets::HelpMarker("The third coordinate of the devices is not utilized in a two-dimensional solution."); flow::ApplyChanges(); - if (ImGui::InputInt("Output Interval in ms", &_outputInterval)) + if (ImGui::InputInt("Output interval [ms]", &_outputInterval)) { LOG_DEBUG("{}: output interval changed to {}", nameId(), _outputInterval); } - ImGui::SameLine(); - gui::widgets::HelpMarker("The output interval should be at least as large as that of the sensors."); flow::ApplyChanges(); } } @@ -267,6 +279,7 @@ void NAV::WiFiPositioning::guiConfig() json j; + j["nWifiInputPins"] = _nWifiInputPins; j["frame"] = static_cast(_frame); j["deviceMacAddresses"] = _deviceMacAddresses; j["devicePositions"] = _devicePositions; @@ -281,6 +294,11 @@ void NAV::WiFiPositioning::restore(json const& j) { LOG_TRACE("{}: called", nameId()); + if (j.contains("nNavInfoPins")) + { + j.at("nNavInfoPins").get_to(_nWifiInputPins); + updateNumberOfInputPins(); + } if (j.contains("frame")) { j.at("frame").get_to(_frame); @@ -315,20 +333,38 @@ bool NAV::WiFiPositioning::initialize() LOG_DEBUG("WiFiPositioning initialized"); + _devices.clear(); + _timer.start(_outputInterval, lsqSolution3d, this); + return true; } void NAV::WiFiPositioning::deinitialize() { LOG_TRACE("{}: called", nameId()); + + if (_timer.is_running()) + { + _timer.stop(); + } } -void NAV::WiFiPositioning::recvEspressifObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +void NAV::WiFiPositioning::updateNumberOfInputPins() { - _devices.clear(); - auto espressifObs = std::static_pointer_cast(queue.extract_front()); + while (inputPins.size() < _nWifiInputPins) + { + nm::CreateInputPin(this, NAV::WiFiObs::type().c_str(), Pin::Type::Flow, { NAV::WiFiObs::type() }, &WiFiPositioning::recvWiFiObs); + } + while (inputPins.size() > _nWifiInputPins) + { + nm::DeleteInputPin(inputPins.back()); + } +} - for (auto const& obs : espressifObs->data) +void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +{ + auto wifiObs = std::static_pointer_cast(queue.extract_front()); + for (auto const& obs : wifiObs->data) { auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs.macAddress); if (it != _deviceMacAddresses.end()) // Device already exists @@ -337,153 +373,128 @@ void NAV::WiFiPositioning::recvEspressifObs(NAV::InputPin::NodeDataQueue& queue, size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); if (_frame == Frame::LLA) { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.measuredDistance }); + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.distance }); } else if (_frame == Frame::ECEF) { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.measuredDistance }); + _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance }); } + else if (_frame == Frame::NED) + { + _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance }); + } + // aufruf kf update } } } -void NAV::WiFiPositioning::recvArubaObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +void NAV::WiFiPositioning::lsqSolution2d(void* userData) { - _devices.clear(); - auto arubaObs = std::static_pointer_cast(queue.extract_front()); - for (auto const& obs : arubaObs->data) + auto* node = static_cast(userData); + if (node->_devices.size() < 3) { - auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs.macAddress); - if (it != _deviceMacAddresses.end()) // Device already exists + LOG_DEBUG("{}: Received less than 3 observations. Can't compute position", node->nameId()); + return; + } + else + { + LOG_DEBUG("{}: Received {} observations", node->nameId(), node->_devices.size()); + } + node->_e_position = { 1, 1, 0 }; + + Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(node->_devices.size()), static_cast(2)); + Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(node->_devices.size())); + size_t numMeasurements = node->_devices.size(); + LeastSquaresResult + lsq; + for (size_t o = 0; o < 10; o++) + { + LOG_DATA("{}: Iteration {}", nameId(), o); + for (size_t i = 0; i < numMeasurements; i++) { - // Get the index of the found element - size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); - if (_frame == Frame::LLA) + double distEst = (node->_devices.at(i).position.head<2>() - node->_e_position.head<2>()).norm(); + ddist(static_cast(i)) = node->_devices.at(i).distance - distEst; + Eigen::Vector2d e_lineOfSightUnitVector = Eigen::Vector2d::Zero(); + if ((node->_e_position.head<2>() - node->_devices.at(i).position.head<2>()).norm() != 0) // Check if it is the same position { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.measuredDistance }); - } - else if (_frame == Frame::ECEF) - { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.measuredDistance }); + e_lineOfSightUnitVector = (node->_e_position.head<2>() - node->_devices.at(i).position.head<2>()) / (node->_e_position.head<2>() - node->_devices.at(i).position.head<2>()).norm(); } + e_H.block<1, 2>(static_cast(i), 0) = -e_lineOfSightUnitVector; } - } -} + // std::cout << "Matrix:\n" + // << e_H << "\n"; + Eigen::Vector2d dx = (e_H.transpose() * e_H).inverse() * e_H.transpose() * ddist; + // LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); + // LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); -// void NAV::WiFiPositioning::lsqSolution2d() -// { -// LOG_DEBUG("{}: Received {} observations", nameId(), _devices.size()); - -// if (_devices.size() < 3) -// { -// LOG_WARN("{}: Received less than 3 observations. Can't compute position", nameId()); -// return; -// } -// else if (_devices.size() == 3 && _calc2dPosition) -// { -// LOG_WARN("{}: Received 3 observations. Only 2D position is possible", nameId()); -// } -// else -// { -// LOG_DEBUG("{}: Received {} observations", nameId(), espressifObs->data.size()); -// } -// _e_position = Eigen::Vector3d::Zero(); - -// std::vector routerPositions; -// std::vector distMeas; -// routerPositions.reserve(espressifObs->data.size()); - -// for (auto const& obs : espressifObs->data) -// { -// routerPositions.emplace_back(static_cast(obs.routerPosition[0]), static_cast(obs.routerPosition[1])); // Remove [2] -// distMeas.emplace_back(static_cast(obs.measuredDistance)); -// } - -// Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(distMeas.size()), 2); -// Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(distMeas.size())); -// LeastSquaresResult lsq; -// for (size_t o = 0; o < 10; o++) -// { -// LOG_DATA("{}: Iteration {}", nameId(), o); -// size_t ix = 0; -// for (auto const& routerPosition : routerPositions) -// { -// double distEst = (routerPosition - _e_position.head<2>()).norm(); -// Eigen::Vector2d e_lineOfSightUnitVector = (_e_position.head<2>() - routerPosition) / (_e_position.head<2>() - routerPosition).norm(); -// e_H.block<1, 2>(static_cast(ix), 0) = -e_lineOfSightUnitVector; -// ddist(static_cast(ix)) = distMeas.at(ix) - distEst; - -// lsq = solveLinearLeastSquaresUncertainties(e_H, ddist); -// LOG_DATA("{}: [{}] dx (lsq) {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1)); -// LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); - -// _e_position.head<2>() += lsq.solution.head<2>(); -// ix++; -// } -// bool solInaccurate = lsq.solution.norm() > 1e-4; - -// if (!solInaccurate) -// { -// break; -// } -// } + node->_e_position.head<2>() += dx; + bool solInaccurate = dx.norm() > 1e-3; -// auto pos = std::make_shared(); -// pos->setPosition_e(_e_position); -// LOG_DEBUG("{}: Position: {}", nameId(), _e_position.transpose()); -// invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); -// } + if (!solInaccurate) + { + break; + } + } + node->_devices.clear(); + auto pos = std::make_shared(); + pos->setPosition_e(node->_e_position); + LOG_DEBUG("{}: Position: {}", node->nameId(), node->_e_position.transpose()); + node->invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); +} -// void NAV::WiFiPositioning::lsqSolution3d() -// { +void NAV::WiFiPositioning::lsqSolution3d(void* userData) +{ + auto* node = static_cast(userData); + if (node->_devices.size() < 4) + { + LOG_DEBUG("{}: Received less than 4 observations. Can't compute position", node->nameId()); + return; + } + else + { + LOG_DEBUG("{}: Received {} observations", node->nameId(), node->_devices.size()); + } + node->_e_position = { 1, 1, 1 }; + + Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(node->_devices.size()), static_cast(3)); + Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(node->_devices.size())); + size_t numMeasurements = node->_devices.size(); + LeastSquaresResult + lsq; + for (size_t o = 0; o < 10; o++) + { + LOG_DATA("{}: Iteration {}", nameId(), o); + for (size_t i = 0; i < numMeasurements; i++) + { + double distEst = (node->_devices.at(i).position - node->_e_position).norm(); + ddist(static_cast(i)) = node->_devices.at(i).distance - distEst; + Eigen::Vector3d e_lineOfSightUnitVector = Eigen::Vector3d::Zero(); + if ((node->_e_position - node->_devices.at(i).position).norm() != 0) // Check if it is the same position + { + e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(node->_e_position, node->_devices.at(i).position); + } + e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; + } + // std::cout << "Matrix:\n" + // << e_H << "\n"; + Eigen::Vector3d dx = (e_H.transpose() * e_H).inverse() * e_H.transpose() * ddist; + // LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); + // LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); -// LOG_DEBUG("{}: Received {} observations", nameId(), _devices.size()); - -// if (_devices.size() < 4) -// { -// LOG_WARN("{}: Received less than 4 observations. Can't compute position", nameId()); -// return; -// } -// else -// { -// LOG_DEBUG("{}: Received {} observations", nameId(), espressifObs->data.size()); -// } -// _e_position = Eigen::Vector3d::Zero(); - -// Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(_numOfMeasurements), static_cast(3)); -// Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_numOfMeasurements)); -// LeastSquaresResult lsq; -// for (size_t o = 0; o < 10; o++) -// { -// LOG_DATA("{}: Iteration {}", nameId(), o); -// size_t ix = 0; -// for (size_t i = 0; i < _numOfMeasurements; i++) -// { -// double distEst = (_devices.at(i).position - _e_position).norm(); -// Eigen::Vector3d e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(_e_position, _devices.at(i).position); -// e_H.block<1, 3>(static_cast(ix), 0) = -e_lineOfSightUnitVector; -// ddist(static_cast(ix)) = distMeas.at(ix) - distEst; - -// lsq = solveLinearLeastSquaresUncertainties(e_H, ddist); -// LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); -// LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); - -// _e_position += lsq.solution.head<3>(); -// ix++; -// } -// bool solInaccurate = lsq.solution.norm() > 1e-4; - -// if (!solInaccurate) -// { -// break; -// } -// } + node->_e_position += dx; + bool solInaccurate = dx.norm() > 1e-3; -// auto pos = std::make_shared(); -// pos->setPosition_e(_e_position); -// LOG_DEBUG("{}: Position: {}", nameId(), _e_position.transpose()); -// invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); -// } + if (!solInaccurate) + { + break; + } + } + node->_devices.clear(); + auto pos = std::make_shared(); + pos->setPosition_e(node->_e_position); + LOG_DEBUG("{}: Position: {}", node->nameId(), node->_e_position.transpose()); + node->invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); +} // void NAV::WiFiPositioning::kfSolution() // { diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp index 523bfcaaf..82255cec2 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp @@ -7,18 +7,18 @@ // file, You can obtain one at https://mozilla.org/MPL/2.0/. /// @file WiFiPositioning.hpp -/// @brief Single Point Positioning (SPP) / Code Phase Positioning +/// @brief Single Point Positioning (SPP) / Code Phase Positioning // TODO /// @author T. Topp (topp@ins.uni-stuttgart.de) /// @date 2022-04-29 #pragma once #include "util/Eigen.hpp" +#include "util/CallbackTimer.hpp" #include #include "internal/Node/Node.hpp" -#include "NodeData/WiFi/EspressifObs.hpp" -#include "NodeData/WiFi/ArubaObs.hpp" +#include "NodeData/WiFi/WiFiObs.hpp" #include "NodeData/State/Pos.hpp" namespace NAV @@ -61,9 +61,8 @@ class WiFiPositioning : public Node void restore(const json& j) override; private: - constexpr static size_t INPUT_PORT_INDEX_ESPRESSIF_OBS = 0; ///< @brief EspressifObs - constexpr static size_t INPUT_PORT_INDEX_ARUBA_OBS = 0; ///< @brief ArubaObs - constexpr static size_t OUTPUT_PORT_INDEX_POS = 0; ///< @brief Pos + constexpr static size_t INPUT_PORT_INDEX_WIFI_OBS = 0; ///< @brief WiFiObs + constexpr static size_t OUTPUT_PORT_INDEX_POS = 0; ///< @brief Pos // --------------------------------------------------------------- Gui ----------------------------------------------------------------- @@ -73,6 +72,12 @@ class WiFiPositioning : public Node /// @brief Deinitialize the node void deinitialize() override; + /// Amount of wifi input pins + size_t _nWifiInputPins = 2; + + /// @brief Adds/Deletes Input Pins depending on the variable _nNavInfoPins + void updateNumberOfInputPins(); + /// @brief Available Frames enum class Frame : int { @@ -112,21 +117,19 @@ class WiFiPositioning : public Node }; std::vector _devices; - /// @brief Receive Function for the Espressif Observations + /// @brief Receive Function for the WiFi Observations /// @param[in] queue Queue with all the received data messages /// @param[in] pinIdx Index of the pin the data is received on - void recvEspressifObs(InputPin::NodeDataQueue& queue, size_t pinIdx); + void recvWiFiObs(InputPin::NodeDataQueue& queue, size_t pinIdx); - /// @brief Receive Function for the Aruba Observations - /// @param[in] queue Queue with all the received data messages - /// @param[in] pinIdx Index of the pin the data is received on - void recvArubaObs(InputPin::NodeDataQueue& queue, size_t pinIdx); + /// Timer object to handle async data requests + CallbackTimer _timer; /// @brief Calculate the position - void lsqSolution2d(); + static void lsqSolution2d(void* userData); /// @brief Calculate the position - void lsqSolution3d(); + static void lsqSolution3d(void* userData); }; } // namespace NAV \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp index 03c095dc9..716024dea 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -18,7 +18,7 @@ namespace nm = NAV::NodeManager; #include "internal/FlowManager.hpp" -#include "NodeData/WiFi/ArubaObs.hpp" +#include "NodeData/WiFi/WiFiObs.hpp" /// Speed of light in air [m/s] constexpr double cAir = 299702547.0; @@ -34,12 +34,13 @@ NAV::ArubaSensor::ArubaSensor() _sshHost = "192.168.178.45"; _sshUser = "admin"; + _sshPassword = "admin1"; _sshHostkeys = "ssh-rsa"; _sshKeyExchange = "ecdh-sha2-nistp256"; _sshPublickeyAcceptedTypes = "ssh-rsa"; _outputInterval = 3000; - nm::CreateOutputPin(this, "ArubaObs", Pin::Type::Flow, { NAV::ArubaObs::type() }); + nm::CreateOutputPin(this, "WiFiObs", Pin::Type::Flow, { NAV::WiFiObs::type() }); } NAV::ArubaSensor::~ArubaSensor() @@ -76,6 +77,12 @@ void NAV::ArubaSensor::guiConfig() flow::ApplyChanges(); doDeinitialize(); } + if (ImGui::InputText("SSH Password", &_sshPassword)) + { + LOG_DEBUG("{}: ssh password changed to {}", nameId(), _sshPassword); + flow::ApplyChanges(); + doDeinitialize(); + } if (ImGui::InputText("SSH Host Keys", &_sshHostkeys)) { LOG_DEBUG("{}: ssh host keys changed to {}", nameId(), _sshHostkeys); @@ -96,7 +103,7 @@ void NAV::ArubaSensor::guiConfig() } ImGui::Spacing(); // Add spacing here ImGui::Separator(); // Add a horizontal line - if (ImGui::InputInt("Output Interval in ms", &_outputInterval)) + if (ImGui::InputInt("Output interval [ms]", &_outputInterval)) { LOG_DEBUG("{}: output interval changed to {}", nameId(), _outputInterval); flow::ApplyChanges(); @@ -111,6 +118,7 @@ void NAV::ArubaSensor::guiConfig() j["sshHost"] = _sshHost; j["sshUser"] = _sshUser; + j["sshPassword"] = _sshPassword; j["sshHostkeys"] = _sshHostkeys; j["sshKeyExchange"] = _sshKeyExchange; j["sshPublickeyAcceptedTypes"] = _sshPublickeyAcceptedTypes; @@ -131,6 +139,10 @@ void NAV::ArubaSensor::restore(json const& j) { j.at("sshUser").get_to(_sshUser); } + if (j.contains("sshPassword")) + { + j.at("sshPassword").get_to(_sshPassword); + } if (j.contains("sshHostkeys")) { j.at("sshHostkeys").get_to(_sshHostkeys); @@ -183,7 +195,7 @@ bool NAV::ArubaSensor::initialize() LOG_DEBUG("{}: Successfully connected to the router", nameId()); // authenticate - if (ssh_userauth_password(_session, NULL, "CNNZKYJ51F") != SSH_AUTH_SUCCESS) + if (ssh_userauth_password(_session, NULL, _sshPassword.c_str()) != SSH_AUTH_SUCCESS) { LOG_INFO("{}: Authentication failed", nameId()); ssh_disconnect(_session); @@ -267,7 +279,7 @@ void NAV::ArubaSensor::deinitialize() void NAV::ArubaSensor::readSensorDataThread(void* userData) { auto* node = static_cast(userData); - auto obs = std::make_shared(); + auto obs = std::make_shared(); obs->insTime = util::time::GetCurrentInsTime(); @@ -318,7 +330,7 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) lineStream >> timeStamp1; lineStream >> timeStamp2; - double measuredDistance = static_cast(rtt) * 1e-9 * cAir; + double measuredDistance = static_cast(rtt) * 1e-9 / 2 * cAir; if (std::regex_match(macAddress, macRegex)) // Check if the MAC address is valid { InsTime_YMDHMS yearMonthDayHMS(std::stoi(timeStamp1.substr(0, 4)), std::stoi(timeStamp1.substr(5, 2)), std::stoi(timeStamp1.substr(8, 2)), std::stoi(timeStamp2.substr(0, 2)), std::stoi(timeStamp2.substr(3, 2)), std::stoi(timeStamp2.substr(6, 2))); @@ -328,5 +340,5 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) } } - node->invokeCallbacks(OUTPUT_PORT_INDEX_Aruba_OBS, obs); + node->invokeCallbacks(OUTPUT_PORT_INDEX_WIFI_OBS, obs); } \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp index 2e219c39b..c9a9ac2ac 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp @@ -61,7 +61,7 @@ class ArubaSensor : public Node bool resetNode() override; private: - constexpr static size_t OUTPUT_PORT_INDEX_Aruba_OBS = 0; ///< @brief Flow (ArubaObs) + constexpr static size_t OUTPUT_PORT_INDEX_WIFI_OBS = 0; ///< @brief Flow (WiFiObs) /// @brief Initialize the node bool initialize() override; @@ -82,6 +82,7 @@ class ArubaSensor : public Node /// Ssh options std::string _sshHost; std::string _sshUser; + std::string _sshPassword; std::string _sshHostkeys; std::string _sshKeyExchange; std::string _sshPublickeyAcceptedTypes; diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp index bfc9ed6b5..6ea549208 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -151,5 +151,5 @@ void NAV::EspressifSensor::binaryAsyncMessageReceived(void* userData, uart::prot auto obs = std::make_shared(p); obs->insTime = util::time::GetCurrentInsTime(); - ubSensor->invokeCallbacks(OUTPUT_PORT_INDEX_Espressif_OBS, obs); + ubSensor->invokeCallbacks(OUTPUT_PORT_INDEX_WIFI_OBS, obs); } diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp index 63b964b47..a01563159 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp @@ -60,7 +60,7 @@ class EspressifSensor : public Node, public UartSensor bool resetNode() override; private: - constexpr static size_t OUTPUT_PORT_INDEX_Espressif_OBS = 0; ///< @brief Flow (EspressifObs) + constexpr static size_t OUTPUT_PORT_INDEX_WIFI_OBS = 0; ///< @brief Flow (EspressifObs) /// @brief Initialize the node bool initialize() override; diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 4598bc5da..47a6a8a4c 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -15,42 +15,105 @@ #include "util/Time/TimeBase.hpp" -// void NAV::vendor::espressif::decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet) -// { -// obs->insTime = util::time::GetCurrentInsTime(); -// if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) -// { -// obs->payloadLength = packet.extractUint16(); // TODO remove -// size_t measurementLength = 14; // TODO irgendwo anders definieren -// size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U -// for (size_t i = 0; i < numOfMeasurement; i++) -// { -// std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); -// std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase -// double measuredDistance = packet.extractDouble(); -// obs->data.push_back({ macAddress, measuredDistance }); -// // Log the measurement details -// LOG_DATA("EspressifObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); -// } -// } -// else -// { -// LOG_DEBUG("Received non-binary packet. Ignoring."); -// } -// } +/// Speed of light in air [m/s] +constexpr double cAir = 299702547.0; -void NAV::vendor::espressif::decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet) +void NAV::vendor::espressif::decryptSingleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) { obs->insTime = util::time::GetCurrentInsTime(); if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) { obs->payloadLength = packet.extractUint16(); // TODO remove + // Mac address std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase - double measuredDistance = packet.extractDouble(); + // Distance + int rtt = packet.extractInt32(); + double measuredDistance = static_cast(rtt) * cAir * 1e-9 / 2; + // Time of measurement + InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); + InsTime timeOfMeasurement(yearMonthDayHMS, UTC); + [[maybe_unused]] int ms = packet.extractInt32(); + // Add the measurement to the WiFiObs + obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); + // Log the measurement details + LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); + } + else + { + LOG_DEBUG("Received non-binary packet. Ignoring."); + } +} + +void NAV::vendor::espressif::decryptMultipleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) +{ + obs->insTime = util::time::GetCurrentInsTime(); + if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) + { + obs->payloadLength = packet.extractUint16(); // TODO remove + size_t measurementLength = 14; // TODO irgendwo anders definieren + size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U + for (size_t i = 0; i < numOfMeasurement; i++) + { + std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); + std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase + double measuredDistance = packet.extractDouble(); + obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); + // Log the measurement details + LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); + } + } + else + { + LOG_DEBUG("Received non-binary packet. Ignoring."); + } +} + +void NAV::vendor::espressif::decryptSingleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) +{ + obs->insTime = util::time::GetCurrentInsTime(); + obs->insTime = util::time::GetCurrentInsTime(); + if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) + { + obs->payloadLength = packet.extractUint16(); // TODO remove + // Mac address + std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); + std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase + // Distance + int rtt = packet.extractInt32(); + double measuredDistance = static_cast(rtt) * cAir * 1e-9 / 2; + // Time of measurement + InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); + InsTime timeOfMeasurement(yearMonthDayHMS, UTC); + [[maybe_unused]] int ms = packet.extractInt32(); + // Add the measurement to the WiFiObs obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); // Log the measurement details - LOG_DATA("EspressifObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); + LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); + } + else + { + LOG_DEBUG("Received non-binary packet. Ignoring."); + } +} + +void NAV::vendor::espressif::decryptMultipleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) +{ + obs->insTime = util::time::GetCurrentInsTime(); + if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) + { + obs->payloadLength = packet.extractUint16(); // TODO remove + size_t measurementLength = 14; // TODO irgendwo anders definieren + size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U + for (size_t i = 0; i < numOfMeasurement; i++) + { + std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); + std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase + double measuredDistance = packet.extractDouble(); + obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); + // Log the measurement details + LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); + } } else { diff --git a/src/util/Vendor/Espressif/EspressifUtilities.hpp b/src/util/Vendor/Espressif/EspressifUtilities.hpp index 68f572537..58af6dc83 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.hpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.hpp @@ -19,18 +19,33 @@ #include "uart/protocol/packet.hpp" -#include "NodeData/WiFi/EspressifObs.hpp" +#include "NodeData/WiFi/WiFiObs.hpp" namespace NAV::vendor::espressif { +/// @brief Decrypts the provided Espressif observation with device time +/// @param[in] obs Espressif Observation to decrypt +/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) +void decryptSingleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); + +/// @brief Decrypts the provided multiple Espressif observation with device time +/// @param[in] obs Espressif Observation to decrypt +/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) +void decryptMultipleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); + /// @brief Decrypts the provided Espressif observation /// @param[in] obs Espressif Observation to decrypt /// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) -void decryptEspressifObs(const std::shared_ptr& obs, uart::protocol::Packet& packet); +void decryptSingleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); + +/// @brief Decrypts the provided multiple Espressif observation +/// @param[in] obs Espressif Observation to decrypt +/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) +void decryptMultipleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); /// @brief Calculates the two UBX checksums for the provided data vector /// @param[in] data Data Vector for which the checksum should be calculated /// @return The checksums CK_A and CK_B -std::pair checksumUBX(const std::vector& data); +std::pair checksumUBX(const std::vector& data); // TODO Name } // namespace NAV::vendor::espressif \ No newline at end of file From 70c37c4b7864343b5e8791c22a454ff93fea2445 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Wed, 13 Mar 2024 17:08:45 +0100 Subject: [PATCH 04/27] Add logging and plotting --- .vscode/tasks.json | 413 +++++-------- src/NodeData/WiFi/WiFiObs.hpp | 4 +- src/NodeData/WiFi/WiFiPositioningSolution.hpp | 122 ++++ src/NodeRegistry.cpp | 4 + .../Converter/GNSS/UartPacketConverter.cpp | 47 +- .../Converter/GNSS/UartPacketConverter.hpp | 19 +- .../DataProcessor/WiFi/WiFiPositioning.cpp | 579 +++++++++++++----- .../DataProcessor/WiFi/WiFiPositioning.hpp | 120 +++- .../DataProvider/WiFi/Sensors/ArubaSensor.hpp | 4 +- .../WiFi/Sensors/EspressifFile.cpp | 246 ++++++++ .../WiFi/Sensors/EspressifFile.hpp | 88 +++ .../WiFi/Sensors/EspressifSensor.cpp | 5 +- .../WiFi/Sensors/EspressifSensor.hpp | 3 +- src/Nodes/Plotting/Plot.cpp | 125 +++- src/Nodes/Plotting/Plot.hpp | 6 + .../Vendor/Espressif/EspressifUartSensor.hpp | 3 +- .../Vendor/Espressif/EspressifUtilities.cpp | 85 +-- .../Vendor/Espressif/EspressifUtilities.hpp | 20 +- 18 files changed, 1309 insertions(+), 584 deletions(-) create mode 100644 src/NodeData/WiFi/WiFiPositioningSolution.hpp create mode 100644 src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp create mode 100644 src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 355f0070b..fbeb92568 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -1,135 +1,116 @@ { "version": "2.0.0", "tasks": [ - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ CLEAN Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // CLEAN: Remove Build Files + { "label": "CLEAN: Remove Build Files", "type": "shell", "group": "build", "command": "rm", "args": [ "-rf", - "build", + "build" ], "windows": { "command": "Remove-Item", "args": [ "-Recurse", - "build", + "build" ] }, "presentation": { "reveal": "never", "echo": false, - "showReuseMessage": false, + "showReuseMessage": false }, - "problemMatcher": [], + "problemMatcher": [] }, - { // CLEAN: Tests + { "label": "CLEAN: Tests", "type": "shell", "group": "build", "command": "rm", "args": [ "-rf", - "test/logs/[^\\.]*", + "test/logs/[^\\.]*" ], "windows": { "command": "Remove-Item", "args": [ "-Recurse", - "build", + "build" ] }, "presentation": { "reveal": "never", "echo": false, - "showReuseMessage": false, + "showReuseMessage": false }, - "problemMatcher": [], + "problemMatcher": [] }, - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ Conan Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // Conan: Detect + { "label": "Conan: Detect", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan profile detect --force", + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan profile detect --force" ], "presentation": { - "clear": true, + "clear": true }, - "problemMatcher": [], + "problemMatcher": [] }, - { // Conan + { "label": "Conan", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan install . --build=missing -s build_type=${input:buildType} -s compiler.cppstd=20", - ], - // "command": "conan", - // "args": [ - // "install", - // ".", - // "--build=missing", - // "-s", - // "build_type=${input:buildType}", - // "-s", - // "compiler.cppstd=20", - // ], + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan install . --build=missing -s build_type=${input:buildType} -s compiler.cppstd=20" + ], "dependsOn": [ - "Conan: Detect", + "Conan: Detect" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // Conan: Debug + { "label": "Conan: Debug", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan install . --build=missing -s build_type=Debug -s compiler.cppstd=20", + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan install . --build=missing -s build_type=Debug -s compiler.cppstd=20" ], "dependsOn": [ - "Conan: Detect", + "Conan: Detect" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // Conan: Release + { "label": "Conan: Release", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan install . --build=missing -s build_type=Release -s compiler.cppstd=20", + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && conan install . --build=missing -s build_type=Release -s compiler.cppstd=20" ], "dependsOn": [ - "Conan: Detect", + "Conan: Detect" ], - "problemMatcher": [], + "problemMatcher": [] }, - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ CMAKE Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // CMAKE: Main + { "label": "CMAKE: Main", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/${input:buildType}-${input:compiler} -S. -DCMAKE_BUILD_TYPE=${input:buildType} -DCMAKE_TOOLCHAIN_FILE=build/${input:buildType}/generators/conan_toolchain.cmake -DENABLE_MAIN=ON -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=OFF -DENABLE_CLANG_TIDY=$(echo \"${input:clangTidy}\" | sed \"s/.*: //\") -DENABLE_CPPCHECK=$(echo \"${input:cppcheck}\" | sed \"s/.*: //\") -DLOG_LEVEL=$(echo \"${input:logLevel}\" | sed \"s/.*: //\") -DUSE_ALTERNATE_LINKER=${input:linker}", + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/${input:buildType}-${input:compiler} -S. -DCMAKE_BUILD_TYPE=${input:buildType} -DCMAKE_TOOLCHAIN_FILE=build/${input:buildType}/generators/conan_toolchain.cmake -DENABLE_MAIN=ON -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=OFF -DENABLE_CLANG_TIDY=$(echo \"${input:clangTidy}\" | sed \"s/.*: //\") -DENABLE_CPPCHECK=$(echo \"${input:cppcheck}\" | sed \"s/.*: //\") -DLOG_LEVEL=$(echo \"${input:logLevel}\" | sed \"s/.*: //\") -DUSE_ALTERNATE_LINKER=${input:linker}" ], "windows": { "command": "cmake", @@ -146,35 +127,29 @@ "-DENABLE_CPPCHECK=OFF", "-DENABLE_INCLUDE_WHAT_YOU_USE=OFF", "-DWARNINGS_AS_ERRORS=OFF", - "-DUSE_ALTERNATE_LINKER=${input:linker}", + "-DUSE_ALTERNATE_LINKER=${input:linker}" ], "options": { "env": { "VSLANG": "1033", - "chcp": "1252", + "chcp": "1252" } - }, + } }, "dependsOn": [ - "Conan", + "Conan" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // CMAKE: Testing + { "label": "CMAKE: Testing", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/${input:buildType}-${input:compiler} -S. -DCMAKE_BUILD_TYPE=${input:buildType} -DCMAKE_TOOLCHAIN_FILE=build/${input:buildType}/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=ON -DENABLE_DOXYGEN=OFF -DENABLE_CLANG_TIDY=$(echo \"${input:clangTidy}\" | sed \"s/.*: //\") -DENABLE_CPPCHECK=$(echo \"${input:cppcheck}\" | sed \"s/.*: //\") -DLOG_LEVEL=$(echo \"${input:logLevel}\" | sed \"s/.*: //\") -DUSE_ALTERNATE_LINKER=${input:linker}", - ], - // "options": { - // "env": { - // "CC": "/usr/bin/clang", - // "CXX": "/usr/bin/clang++", - // } - // }, + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/${input:buildType}-${input:compiler} -S. -DCMAKE_BUILD_TYPE=${input:buildType} -DCMAKE_TOOLCHAIN_FILE=build/${input:buildType}/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=ON -DENABLE_DOXYGEN=OFF -DENABLE_CLANG_TIDY=$(echo \"${input:clangTidy}\" | sed \"s/.*: //\") -DENABLE_CPPCHECK=$(echo \"${input:cppcheck}\" | sed \"s/.*: //\") -DLOG_LEVEL=$(echo \"${input:logLevel}\" | sed \"s/.*: //\") -DUSE_ALTERNATE_LINKER=${input:linker}" + ], "windows": { "command": "cmake", "args": [ @@ -189,21 +164,21 @@ "-DENABLE_CLANG_TIDY=OFF", "-DENABLE_CPPCHECK=OFF", "-DENABLE_INCLUDE_WHAT_YOU_USE=OFF", - "-DWARNINGS_AS_ERRORS=OFF", + "-DWARNINGS_AS_ERRORS=OFF" ], "options": { "env": { "VSLANG": "1033", - "chcp": "1252", + "chcp": "1252" } - }, + } }, "dependsOn": [ - "Conan", + "Conan" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // CMAKE: Coverage + { "label": "CMAKE: Coverage", "type": "shell", "group": "build", @@ -221,51 +196,48 @@ "-DENABLE_CLANG_TIDY=OFF", "-DENABLE_CPPCHECK=OFF", "-DENABLE_INCLUDE_WHAT_YOU_USE=OFF", - "-DUSE_ALTERNATE_LINKER=${input:linker}", + "-DUSE_ALTERNATE_LINKER=${input:linker}" ], "options": { "env": { "CC": "/usr/bin/gcc", - "CXX": "/usr/bin/g++", + "CXX": "/usr/bin/g++" } }, "dependsOn": [ - "Conan: Debug", + "Conan: Debug" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // CMAKE: Documentation + { "label": "CMAKE: Documentation", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/Release-doc -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=ON -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=OFF -DENABLE_INCLUDE_WHAT_YOU_USE=OFF -DDOC_CHECK_CODE_DOCUMENTATION=NO -DUSE_ALTERNATE_LINKER=${input:linker}", + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/Release-doc -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=ON -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=OFF -DENABLE_INCLUDE_WHAT_YOU_USE=OFF -DDOC_CHECK_CODE_DOCUMENTATION=NO -DUSE_ALTERNATE_LINKER=${input:linker}" ], "dependsOn": [ - "Conan: Release", + "Conan: Release" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // CMAKE: Documentation Check + { "label": "CMAKE: Documentation Check", "type": "shell", "group": "build", "command": "bash", "args": [ "-c", - "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/Release-doc -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=ON -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=OFF -DENABLE_INCLUDE_WHAT_YOU_USE=OFF -DDOC_CHECK_CODE_DOCUMENTATION=YES -DUSE_ALTERNATE_LINKER=${input:linker}", + "export CC=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo gcc || echo clang) && export CXX=$([[ \"${input:compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/Release-doc -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=ON -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=OFF -DENABLE_INCLUDE_WHAT_YOU_USE=OFF -DDOC_CHECK_CODE_DOCUMENTATION=YES -DUSE_ALTERNATE_LINKER=${input:linker}" ], "dependsOn": [ - "Conan: Release", + "Conan: Release" ], - "problemMatcher": [], + "problemMatcher": [] }, - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ CMAKE Configure Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // CMAKE: Configure + { "label": "CMAKE: Configure", "type": "shell", "group": "build", @@ -273,26 +245,23 @@ "args": [ "-Bbuild/${input:buildType}-${input:compiler}", "-S.", - "-DCMAKE_BUILD_TYPE=${input:buildType}", + "-DCMAKE_BUILD_TYPE=${input:buildType}" ], "options": { "env": { "CC": "/usr/bin/clang", - "CXX": "/usr/bin/clang++", + "CXX": "/usr/bin/clang++" } }, "presentation": { "focus": true }, "dependsOn": [ - "CMAKE: Main", + "CMAKE: Main" ], - "problemMatcher": [], + "problemMatcher": [] }, - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ Build Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // MAIN: Build project + { "label": "MAIN: Build project", "type": "shell", "group": "build", @@ -300,15 +269,15 @@ "linux": { "args": [ "-c", - "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(nproc)", - ], + "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(nproc)" + ] }, "osx": { "command": "bash", "args": [ "-c", - "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(sysctl -n hw.physicalcpu)", - ], + "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(sysctl -n hw.physicalcpu)" + ] }, "windows": { "command": "cmake", @@ -317,21 +286,21 @@ "build/${input:buildType}-msvc", "--config", "${input:buildType}", - "--parallel8", + "--parallel8" ], "options": { "env": { "VSLANG": "1033", - "chcp": "1252", + "chcp": "1252" } - }, + } }, "dependsOn": [ - "CMAKE: Main", + "CMAKE: Main" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // TEST: Build project + { "label": "TEST: Build project", "type": "shell", "group": "build", @@ -339,15 +308,15 @@ "command": "bash", "args": [ "-c", - "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(nproc)", - ], + "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(nproc)" + ] }, "osx": { "command": "bash", "args": [ "-c", - "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(sysctl -n hw.physicalcpu)", - ], + "cmake --build build/${input:buildType}-${input:compiler} --config ${input:buildType} --parallel$(sysctl -n hw.physicalcpu)" + ] }, "windows": { "command": "cmake", @@ -356,21 +325,21 @@ "build/${input:buildType}-msvc", "--config", "${input:buildType}", - "--parallel8", + "--parallel8" ], "options": { "env": { "VSLANG": "1033", - "chcp": "1252", + "chcp": "1252" } - }, + } }, "dependsOn": [ - "CMAKE: Testing", + "CMAKE: Testing" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Build Coverage Report + { "label": "COVERAGE: Build Coverage Report", "type": "shell", "group": "build", @@ -378,22 +347,22 @@ "command": "bash", "args": [ "-c", - "cmake --build build/Debug-cov --config ${input:buildType} --parallel$(nproc)", - ], + "cmake --build build/Debug-cov --config ${input:buildType} --parallel$(nproc)" + ] }, "osx": { "command": "bash", "args": [ "-c", - "cmake --build build/Debug-cov --config ${input:buildType} --parallel$(sysctl -n hw.physicalcpu)", - ], + "cmake --build build/Debug-cov --config ${input:buildType} --parallel$(sysctl -n hw.physicalcpu)" + ] }, "dependsOn": [ - "CMAKE: Coverage", + "CMAKE: Coverage" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // DOXYGEN: Build Documentation + { "label": "DOXYGEN: Build Documentation", "type": "shell", "group": "build", @@ -402,14 +371,14 @@ "--build", "build/Release-doc", "--target", - "doc", + "doc" ], "dependsOn": [ - "CMAKE: Documentation", + "CMAKE: Documentation" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // DOXYGEN: Check for errors + { "label": "DOXYGEN: Check for errors", "type": "shell", "group": "test", @@ -418,17 +387,14 @@ "--build", "build/Release-doc", "--target", - "doc", + "doc" ], "dependsOn": [ - "CMAKE: Documentation Check", + "CMAKE: Documentation Check" ], - "problemMatcher": [], + "problemMatcher": [] }, - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ Run Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // MAIN: Build & run project + { "label": "MAIN: Build & run project", "type": "shell", "group": { @@ -439,32 +405,18 @@ "args": [ "--config=config.ini", "--load=\"flow/Default.flow\"", - // "--load=\"test/flow/Nodes/DataProcessor/GNSS/SinglePointPositioning.flow\"", - // "--sigterm", - // "--duration=60", - // "--nogui", - // "--noinit", - // "--input-path=data", - // "--output-path=logs", - // "--rotate-output", - // "--implot-config=\"config/implot.json\"", - "--console-log-level=trace", // trace/debug/info/warning/error/critical/off - "--file-log-level=trace", // trace/debug/info/warning/error/critical/off - // "--log-filter=SinglePointPositioning.cpp", - // ================ To configure tests ================ - // "--flow-path=test/flow", - // "--input-path=test/data", - // "--output-path=test/logs", + "--console-log-level=trace", + "--file-log-level=trace" ], "options": { - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" }, "dependsOn": [ - "MAIN: Build project", + "MAIN: Build project" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // TEST: Build & run + { "label": "TEST: Build & run", "type": "shell", "group": { @@ -473,26 +425,22 @@ }, "command": "ctest", "args": [ - // "-j4", - "--output-on-failure", - // "--tests-regex", - // "'PVAError|PosVelAtt'", - // "--verbose", + "--output-on-failure" ], "options": { - "cwd": "${workspaceFolder}/build/${input:buildType}-${input:compiler}", + "cwd": "${workspaceFolder}/build/${input:buildType}-${input:compiler}" }, "windows": { "options": { - "cwd": "${workspaceFolder}/build/${input:buildType}-msvc", - }, + "cwd": "${workspaceFolder}/build/${input:buildType}-msvc" + } }, "dependsOn": [ - "TEST: Build project", + "TEST: Build project" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // TEST: Build & run w/o flow tests + { "label": "TEST: Build & run w/o flow tests", "type": "shell", "group": "test", @@ -501,38 +449,34 @@ "-j4", "--output-on-failure", "--exclude-regex", - "'\\[flow\\]'", - // "--tests-regex", - // "'ScrollingBuffer'" - // "--verbose", + "'\\[flow\\]'" ], "options": { - "cwd": "${workspaceFolder}/build/${input:buildType}-${input:compiler}", + "cwd": "${workspaceFolder}/build/${input:buildType}-${input:compiler}" }, "dependsOn": [ - "TEST: Build project", + "TEST: Build project" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Run Coverage Tests + { "label": "COVERAGE: Run Coverage Tests", "type": "shell", "group": "test", "command": "ctest", "args": [ "-j4", - "--output-on-failure", - // "--verbose", + "--output-on-failure" ], "options": { - "cwd": "${workspaceFolder}/build/Debug-cov", + "cwd": "${workspaceFolder}/build/Debug-cov" }, "dependsOn": [ - "COVERAGE: Build Coverage Report", + "COVERAGE: Build Coverage Report" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Run Coverage Tests w/o flow tests + { "label": "COVERAGE: Run Coverage Tests w/o flow tests", "type": "shell", "group": "test", @@ -542,87 +486,83 @@ "--output-on-failure", "--exclude-regex", "'\\[flow\\]'" - // "--verbose", ], "options": { - "cwd": "${workspaceFolder}/build/Debug-cov", + "cwd": "${workspaceFolder}/build/Debug-cov" }, "dependsOn": [ - "COVERAGE: Build Coverage Report", + "COVERAGE: Build Coverage Report" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Create + { "label": "COVERAGE: Create", "type": "shell", "group": "test", "command": "bash", "args": [ "-c", - "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg", + "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg" ], "dependsOn": [ - "COVERAGE: Run Coverage Tests", + "COVERAGE: Run Coverage Tests" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Create w/o flow tests + { "label": "COVERAGE: Create w/o flow tests", "type": "shell", "group": "test", "command": "bash", "args": [ "-c", - "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg", + "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg" ], "dependsOn": [ - "COVERAGE: Run Coverage Tests w/o flow tests", + "COVERAGE: Run Coverage Tests w/o flow tests" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Create & Show + { "label": "COVERAGE: Create & Show", "type": "shell", "group": "build", "command": "xdg-open", "args": [ - "build/coverage/index.html", + "build/coverage/index.html" ], "dependsOn": [ - "COVERAGE: Create", + "COVERAGE: Create" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // COVERAGE: Create & Show w/o flow tests + { "label": "COVERAGE: Create & Show w/o flow tests", "type": "shell", "group": "build", "command": "xdg-open", "args": [ - "build/coverage/index.html", + "build/coverage/index.html" ], "dependsOn": [ - "COVERAGE: Create w/o flow tests", + "COVERAGE: Create w/o flow tests" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // DOXYGEN: Create & Show + { "label": "DOXYGEN: Create & Show", "type": "shell", "group": "build", "command": "xdg-open", "args": [ - "build/doc/html/index.html", + "build/doc/html/index.html" ], "dependsOn": [ - "DOXYGEN: Build Documentation", + "DOXYGEN: Build Documentation" ], - "problemMatcher": [], + "problemMatcher": [] }, - // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ - // ║ Debugging Tasks ║ - // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ - { // Gperftools: Run profiler + { "label": "Gperftools: Run profiler", "type": "shell", "group": "test", @@ -630,88 +570,71 @@ "args": [ "--config=config.ini", "--load=\"flow/RTK-2020-08-05-Icare.flow\"", - // "--load=\"test/flow/Nodes/DataProcessor/GNSS/SinglePointPositioning.flow\"", - // "--sigterm", - // "--duration=60", - // "--nogui", - // "--noinit", - // "--input-path=data", - // "--output-path=logs", - // "--rotate-output", - // "--implot-config=\"config/implot.json\"", - "--console-log-level=trace", // trace/debug/info/warning/error/critical/off - "--file-log-level=trace", // trace/debug/info/warning/error/critical/off - // "--log-filter=SinglePointPositioning.cpp", - // ================ To configure tests ================ - // "--flow-path=test/flow", - // "--input-path=test/data", - // "--output-path=test/logs", + "--console-log-level=trace", + "--file-log-level=trace" ], "options": { - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" }, "linux": { "options": { "env": { "CPUPROFILE": "build/bin/${input:buildType}/instinct.prof", - "CPUPROFILE_FREQUENCY": "500", // Default: 100 How many interrupts/second the cpu-profiler samples. + "CPUPROFILE_FREQUENCY": "500" } } }, "dependsOn": [ - "MAIN: Build project", + "MAIN: Build project" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // Gperftools: Show profiled output + { "label": "Gperftools: Show profiled output", "type": "shell", "group": "test", "command": "pprof", "args": [ - "--gv", // Displays annotated call-graph via 'gv' - // "--callgrind", // Outputs the call information in callgrind format + "--gv", "build/bin/${input:buildType}/instinct", - "build/bin/${input:buildType}/instinct.prof", - // ">", // For --callgrind option - // "build/bin/${input:buildType}/callgrind.out", // For --callgrind option + "build/bin/${input:buildType}/instinct.prof" ], "options": { - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" }, - "problemMatcher": [], + "problemMatcher": [] }, - { // VALGRIND: Run profiler + { "label": "VALGRIND: Run profiler", "type": "shell", "group": "test", "command": "bash", "args": [ "-c", - "valgrind --callgrind-out-file=build/bin/${input:buildType}/callgrind.out --tool=callgrind build/bin/${input:buildType}/instinct -f config.ini --nogui -l flow/Default.flow && kcachegrind build/bin/${input:buildType}/callgrind.out", + "valgrind --callgrind-out-file=build/bin/${input:buildType}/callgrind.out --tool=callgrind build/bin/${input:buildType}/instinct -f config.ini --nogui -l flow/Default.flow && kcachegrind build/bin/${input:buildType}/callgrind.out" ], "options": { - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" }, "dependsOn": [ - "MAIN: Build project", + "MAIN: Build project" ], - "problemMatcher": [], + "problemMatcher": [] }, - { // VALGRIND: Show profiled output + { "label": "VALGRIND: Show profiled output", "type": "shell", "group": "test", "command": "kcachegrind", "args": [ - "build/bin/${input:buildType}/callgrind.out", + "build/bin/${input:buildType}/callgrind.out" ], "options": { - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" }, - "problemMatcher": [], + "problemMatcher": [] }, - { // VALGRIND: Memory Leak Check + { "label": "VALGRIND: Memory Leak Check", "type": "shell", "group": "test", @@ -723,15 +646,15 @@ "config.ini", "--nogui", "-l", - "flow/Default.flow", + "flow/Default.flow" ], "options": { - "cwd": "${workspaceFolder}", + "cwd": "${workspaceFolder}" }, "dependsOn": [ - "MAIN: Build project", + "MAIN: Build project" ], - "problemMatcher": [], + "problemMatcher": [] } ], // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ diff --git a/src/NodeData/WiFi/WiFiObs.hpp b/src/NodeData/WiFi/WiFiObs.hpp index 1cd3113a9..79c13b568 100644 --- a/src/NodeData/WiFi/WiFiObs.hpp +++ b/src/NodeData/WiFi/WiFiObs.hpp @@ -8,8 +8,8 @@ /// @file WiFiObs.hpp /// @brief Espressif Observation Class -/// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date TODO +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @date 2024-01-08 #pragma once diff --git a/src/NodeData/WiFi/WiFiPositioningSolution.hpp b/src/NodeData/WiFi/WiFiPositioningSolution.hpp new file mode 100644 index 000000000..8f76df82a --- /dev/null +++ b/src/NodeData/WiFi/WiFiPositioningSolution.hpp @@ -0,0 +1,122 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file WiFiPositioningSolution.hpp +/// @brief WiFi Positioning Algorithm output +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2024-03-12 + +#pragma once + +#include "NodeData/State/PosVel.hpp" +#include + +namespace NAV +{ +class WiFiPositioningSolution : public PosVel +{ + public: + /// @brief Returns the type of the data class + /// @return The data type + [[nodiscard]] static std::string type() + { + return "WiFiPositioningSolution"; + } + + /// @brief Returns the parent types of the data class + /// @return The parent data types + [[nodiscard]] static std::vector parentTypes() + { + auto parent = PosVel::parentTypes(); + parent.push_back(PosVel::type()); + return parent; + } + + // ------------------------------------------------------------- Getter ---------------------------------------------------------------- + + /// Returns the standard deviation of the position in ECEF frame coordinates in [m] + [[nodiscard]] const Eigen::Vector3d& e_positionStdev() const { return _e_positionStdev; } + + /// Returns the standard deviation of the position in local navigation frame coordinates in [m] + [[nodiscard]] const Eigen::Vector3d& n_positionStdev() const { return _n_positionStdev; } + + /// Returns the standard deviation of the velocity in [m/s], in earth coordinates + [[nodiscard]] const Eigen::Vector3d& e_velocityStdev() const { return _e_velocityStdev; } + + /// Returns the standard deviation of the velocity in [m/s], in navigation coordinates + [[nodiscard]] const Eigen::Vector3d& n_velocityStdev() const { return _n_velocityStdev; } + + /// Returns the Covariance matrix in ECEF frame + [[nodiscard]] const Eigen::MatrixXd& e_CovarianceMatrix() const { return _e_covarianceMatrix; } + + /// Returns the Covariance matrix in local navigation frame + [[nodiscard]] const Eigen::MatrixXd& n_CovarianceMatrix() const { return _n_covarianceMatrix; } + + // ------------------------------------------------------------- Setter ---------------------------------------------------------------- + + /// @brief Set the Position in ECEF coordinates and its standard deviation + /// @param[in] e_position New Position in ECEF coordinates [m] + /// @param[in] e_PositionCovarianceMatrix Standard deviation of Position in ECEF coordinates [m] + void setPositionAndStdDev_e(const Eigen::Vector3d& e_position, const Eigen::Matrix3d& e_PositionCovarianceMatrix) + { + setPosition_e(e_position); + _e_positionStdev = e_PositionCovarianceMatrix.diagonal().cwiseSqrt(); + _n_positionStdev = (n_Quat_e().toRotationMatrix() * e_PositionCovarianceMatrix * n_Quat_e().conjugate().toRotationMatrix()).diagonal().cwiseSqrt(); + } + + /// @brief Set the Velocity in ECEF coordinates and its standard deviation + /// @param[in] e_velocity New Velocity in ECEF coordinates [m/s] + /// @param[in] e_velocityCovarianceMatrix Covariance matrix of Velocity in earth coordinates [m/s] + void setVelocityAndStdDev_e(const Eigen::Vector3d& e_velocity, const Eigen::Matrix3d& e_velocityCovarianceMatrix) + { + setVelocity_e(e_velocity); + _e_velocityStdev = e_velocityCovarianceMatrix.diagonal().cwiseSqrt(); + _n_velocityStdev = (n_Quat_e().toRotationMatrix() * e_velocityCovarianceMatrix * n_Quat_e().conjugate().toRotationMatrix()).diagonal().cwiseSqrt(); + } + + /// @brief Set the Covariance matrix + /// @param[in] P Covariance matrix + void setCovarianceMatrix(const Eigen::MatrixXd& P) + { + _e_covarianceMatrix = P; + n_CovarianceMatrix_e(); + } + + /// @brief Transforms the covariance matrix from ECEF frame to local navigation frame + void n_CovarianceMatrix_e() + { + _n_covarianceMatrix = _e_covarianceMatrix; + + Eigen::Vector3d lla_pos = lla_position(); + Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_pos(0), lla_pos(1)); + _n_covarianceMatrix.block<3, 3>(0, 0) = n_Quat_e.toRotationMatrix() * _e_covarianceMatrix.block<3, 3>(0, 0) * n_Quat_e.conjugate().toRotationMatrix(); // variance of position + if (_e_covarianceMatrix.rows() >= 4 && _e_covarianceMatrix.cols() >= 4) // velocity is also available + { + _n_covarianceMatrix.block<3, 3>(3, 3) = n_Quat_e.toRotationMatrix() * _e_covarianceMatrix.block<3, 3>(3, 3) * n_Quat_e.toRotationMatrix(); // variance of velocity + } + } + + private: + /// Standard deviation of Position in ECEF coordinates [m] + Eigen::Vector3d _e_positionStdev = Eigen::Vector3d::Zero() * std::nan(""); + /// Standard deviation of Position in local navigation frame coordinates [m] + Eigen::Vector3d _n_positionStdev = Eigen::Vector3d::Zero() * std::nan(""); + + /// Standard deviation of Velocity in earth coordinates [m/s] + Eigen::Vector3d _e_velocityStdev = Eigen::Vector3d::Zero() * std::nan(""); + /// Standard deviation of Velocity in navigation coordinates [m/s] + Eigen::Vector3d _n_velocityStdev = Eigen::Vector3d::Zero() * std::nan(""); + + /// Covariance matrix in ECEF coordinates (Position, Velocity) + Eigen::MatrixXd _e_covarianceMatrix; + /// Covariance matrix in local navigation coordinates (Position, Velocity) + Eigen::MatrixXd _n_covarianceMatrix; +}; + +} // namespace NAV diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index c2f65ea2f..a0d680e0e 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -219,6 +219,7 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataProvider/IMU/FileReader/MultiImuFile.hpp" #include "Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp" #include "Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp" +#include "Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp" // Data Simulator #include "Nodes/DataProvider/IMU/Simulators/ImuSimulator.hpp" // Plotting @@ -287,6 +288,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); // Data Simulator registerNodeType(); // Experimental @@ -319,6 +321,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() #include "NodeData/State/PosVel.hpp" #include "NodeData/State/PosVelAtt.hpp" #include "NodeData/WiFi/WiFiObs.hpp" +#include "NodeData/WiFi/WiFiPositioningSolution.hpp" void NAV::NodeRegistry::RegisterNodeDataTypes() { @@ -346,4 +349,5 @@ void NAV::NodeRegistry::RegisterNodeDataTypes() registerNodeDataType(); // WiFi registerNodeDataType(); + registerNodeDataType(); } \ No newline at end of file diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index 35b586abe..afaae76a6 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -95,22 +95,8 @@ void NAV::UartPacketConverter::guiConfig() outputPins.front().recreateLink(*connectedPin); } } - flow::ApplyChanges(); } - if (_outputType == OutputType_WiFiObs) - { - if (ImGui::Combo(fmt::format("Number of measurements##{}", size_t(id)).c_str(), reinterpret_cast(&_numberOfMeasurements), "Single\0Multiple\0\0")) - { - LOG_DEBUG("{}: Number of measurements changed to {}", nameId(), _numberOfMeasurements == NumberOfMeasurements::SINGLE ? "single" : "multiple"); - flow::ApplyChanges(); - } - if (ImGui::Combo(fmt::format("Time system##{}", size_t(id)).c_str(), reinterpret_cast(&_timeSystem), "INSTINCT\0WiFi device\0\0")) - { - LOG_DEBUG("{}: Frame changed to {}", nameId(), _timeSystem == WifiTimeSystem::INSTINCT ? "INSTINCT" : "WiFi device"); - flow::ApplyChanges(); - } - } } [[nodiscard]] json NAV::UartPacketConverter::save() const @@ -120,8 +106,6 @@ void NAV::UartPacketConverter::guiConfig() json j; j["outputType"] = _outputType; - j["numberOfMeasurements"] = _numberOfMeasurements; - j["timeSystem"] = _timeSystem; return j; } @@ -153,14 +137,6 @@ void NAV::UartPacketConverter::restore(json const& j) } } } - if (j.contains("numberOfMeasurements")) - { - j.at("numberOfMeasurements").get_to(_numberOfMeasurements); - } - if (j.contains("timeSystem")) - { - j.at("timeSystem").get_to(_timeSystem); - } } bool NAV::UartPacketConverter::initialize() @@ -187,28 +163,7 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, s { auto obs = std::make_shared(); auto packet = uartPacket->raw; - if (_numberOfMeasurements == NumberOfMeasurements::SINGLE) - { - if (_timeSystem == WifiTimeSystem::INSTINCT) - { - vendor::espressif::decryptSingleWiFiObsInstinctTime(obs, packet); - } - else - { - vendor::espressif::decryptSingleWiFiObsDeviceTime(obs, packet); - } - } - else - { - if (_timeSystem == WifiTimeSystem::INSTINCT) - { - vendor::espressif::decryptMultipleWiFiObsInstinctTime(obs, packet); - } - else - { - vendor::espressif::decryptMultipleWiFiObsDeviceTime(obs, packet); - } - } + vendor::espressif::decryptWiFiObs(obs, packet, nameId()); convertedData = obs; } else /* if (_outputType == OutputType_EmlidObs) */ diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp index aa5fa3ec4..eb5b21544 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp @@ -9,6 +9,7 @@ /// @file UartPacketConverter.hpp /// @brief Decrypts Uart packets /// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) /// @date 2022-06-13 #pragma once @@ -77,24 +78,6 @@ class UartPacketConverter : public Node /// The selected output type in the GUI OutputType _outputType = OutputType_UbloxObs; - /// @brief Number of measurements in one message - enum class NumberOfMeasurements : int - { - SINGLE, ///< Single measurement - MULTIPLE, ///< Multiple measurements - }; - /// Number of measurements in one message - NumberOfMeasurements _numberOfMeasurements = NumberOfMeasurements::SINGLE; - - /// @brief Specifies the time system in which the timestamps of the FTM measurements are. - enum class WifiTimeSystem : int - { - INSTINCT, ///< time system of INSTINCT - WIFIDEVICE, ///< time system of the WiFi device - }; - /// Time system of the FTM measurements - WifiTimeSystem _timeSystem = WifiTimeSystem::INSTINCT; - /// @brief Initialize the node bool initialize() override; diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index b5d15d4bc..f6ff78efd 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -20,13 +20,14 @@ #include "internal/gui/NodeEditorApplication.hpp" #include "internal/gui/widgets/HelpMarker.hpp" #include "internal/gui/widgets/imgui_ex.hpp" +#include "internal/gui/widgets/InputWithUnit.hpp" #include "internal/NodeManager.hpp" namespace nm = NAV::NodeManager; #include "internal/FlowManager.hpp" #include "NodeData/WiFi/WiFiObs.hpp" -#include "NodeData/State/PosVelAtt.hpp" +#include "NodeData/WiFi/WiFiPositioningSolution.hpp" #include "Navigation/GNSS/Functions.hpp" #include "Navigation/Transformations/CoordinateFrames.hpp" @@ -39,11 +40,11 @@ NAV::WiFiPositioning::WiFiPositioning() _hasConfig = true; _guiConfigDefaultWindowSize = { 575, 300 }; - _outputInterval = 3000; updateNumberOfInputPins(); - nm::CreateOutputPin(this, NAV::Pos::type().c_str(), Pin::Type::Flow, { NAV::Pos::type() }); + // updateOutputPin(); + nm::CreateOutputPin(this, NAV::WiFiPositioningSolution::type().c_str(), Pin::Type::Flow, { NAV::WiFiPositioningSolution::type() }); } NAV::WiFiPositioning::~WiFiPositioning() @@ -69,6 +70,8 @@ std::string NAV::WiFiPositioning::category() void NAV::WiFiPositioning::guiConfig() { float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio(); + float configWidth = 380.0F * gui::NodeEditorApplication::windowFontRatio(); + float unitWidth = 150.0F * gui::NodeEditorApplication::windowFontRatio(); if (ImGui::Button(fmt::format("Add Input Pin##{}", size_t(id)).c_str())) { @@ -90,30 +93,45 @@ void NAV::WiFiPositioning::guiConfig() if (_numOfDevices == 0) { - if (ImGui::Combo(fmt::format("Frame##{}", size_t(id)).c_str(), reinterpret_cast(&_frame), "ECEF\0LLA\0NED\0\0")) + if (ImGui::Combo(fmt::format("Frame##{}", size_t(id)).c_str(), reinterpret_cast(&_frame), "ENU\0NED\0ECEF\0LLA\0\0")) { switch (_frame) { + case Frame::ENU: + LOG_DEBUG("{}: Frame changed to ENU", nameId()); + break; + case Frame::NED: + LOG_DEBUG("{}: Frame changed to NED", nameId()); + break; case Frame::ECEF: LOG_DEBUG("{}: Frame changed to ECEF", nameId()); break; case Frame::LLA: LOG_DEBUG("{}: Frame changed to LLA", nameId()); break; - case Frame::NED: - LOG_DEBUG("{}: Frame changed to NED", nameId()); - break; } } flow::ApplyChanges(); } - if (ImGui::BeginTable("RouterInput", 4, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) + if (ImGui::BeginTable("RouterInput", 6, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) { // Column headers ImGui::TableSetupColumn("MAC address", ImGuiTableColumnFlags_WidthFixed, columnWidth); - if (_frame == Frame::ECEF) + if (_frame == Frame::ENU) + { + ImGui::TableSetupColumn("East", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("North", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Up", ImGuiTableColumnFlags_WidthFixed, columnWidth); + } + else if (_frame == Frame::NED) + { + ImGui::TableSetupColumn("North", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("East", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Down", ImGuiTableColumnFlags_WidthFixed, columnWidth); + } + else if (_frame == Frame::ECEF) { ImGui::TableSetupColumn("X", ImGuiTableColumnFlags_WidthFixed, columnWidth); ImGui::TableSetupColumn("Y", ImGuiTableColumnFlags_WidthFixed, columnWidth); @@ -125,12 +143,8 @@ void NAV::WiFiPositioning::guiConfig() ImGui::TableSetupColumn("Longitude", ImGuiTableColumnFlags_WidthFixed, columnWidth); ImGui::TableSetupColumn("Altitude", ImGuiTableColumnFlags_WidthFixed, columnWidth); } - else if (_frame == Frame::NED) - { - ImGui::TableSetupColumn("North", ImGuiTableColumnFlags_WidthFixed, columnWidth); - ImGui::TableSetupColumn("East", ImGuiTableColumnFlags_WidthFixed, columnWidth); - ImGui::TableSetupColumn("Down", ImGuiTableColumnFlags_WidthFixed, columnWidth); - } + ImGui::TableSetupColumn("Bias", ImGuiTableColumnFlags_WidthFixed, columnWidth); + ImGui::TableSetupColumn("Scale", ImGuiTableColumnFlags_WidthFixed, columnWidth); // Automatic header row ImGui::TableHeadersRow(); @@ -157,72 +171,103 @@ void NAV::WiFiPositioning::guiConfig() flow::ApplyChanges(); } } - - if (_frame == Frame::ECEF) + if (_frame == Frame::ENU) { ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputX{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputEast{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputY{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputNorth{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputZ{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputUp{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } } - else if (_frame == Frame::LLA) + else if (_frame == Frame::NED) { ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDoubleL(fmt::format("##InputLat{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], -180, 180, 0.0, 0.0, "%.8f°")) + if (ImGui::InputDouble(fmt::format("##InputNorth{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDoubleL(fmt::format("##InputLon{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], -180, 180, 0.0, 0.0, "%.8f°")) + if (ImGui::InputDouble(fmt::format("##InputEast{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputHeight{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputDown{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } } - else if (_frame == Frame::NED) + else if (_frame == Frame::ECEF) { ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputNorth{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputX{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputEast{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputY{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputDown{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputZ{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } } + else if (_frame == Frame::LLA) + { + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDoubleL(fmt::format("##InputLat{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], -180, 180, 0.0, 0.0, "%.8f°")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDoubleL(fmt::format("##InputLon{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], -180, 180, 0.0, 0.0, "%.8f°")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputHeight{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputBias{}", size_t(rowIndex)).c_str(), &_deviceBias.at(rowIndex), 0.0, 0.0, "%.4fm")) + { + flow::ApplyChanges(); + } + ImGui::TableNextColumn(); + ImGui::SetNextItemWidth(columnWidth); + if (ImGui::InputDouble(fmt::format("##InputScale{}", size_t(rowIndex)).c_str(), &_deviceScale.at(rowIndex), 0.0, 0.0, "%.4f")) + { + flow::ApplyChanges(); + } } - ImGui::EndTable(); } if (ImGui::Button(fmt::format("Add Device##{}", size_t(id)).c_str(), ImVec2(columnWidth * 2.1f, 0))) @@ -230,6 +275,8 @@ void NAV::WiFiPositioning::guiConfig() _numOfDevices++; _deviceMacAddresses.push_back("00:00:00:00:00:00"); _devicePositions.push_back(Eigen::Vector3d::Zero()); + _deviceBias.push_back(0.0); + _deviceScale.push_back(0.0); flow::ApplyChanges(); } ImGui::SameLine(); @@ -240,18 +287,17 @@ void NAV::WiFiPositioning::guiConfig() _numOfDevices--; _deviceMacAddresses.pop_back(); _devicePositions.pop_back(); + _deviceBias.pop_back(); + _deviceScale.pop_back(); flow::ApplyChanges(); } } ImGui::Separator(); - if (ImGui::Combo(fmt::format("Solution##{}", size_t(id)).c_str(), reinterpret_cast(&_solutionMode), "Least squares 2D\0Least squares 3D\0Kalman Filter\0\0")) + if (ImGui::Combo(fmt::format("Solution##{}", size_t(id)).c_str(), reinterpret_cast(&_solutionMode), "Least squares\0Kalman Filter\0\0")) { switch (_solutionMode) { - case SolutionMode::LSQ2D: - LOG_DEBUG("{}: Solution changed to Least squares 2D", nameId()); - break; - case SolutionMode::LSQ3D: + case SolutionMode::LSQ: LOG_DEBUG("{}: Solution changed to Least squares 3D", nameId()); break; case SolutionMode::KF: @@ -260,16 +306,125 @@ void NAV::WiFiPositioning::guiConfig() } } flow::ApplyChanges(); - if (_solutionMode == SolutionMode::LSQ2D || _solutionMode == SolutionMode::LSQ3D) + // updateOutputPin(); + + if (_solutionMode == SolutionMode::KF) { - ImGui::SameLine(); - gui::widgets::HelpMarker("The third coordinate of the devices is not utilized in a two-dimensional solution."); - flow::ApplyChanges(); - if (ImGui::InputInt("Output interval [ms]", &_outputInterval)) + // ########################################################################################################### + // Measurement Uncertainties 𝐑 + // ########################################################################################################### + + ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); + if (ImGui::TreeNode(fmt::format("R - Measurement Noise ##{}", size_t(id)).c_str())) + { + if (gui::widgets::InputDoubleWithUnit(fmt::format("({})##{}", + _measurementNoiseUnit == MeasurementNoiseUnit::meter2 + ? "Variance σ²" + : "Standard deviation σ", + size_t(id)) + .c_str(), + configWidth, unitWidth, &_measurementNoise, reinterpret_cast(&_measurementNoiseUnit), "m^2, m^2, m^2\0" + "m, m, m\0\0", + 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: measurementNoise changed to {}", nameId(), _measurementNoise); + LOG_DEBUG("{}: measurementNoiseUnit changed to {}", nameId(), fmt::underlying(_measurementNoiseUnit)); + flow::ApplyChanges(); + } + ImGui::TreePop(); + } + + // ########################################################################################################### + // Process Noise Covariance 𝐐 + // ########################################################################################################### + ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); + if (ImGui::TreeNode(fmt::format("Q - Process Noise ##{}", size_t(id)).c_str())) { - LOG_DEBUG("{}: output interval changed to {}", nameId(), _outputInterval); + if (gui::widgets::InputDoubleWithUnit(fmt::format("({})##{}", + _processNoiseUnit == ProcessNoiseUnit::meter2 + ? "Variance σ²" + : "Standard deviation σ", + size_t(id)) + .c_str(), + configWidth, unitWidth, &_processNoise, reinterpret_cast(&_processNoiseUnit), "m^2, m^2, m^2\0" + "m, m, m\0\0", + 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: processNoise changed to {}", nameId(), _processNoise); + LOG_DEBUG("{}: processNoiseUnit changed to {}", nameId(), fmt::underlying(_processNoiseUnit)); + flow::ApplyChanges(); + } + ImGui::TreePop(); + } + + // ########################################################################################################### + // Initial State Estimate 𝐱0 + // ########################################################################################################### + ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); + if (ImGui::TreeNode(fmt::format("x0 - Initial State##{}", size_t(id)).c_str())) + { + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble3(fmt::format("Position (m)##{}", "m", + size_t(id)) + .c_str(), + _state.e_position.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: e_position changed to {}", nameId(), _state.e_position); + flow::ApplyChanges(); + } + + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble3(fmt::format("Velocity (m/s)##{}", "m", + size_t(id)) + .c_str(), + _state.e_velocity.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: e_position changed to {}", nameId(), _state.e_velocity); + flow::ApplyChanges(); + } + + ImGui::TreePop(); + } + + // ########################################################################################################### + // 𝐏 Error covariance matrix + // ########################################################################################################### + + ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); + if (ImGui::TreeNode(fmt::format("P - Error covariance matrix (init)##{}", size_t(id)).c_str())) + { + if (gui::widgets::InputDouble3WithUnit(fmt::format("Position covariance ({})##{}", + _initCovariancePositionUnit == InitCovariancePositionUnit::meter2 + ? "Variance σ²" + : "Standard deviation σ", + size_t(id)) + .c_str(), + configWidth, unitWidth, _initCovariancePosition.data(), reinterpret_cast(&_initCovariancePositionUnit), "m^2, m^2, m^2\0" + "m, m, m\0\0", + "%.2e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: initCovariancePosition changed to {}", nameId(), _initCovariancePosition); + LOG_DEBUG("{}: initCovariancePositionUnit changed to {}", nameId(), fmt::underlying(_initCovariancePositionUnit)); + flow::ApplyChanges(); + } + + if (gui::widgets::InputDouble3WithUnit(fmt::format("Velocity covariance ({})##{}", + _initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2 + ? "Variance σ²" + : "Standard deviation σ", + size_t(id)) + .c_str(), + configWidth, unitWidth, _initCovarianceVelocity.data(), reinterpret_cast(&_initCovarianceVelocityUnit), "m^2/s^2\0" + "m/s\0\0", + "%.2e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: initCovarianceVelocity changed to {}", nameId(), _initCovarianceVelocity); + LOG_DEBUG("{}: initCovarianceVelocityUnit changed to {}", nameId(), fmt::underlying(_initCovarianceVelocityUnit)); + flow::ApplyChanges(); + } + + ImGui::TreePop(); } - flow::ApplyChanges(); } } @@ -280,12 +435,23 @@ void NAV::WiFiPositioning::guiConfig() json j; j["nWifiInputPins"] = _nWifiInputPins; - j["frame"] = static_cast(_frame); + j["frame"] = _frame; j["deviceMacAddresses"] = _deviceMacAddresses; j["devicePositions"] = _devicePositions; + j["deviceBias"] = _deviceBias; + j["deviceScale"] = _deviceScale; j["numOfDevices"] = _numOfDevices; - j["solutionMode"] = static_cast(_solutionMode); - j["outputInterval"] = _outputInterval; + j["solutionMode"] = _solutionMode; + j["e_position"] = _state.e_position; + j["e_velocity"] = _state.e_velocity; + j["initCovariancePosition"] = _initCovariancePosition; + j["initCovariancePositionUnit"] = _initCovariancePositionUnit; + j["initCovarianceVelocity"] = _initCovarianceVelocity; + j["initCovarianceVelocityUnit"] = _initCovarianceVelocityUnit; + j["measurementNoise"] = _measurementNoise; + j["measurementNoiseUnit"] = _measurementNoiseUnit; + j["processNoise"] = _processNoise; + j["processNoiseUnit"] = _processNoiseUnit; return j; } @@ -311,6 +477,14 @@ void NAV::WiFiPositioning::restore(json const& j) { j.at("devicePositions").get_to(_devicePositions); } + if (j.contains("deviceBias")) + { + j.at("deviceBias").get_to(_deviceBias); + } + if (j.contains("deviceScale")) + { + j.at("deviceScale").get_to(_deviceScale); + } if (j.contains("numOfDevices")) { j.at("numOfDevices").get_to(_numOfDevices); @@ -319,9 +493,45 @@ void NAV::WiFiPositioning::restore(json const& j) { j.at("solutionMode").get_to(_solutionMode); } - if (j.contains("outputInterval")) + if (j.contains("e_position")) + { + j.at("e_position").get_to(_state.e_position); + } + if (j.contains("e_velocity")) + { + j.at("e_velocity").get_to(_state.e_velocity); + } + if (j.contains("initCovariancePosition")) + { + j.at("initCovariancePosition").get_to(_initCovariancePosition); + } + if (j.contains("initCovariancePositionUnit")) + { + j.at("initCovariancePositionUnit").get_to(_initCovariancePositionUnit); + } + if (j.contains("initCovarianceVelocity")) + { + j.at("initCovarianceVelocity").get_to(_initCovarianceVelocity); + } + if (j.contains("initCovarianceVelocityUnit")) + { + j.at("initCovarianceVelocityUnit").get_to(_initCovarianceVelocityUnit); + } + if (j.contains("measurementNoise")) + { + j.at("measurementNoise").get_to(_measurementNoise); + } + if (j.contains("measurementNoiseUnit")) { - j.at("outputInterval").get_to(_outputInterval); + j.at("measurementNoiseUnit").get_to(_measurementNoiseUnit); + } + if (j.contains("processNoise")) + { + j.at("processNoise").get_to(_processNoise); + } + if (j.contains("processNoiseUnit")) + { + j.at("processNoiseUnit").get_to(_processNoiseUnit); } } @@ -329,12 +539,40 @@ bool NAV::WiFiPositioning::initialize() { LOG_TRACE("{}: called", nameId()); - _e_position = Eigen::Vector3d::Zero(); + // Initial Covariance of the velocity in [m²/s²] + Eigen::Vector3d variance_vel = Eigen::Vector3d::Zero(); + if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2) + { + variance_vel = _initCovarianceVelocity; + } + else if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m_s) + { + variance_vel = _initCovarianceVelocity.array().pow(2); + } - LOG_DEBUG("WiFiPositioning initialized"); + // Initial Covariance of the position in [m²] + Eigen::Vector3d variance_pos = Eigen::Vector3d::Zero(); + if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter2) + { + variance_pos = _initCovariancePosition; + } + else if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter) + { + variance_pos = _initCovariancePosition.array().pow(2); + } - _devices.clear(); - _timer.start(_outputInterval, lsqSolution3d, this); + _kalmanFilter.P.diagonal() << variance_pos, variance_vel; + _kalmanFilter.x << _state.e_position, _state.e_velocity; + if (_measurementNoiseUnit == MeasurementNoiseUnit::meter2) + { + _kalmanFilter.R << _measurementNoise; + } + else if (_measurementNoiseUnit == MeasurementNoiseUnit::meter) + { + _kalmanFilter.R << std::pow(_measurementNoise, 2); + } + + LOG_DEBUG("WiFiPositioning initialized"); return true; } @@ -342,11 +580,6 @@ bool NAV::WiFiPositioning::initialize() void NAV::WiFiPositioning::deinitialize() { LOG_TRACE("{}: called", nameId()); - - if (_timer.is_running()) - { - _timer.stop(); - } } void NAV::WiFiPositioning::updateNumberOfInputPins() @@ -367,139 +600,191 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size for (auto const& obs : wifiObs->data) { auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs.macAddress); - if (it != _deviceMacAddresses.end()) // Device already exists + if (it != _deviceMacAddresses.end()) // Device exists { // Get the index of the found element size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); - if (_frame == Frame::LLA) + + // Check if a device with the same position already exists and update the distance + bool deviceExists = false; + for (auto& device : _devices) { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.distance }); + if (device.position == _devicePositions.at(index)) + { + deviceExists = true; + device.distance = obs.distance * _deviceScale.at(index) + _deviceBias.at(index); + device.time = obs.time; + break; + } } - else if (_frame == Frame::ECEF) + + // If the device does not exist, add it to the list + if (!deviceExists) + { + if (_frame == Frame::LLA) + { + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); + } + else if (_frame == Frame::ECEF) + { + _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); + } + else if (_frame == Frame::ENU) + { + _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); + } + else if (_frame == Frame::NED) + { + _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); + } + } + + auto wifiPositioningSolution = std::make_shared(); + wifiPositioningSolution->insTime = obs.time; + if (_solutionMode == SolutionMode::LSQ) { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance }); + if (_devices.size() == _numOfDevices) + { + LeastSquaresResult lsqSolution = WiFiPositioning::lsqSolution(); + wifiPositioningSolution->setPositionAndStdDev_e(_state.e_position, lsqSolution.variance.cwiseSqrt()); + std::cout << lsqSolution.solution << std::endl; + wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance); + invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); + } } - else if (_frame == Frame::NED) + else if (_solutionMode == SolutionMode::KF) { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance }); + WiFiPositioning::kfSolution(); + wifiPositioningSolution->setPositionAndStdDev_e(_kalmanFilter.x.block<3, 1>(0, 0), _kalmanFilter.P.block<3, 3>(0, 0).cwiseSqrt()); + wifiPositioningSolution->setVelocityAndStdDev_e(_kalmanFilter.x.block<3, 1>(3, 0), _kalmanFilter.P.block<3, 3>(3, 3).cwiseSqrt()); + wifiPositioningSolution->setCovarianceMatrix(_kalmanFilter.P); + invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); } - // aufruf kf update + + // print // TODO delete + LOG_DEBUG("{}: Received distance to device {} at position {} with distance {}", nameId(), obs.macAddress, _devicePositions.at(index).transpose(), obs.distance); } } } -void NAV::WiFiPositioning::lsqSolution2d(void* userData) +NAV::LeastSquaresResult NAV::WiFiPositioning::lsqSolution() { - auto* node = static_cast(userData); - if (node->_devices.size() < 3) + LeastSquaresResult lsq; + + if (_devices.size() < 4) { - LOG_DEBUG("{}: Received less than 3 observations. Can't compute position", node->nameId()); - return; + LOG_DEBUG("{}: Received less than 4 observations. Can't compute position", nameId()); + return lsq; } else { - LOG_DEBUG("{}: Received {} observations", node->nameId(), node->_devices.size()); + LOG_DEBUG("{}: Received {} observations", nameId(), _devices.size()); + } + _state.e_position = { 10.0, 10.0, 10.0 }; // TODO Initialwerte + + // calculate the centroid of device positions + Eigen::Vector3d centroid = Eigen::Vector3d::Zero(); + for (const auto& device : _devices) + { + centroid += device.position; } - node->_e_position = { 1, 1, 0 }; + centroid /= _devices.size(); + _state.e_position = centroid; + + Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(_devices.size()), static_cast(3)); + Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_devices.size())); + size_t numMeasurements = _devices.size(); - Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(node->_devices.size()), static_cast(2)); - Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(node->_devices.size())); - size_t numMeasurements = node->_devices.size(); - LeastSquaresResult - lsq; for (size_t o = 0; o < 10; o++) { LOG_DATA("{}: Iteration {}", nameId(), o); for (size_t i = 0; i < numMeasurements; i++) { - double distEst = (node->_devices.at(i).position.head<2>() - node->_e_position.head<2>()).norm(); - ddist(static_cast(i)) = node->_devices.at(i).distance - distEst; - Eigen::Vector2d e_lineOfSightUnitVector = Eigen::Vector2d::Zero(); - if ((node->_e_position.head<2>() - node->_devices.at(i).position.head<2>()).norm() != 0) // Check if it is the same position + // calculate the distance between the device and the estimated position + double distEst = (_devices.at(i).position - _state.e_position).norm(); + // calculate the residual vector + ddist(static_cast(i)) = _devices.at(i).distance - distEst; + + Eigen::Vector3d e_lineOfSightUnitVector = Eigen::Vector3d::Zero(); + if ((_state.e_position - _devices.at(i).position).norm() != 0) // Check if it is the same position { - e_lineOfSightUnitVector = (node->_e_position.head<2>() - node->_devices.at(i).position.head<2>()) / (node->_e_position.head<2>() - node->_devices.at(i).position.head<2>()).norm(); + e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(_state.e_position, _devices.at(i).position); } - e_H.block<1, 2>(static_cast(i), 0) = -e_lineOfSightUnitVector; + // calculate the design matrix + e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; } - // std::cout << "Matrix:\n" - // << e_H << "\n"; - Eigen::Vector2d dx = (e_H.transpose() * e_H).inverse() * e_H.transpose() * ddist; - // LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); - // LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); + // solve the linear least squares problem + lsq = solveLinearLeastSquaresUncertainties(e_H, ddist); + LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); + LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); - node->_e_position.head<2>() += dx; - bool solInaccurate = dx.norm() > 1e-3; + // update the estimated position + _state.e_position += lsq.solution; - if (!solInaccurate) + bool solInaccurate = lsq.solution.norm() > 1e-3; + if (!solInaccurate) // Solution is accurate enough { break; } } - node->_devices.clear(); - auto pos = std::make_shared(); - pos->setPosition_e(node->_e_position); - LOG_DEBUG("{}: Position: {}", node->nameId(), node->_e_position.transpose()); - node->invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); + _devices.clear(); + LOG_DEBUG("{}: Position: {}", nameId(), _state.e_position.transpose()); + + return lsq; } -void NAV::WiFiPositioning::lsqSolution3d(void* userData) +void NAV::WiFiPositioning::kfSolution() { - auto* node = static_cast(userData); - if (node->_devices.size() < 4) - { - LOG_DEBUG("{}: Received less than 4 observations. Can't compute position", node->nameId()); - return; - } - else - { - LOG_DEBUG("{}: Received {} observations", node->nameId(), node->_devices.size()); - } - node->_e_position = { 1, 1, 1 }; - - Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(node->_devices.size()), static_cast(3)); - Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(node->_devices.size())); - size_t numMeasurements = node->_devices.size(); - LeastSquaresResult - lsq; - for (size_t o = 0; o < 10; o++) + double tau_i = !_lastPredictTime.empty() + ? static_cast((_devices.at(0).time - _lastPredictTime).count()) + : 0.0; + + // ########################################################################################################### + // Prediction + // ########################################################################################################### + _lastPredictTime = _devices.at(0).time; + if (tau_i > 0) { - LOG_DATA("{}: Iteration {}", nameId(), o); - for (size_t i = 0; i < numMeasurements; i++) + // Transition matrix + Eigen::MatrixXd F = Eigen::MatrixXd::Zero(6, 6); + F(0, 3) = 1; + F(1, 4) = 1; + F(2, 5) = 1; + _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, 1); + // Process noise covariance matrix + Eigen::Matrix3d Q1 = Eigen::Matrix3d::Zero(); + Q1.diagonal() = Eigen::Vector3d(std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0); + Eigen::Matrix3d Q2 = Eigen::Matrix3d::Zero(); + Q2.diagonal() = Eigen::Vector3d(std::pow(tau_i, 2) / 2.0, std::pow(tau_i, 2) / 2.0, std::pow(tau_i, 2) / 2.0); + Eigen::Matrix3d Q4 = Eigen::Matrix3d::Zero(); + Q4.diagonal() = Eigen::Vector3d(tau_i, tau_i, tau_i); + _kalmanFilter.Q << Q1, Q2, Q2, Q4; + if (_processNoiseUnit == ProcessNoiseUnit::meter2) { - double distEst = (node->_devices.at(i).position - node->_e_position).norm(); - ddist(static_cast(i)) = node->_devices.at(i).distance - distEst; - Eigen::Vector3d e_lineOfSightUnitVector = Eigen::Vector3d::Zero(); - if ((node->_e_position - node->_devices.at(i).position).norm() != 0) // Check if it is the same position - { - e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(node->_e_position, node->_devices.at(i).position); - } - e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; + _kalmanFilter.Q *= _processNoise; } - // std::cout << "Matrix:\n" - // << e_H << "\n"; - Eigen::Vector3d dx = (e_H.transpose() * e_H).inverse() * e_H.transpose() * ddist; - // LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); - // LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); - - node->_e_position += dx; - bool solInaccurate = dx.norm() > 1e-3; - - if (!solInaccurate) + else if (_processNoiseUnit == ProcessNoiseUnit::meter) { - break; + _kalmanFilter.Q *= std::pow(_processNoise, 2); } + // Predict + _kalmanFilter.predict(); } - node->_devices.clear(); - auto pos = std::make_shared(); - pos->setPosition_e(node->_e_position); - LOG_DEBUG("{}: Position: {}", node->nameId(), node->_e_position.transpose()); - node->invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); -} -// void NAV::WiFiPositioning::kfSolution() -// { -// auto pos = std::make_shared(); -// pos->setPosition_e(_e_position); -// LOG_DEBUG("{}: Position: {}", nameId(), _e_position.transpose()); -// invokeCallbacks(OUTPUT_PORT_INDEX_POS, pos); -// } \ No newline at end of file + // ########################################################################################################### + // Update + // ########################################################################################################### + // Measurement + double estimatedDistance = (_devices.at(0).position - _kalmanFilter.x.block<3, 1>(0, 0)).norm(); + _kalmanFilter.z << _devices.at(0).distance - estimatedDistance; + // Design matrix + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 6); + H.block<1, 3>(0, 0) = -e_calcLineOfSightUnitVector(_kalmanFilter.x.block<3, 1>(0, 0), _devices.at(0).position); + _kalmanFilter.H << H; + // Correct + _kalmanFilter.correctWithMeasurementInnovation(); + + _devices.clear(); + LOG_DATA("{}: Position: {}", nameId(), _kalmanFilter.x.block<3, 1>(0, 0).transpose()); + LOG_DATA("{}: Velocity: {}", nameId(), _kalmanFilter.x.block<3, 1>(3, 0).transpose()); +} \ No newline at end of file diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp index 82255cec2..04fd1b8ec 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp @@ -7,9 +7,9 @@ // file, You can obtain one at https://mozilla.org/MPL/2.0/. /// @file WiFiPositioning.hpp -/// @brief Single Point Positioning (SPP) / Code Phase Positioning // TODO -/// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2022-04-29 +/// @brief WiFi Positioning +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @date 2024-01-08 #pragma once @@ -20,6 +20,10 @@ #include "internal/Node/Node.hpp" #include "NodeData/WiFi/WiFiObs.hpp" #include "NodeData/State/Pos.hpp" +#include "NodeData/State/PosVel.hpp" + +#include "Navigation/Math/KalmanFilter.hpp" +#include "Navigation/Math/LeastSquares.hpp" namespace NAV { @@ -62,7 +66,10 @@ class WiFiPositioning : public Node private: constexpr static size_t INPUT_PORT_INDEX_WIFI_OBS = 0; ///< @brief WiFiObs - constexpr static size_t OUTPUT_PORT_INDEX_POS = 0; ///< @brief Pos + constexpr static size_t OUTPUT_PORT_INDEX_WIFISOL = 0; ///< @brief WiFiPositioningSolution + + /// Kalman Filter representation - States: 3xVel, 3xPos - Measurements: 1xDist + KalmanFilter _kalmanFilter{ 6, 1 }; // --------------------------------------------------------------- Gui ----------------------------------------------------------------- @@ -72,18 +79,21 @@ class WiFiPositioning : public Node /// @brief Deinitialize the node void deinitialize() override; - /// Amount of wifi input pins - size_t _nWifiInputPins = 2; + /// @brief Amount of wifi input pins + size_t _nWifiInputPins = 1; /// @brief Adds/Deletes Input Pins depending on the variable _nNavInfoPins void updateNumberOfInputPins(); + // ------------------------------------------------------------ Algorithm -------------------------------------------------------------- + /// @brief Available Frames enum class Frame : int { - ECEF, ///< Earth-Centered Earth-Fixed frame - LLA, ///< Latitude-Longitude-Altitude frame + ENU, ///< East-North-Up frame NED, ///< North-East-Down frame + ECEF, ///< Earth-Centered Earth-Fixed frame // TODO + LLA, ///< Latitude-Longitude-Altitude frame // TODO }; /// Frame to calculate the position in Frame _frame = Frame::ECEF; @@ -91,22 +101,29 @@ class WiFiPositioning : public Node /// @brief Available Solution Modes enum class SolutionMode : int { - LSQ2D, ///< Least Squares in 2D - LSQ3D, ///< Least Squares in 3D - KF, ///< Kalman Filter + LSQ, ///< Least Squares + KF, ///< Kalman Filter }; /// Solution Mode - SolutionMode _solutionMode = SolutionMode::LSQ3D; + SolutionMode _solutionMode = SolutionMode::LSQ; - /// Output interval in ms - int _outputInterval; - // ------------------------------------------------------------ Algorithm -------------------------------------------------------------- + /// @brief State estimated by the positioning algorithm + struct State + { + /// Estimated position in ECEF frame [m] + Eigen::Vector3d e_position = Eigen::Vector3d::Zero(); + /// Estimated velocity in ECEF frame [m/s] + Eigen::Vector3d e_velocity = Eigen::Vector3d::Zero(); + }; - /// Estimated position in local frame [m] - Eigen::Vector3d _e_position = Eigen::Vector3d::Zero(); + /// State estimated by the algorithm + State _state; - std::vector _deviceMacAddresses; + std::vector + _deviceMacAddresses; std::vector _devicePositions; + std::vector _deviceBias; + std::vector _deviceScale; size_t _numOfDevices; struct Device @@ -117,19 +134,76 @@ class WiFiPositioning : public Node }; std::vector _devices; + /// Time when the last prediction was triggered + InsTime _lastPredictTime; + /// @brief Receive Function for the WiFi Observations /// @param[in] queue Queue with all the received data messages /// @param[in] pinIdx Index of the pin the data is received on void recvWiFiObs(InputPin::NodeDataQueue& queue, size_t pinIdx); - /// Timer object to handle async data requests - CallbackTimer _timer; + /// @brief Calculate the position using the least squares method + /// @return Least Squares solution + LeastSquaresResult lsqSolution(); /// @brief Calculate the position - static void lsqSolution2d(void* userData); + void kfSolution(); - /// @brief Calculate the position - static void lsqSolution3d(void* userData); + // ########################################################################################################### + + /// Possible Units for the measurement noise (standard deviation σ or Variance σ²) + enum class MeasurementNoiseUnit + { + meter2, ///< Variance NED [m^2, m^2, m^2] + meter, ///< Standard deviation NED [m, m, m] + }; + /// Gui selection for the Unit of the initial covariance for the position + MeasurementNoiseUnit _measurementNoiseUnit = MeasurementNoiseUnit::meter; + + /// GUI selection of the process noise (standard deviation σ or Variance σ²) + double _measurementNoise = 10; + + // ########################################################################################################### + + /// Possible Units for the process noise (standard deviation σ or Variance σ²) + enum class ProcessNoiseUnit + { + meter2, ///< Variance NED [m^2, m^2, m^2] + meter, ///< Standard deviation NED [m, m, m] + }; + /// Gui selection for the Unit of the initial covariance for the position + ProcessNoiseUnit _processNoiseUnit = ProcessNoiseUnit::meter; + + /// GUI selection of the process noise (standard deviation σ or Variance σ²) + double _processNoise = 10; + + // ########################################################################################################### + + /// Possible Units for the initial covariance for the position (standard deviation σ or Variance σ²) + enum class InitCovariancePositionUnit + { + meter2, ///< Variance NED [m^2, m^2, m^2] + meter, ///< Standard deviation NED [m, m, m] + }; + /// Gui selection for the Unit of the initial covariance for the position + InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter; + + /// GUI selection of the initial covariance diagonal values for position (standard deviation σ or Variance σ²) + Eigen::Vector3d _initCovariancePosition{ 100, 100, 100 }; + + // ########################################################################################################### + + /// Possible Units for the initial covariance for the velocity (standard deviation σ or Variance σ²) + enum class InitCovarianceVelocityUnit + { + m2_s2, ///< Variance [m^2/s^2] + m_s, ///< Standard deviation [m/s] + }; + /// Gui selection for the Unit of the initial covariance for the velocity + InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s; + + /// GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Variance σ²) + Eigen::Vector3d _initCovarianceVelocity{ 10, 10, 10 }; }; } // namespace NAV \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp index c9a9ac2ac..6a568c1f1 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp @@ -8,8 +8,8 @@ /// @file ArubaSensor.hpp /// @brief Aruba Sensor Class -/// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2020-03-19 +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @date 2024-01-08 #pragma once diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp new file mode 100644 index 000000000..35306ec20 --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp @@ -0,0 +1,246 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "EspressifFile.hpp" + +#include "util/Logger.hpp" + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +#include "util/Vendor/Espressif/EspressifUtilities.hpp" +#include "util/Time/TimeBase.hpp" + +#include "NodeData/WiFi/WiFiObs.hpp" + +NAV::EspressifFile::EspressifFile() + : Node(typeStatic()), _sensor(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _hasConfig = true; + _guiConfigDefaultWindowSize = { 380, 70 }; + + nm::CreateOutputPin(this, "WiFiObs", Pin::Type::Flow, { NAV::WiFiObs::type() }, &EspressifFile::pollData); +} + +NAV::EspressifFile::~EspressifFile() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::EspressifFile::typeStatic() +{ + return "EspressifFile"; +} + +std::string NAV::EspressifFile::type() const +{ + return typeStatic(); +} + +std::string NAV::EspressifFile::category() +{ + return "Data Provider"; +} + +void NAV::EspressifFile::guiConfig() +{ + if (auto res = FileReader::guiConfig(".bin,.*", { ".bin" }, size_t(id), nameId())) + { + LOG_DEBUG("{}: Path changed to {}", nameId(), _path); + flow::ApplyChanges(); + if (res == FileReader::PATH_CHANGED) + { + doReinitialize(); + } + else + { + doDeinitialize(); + } + } +} + +[[nodiscard]] json NAV::EspressifFile::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["FileReader"] = FileReader::save(); + + return j; +} + +void NAV::EspressifFile::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("FileReader")) + { + FileReader::restore(j.at("FileReader")); + } +} + +bool NAV::EspressifFile::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + return FileReader::initialize(); +} + +void NAV::EspressifFile::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); + + FileReader::deinitialize(); +} + +bool NAV::EspressifFile::resetNode() +{ + FileReader::resetReader(); + + return true; +} + +std::shared_ptr NAV::EspressifFile::pollData() +{ + uint8_t i = 0; + std::unique_ptr packet = nullptr; + std::shared_ptr obs; + while (true) + { + while (!eof() && read(reinterpret_cast(&i), 1)) + { + packet = _sensor.findPacket(i); + + if (packet != nullptr) + { + break; + } + } + + if (!packet || eof()) + { + LOG_DEBUG("{}: End of file reached.", nameId()); + return nullptr; + } + + if (packet->getRawDataLength() == 0) + { + LOG_TRACE("{}: Packet has empty payload", nameId()); + return nullptr; + } + + obs = std::make_shared(); + if (!vendor::espressif::decryptWiFiObs(obs, *packet, nameId())) { continue; }; + if (packet->type() != uart::protocol::Packet::Type::TYPE_BINARY) { continue; }; + + if (!obs->insTime.empty()) + { + _lastObsTime = obs->insTime; + } + else + { + if (!_lastObsTime.empty()) + { + obs->insTime = _lastObsTime; + } + else + { + LOG_DATA("{}: Could not set valid time. Skipping package.", nameId()); + continue; + } + } + break; + } + + LOG_DATA("{}: [{}] Packet found [{}][{}]", nameId(), obs->insTime.toYMDHMS(GPST), + obs->msgClass, vendor::ublox::getStringFromMsgId(obs->msgClass, obs->msgId)); + + invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); + return obs; +} + +// std::shared_ptr NAV::EspressifFile::pollData() +// { +// auto filestream = std::ifstream(getFilepath(), std::ios::binary); // TODO +// uint8_t i = 0; +// std::unique_ptr packet = nullptr; +// // while (filestream.read(reinterpret_cast(&i), 1)) +// while (readsome(reinterpret_cast(&i), 1)) // TODO Wieso funktioniert das nicht? +// { +// packet = _sensor.findPacket(i); + +// if (packet != nullptr) +// { +// break; +// } +// } + +// if (!packet) +// { +// return nullptr; +// } + +// // Check if package is empty +// if (packet->getRawDataLength() == 0) +// { +// return nullptr; +// } + +// auto obs = std::make_shared(); +// vendor::espressif::decryptSingleWiFiObsDeviceTime(obs, *packet); // TODO weitere neben single? + +// if (!obs->insTime.empty()) +// { +// if (util::time::GetMode() == util::time::Mode::REAL_TIME) +// { +// util::time::SetCurrentTime(obs->insTime); +// } +// } +// else if (auto currentTime = util::time::GetCurrentInsTime(); +// !currentTime.empty()) +// { +// obs->insTime = currentTime; +// } + +// invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); +// // filestream.close(); // TODO +// return obs; +// } + +NAV::FileReader::FileType NAV::EspressifFile::determineFileType() +{ + LOG_TRACE("called for {}", nameId()); + + auto filestream = std::ifstream(getFilepath()); + + constexpr uint16_t BUFFER_SIZE = 10; + + std::array buffer{}; + if (filestream.good()) + { + filestream.read(buffer.data(), BUFFER_SIZE); + + if ((static_cast(buffer.at(0)) == vendor::espressif::EspressifUartSensor::BINARY_SYNC_CHAR_1 + && static_cast(buffer.at(1)) == vendor::espressif::EspressifUartSensor::BINARY_SYNC_CHAR_2)) + { + filestream.close(); + LOG_DEBUG("{} has the file type: Binary", nameId()); + return FileType::BINARY; + } + filestream.close(); + + LOG_ERROR("{} could not determine file type", nameId()); + return FileType::NONE; + } + LOG_ERROR("{} could not open file {}", nameId(), getFilepath()); + return FileType::NONE; +} \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp new file mode 100644 index 000000000..f7577d924 --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp @@ -0,0 +1,88 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file EspressifFile.hpp +/// @brief File Reader for ubx files +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2024-01-08 + +#pragma once + +#include "internal/Node/Node.hpp" +#include "Nodes/DataProvider/Protocol/FileReader.hpp" + +#include "util/Vendor/Espressif/EspressifUartSensor.hpp" + +namespace NAV +{ +/// File Reader for WiFiObs log files +class EspressifFile : public Node, public FileReader +{ + public: + /// @brief Default constructor + EspressifFile(); + /// @brief Destructor + ~EspressifFile() override; + /// @brief Copy constructor + EspressifFile(const EspressifFile&) = delete; + /// @brief Move constructor + EspressifFile(EspressifFile&&) = delete; + /// @brief Copy assignment operator + EspressifFile& operator=(const EspressifFile&) = delete; + /// @brief Move assignment operator + EspressifFile& operator=(EspressifFile&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + /// @brief Resets the node. Moves the read cursor to the start + bool resetNode() override; + + private: + constexpr static size_t OUTPUT_PORT_INDEX_WiFiObs_OBS = 0; ///< @brief Flow (WiFiObs) + + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Polls data from the file + /// @return The read observation + [[nodiscard]] std::shared_ptr pollData(); + + /// @brief Determines the type of the file + /// @return The File Type + [[nodiscard]] FileType determineFileType() override; + + /// + InsTime _lastObsTime; + + /// Sensor Object + vendor::espressif::EspressifUartSensor _sensor; +}; + +} // namespace NAV diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp index 6ea549208..d13ea21c8 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -30,7 +30,8 @@ NAV::EspressifSensor::EspressifSensor() _guiConfigDefaultWindowSize = { 360, 70 }; // TODO: Update the library to handle different baudrates - _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_9600); + // _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_9600); + _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_115200); _sensorPort = "/dev/ttyACM0"; nm::CreateOutputPin(this, "UartPacket", Pin::Type::Flow, { NAV::UartPacket::type() }); @@ -106,7 +107,7 @@ bool NAV::EspressifSensor::initialize() // connect to the sensor try { - _sensor->connect(_sensorPort, sensorBaudrate()); + _sensor->connect(_sensorPort, BAUDRATE_115200); // change Baudrate here LOG_DEBUG("{} connected on port {} with baudrate {}", nameId(), _sensorPort, sensorBaudrate()); } diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp index a01563159..417cd9ed8 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp @@ -8,8 +8,9 @@ /// @file EspressifSensor.hpp /// @brief Espressif Sensor Class +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) /// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2020-03-19 +/// @date 2024-01-08 #pragma once diff --git a/src/Nodes/Plotting/Plot.cpp b/src/Nodes/Plotting/Plot.cpp index e960e3b8b..6a5994e90 100644 --- a/src/Nodes/Plotting/Plot.cpp +++ b/src/Nodes/Plotting/Plot.cpp @@ -358,7 +358,8 @@ NAV::Plot::Plot() ImuObsSimulated::type(), KvhObs::type(), ImuObsWDelta::type(), - VectorNavBinaryOutput::type() }; + VectorNavBinaryOutput::type(), + WiFiPositioningSolution::type() }; updateNumberOfInputPins(); @@ -2236,6 +2237,53 @@ void NAV::Plot::afterCreateLink(OutputPin& startPin, InputPin& endPin) _pinData.at(pinIndex).addPlotDataItem(i++, "GNSS2::RawMeas::Week"); _pinData.at(pinIndex).addPlotDataItem(i++, "GNSS2::RawMeas::NumSats"); } + else if (startPin.dataIdentifier.front() == WiFiPositioningSolution::type()) + { + // NodeData + _pinData.at(pinIndex).addPlotDataItem(i++, "Time [s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "GPS time of week [s]"); + // PosVel + _pinData.at(pinIndex).addPlotDataItem(i++, "Latitude [deg]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Longitude [deg]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Altitude [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "North/South [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "East/West [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "X-ECEF [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Y-ECEF [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Z-ECEF [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Velocity norm [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "X velocity ECEF [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Y velocity ECEF [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Z velocity ECEF [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "North velocity [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "East velocity [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Down velocity [m/s]"); + // WiFiPositioningSolution + _pinData.at(pinIndex).addPlotDataItem(i++, "X-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Y-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Z-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "XY-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "XZ-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "YZ-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "North StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "East StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Down StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "NE-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "ND-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "ED-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "X velocity ECEF StDev [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Y velocity ECEF StDev [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Z velocity ECEF StDev [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "XY velocity StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "XZ velocity StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "YZ velocity StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "North velocity StDev [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "East velocity StDev [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Down velocity StDev [m/s]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "NE velocity StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "ND velocity StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "ED velocity StDev [m]"); + } } else if (inputPins.at(pinIndex).type == Pin::Type::Bool) { @@ -2633,6 +2681,10 @@ void NAV::Plot::plotData(NAV::InputPin::NodeDataQueue& queue, size_t pinIdx) { plotVectorNavBinaryObs(std::static_pointer_cast(nodeData), pinIdx); } + else if (sourcePin->dataIdentifier.front() == WiFiPositioningSolution::type()) + { + plotWiFiPositioningSolution(std::static_pointer_cast(nodeData), pinIdx); + } } } @@ -3550,4 +3602,75 @@ void NAV::Plot::plotVectorNavBinaryObs(const std::shared_ptrgnss2Outputs && (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS) ? obs->gnss2Outputs->raw.tow : std::nan("")); addData(pinIndex, i++, obs->gnss2Outputs && (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS) ? static_cast(obs->gnss2Outputs->raw.week) : std::nan("")); addData(pinIndex, i++, obs->gnss2Outputs && (obs->gnss2Outputs->gnssField & vn::protocol::uart::GpsGroup::GPSGROUP_RAWMEAS) ? static_cast(obs->gnss2Outputs->raw.numSats) : std::nan("")); +} + +void NAV::Plot::plotWiFiPositioningSolution(const std::shared_ptr& obs, size_t pinIndex) +{ + if (!obs->insTime.empty() && _startTime.empty()) { _startTime = obs->insTime; } + size_t i = 0; + + if (!_originPosition) + { + _originPosition = { .frame = gui::widgets::PositionWithFrame::ReferenceFrame::ECEF, + .e_position = obs->e_position() }; + } + + int sign = obs->lla_position().x() > _originPosition->latitude() ? 1 : -1; + // North/South deviation [m] + double northSouth = calcGeographicalDistance(obs->lla_position().x(), obs->lla_position().y(), + _originPosition->latitude(), obs->lla_position().y()) + * sign; + + sign = obs->lla_position().y() > _originPosition->longitude() ? 1 : -1; + // East/West deviation [m] + double eastWest = calcGeographicalDistance(obs->lla_position().x(), obs->lla_position().y(), + obs->lla_position().x(), _originPosition->longitude()) + * sign; + + std::scoped_lock guard(_pinData.at(pinIndex).mutex); + + // NodeData + addData(pinIndex, i++, !obs->insTime.empty() ? static_cast((obs->insTime - _startTime).count()) : std::nan("")); + addData(pinIndex, i++, !obs->insTime.empty() ? static_cast(obs->insTime.toGPSweekTow().tow) : std::nan("")); + // PosVel + addData(pinIndex, i++, rad2deg(obs->lla_position()(0))); + addData(pinIndex, i++, rad2deg(obs->lla_position()(1))); + addData(pinIndex, i++, obs->lla_position()(2)); + addData(pinIndex, i++, northSouth); + addData(pinIndex, i++, eastWest); + addData(pinIndex, i++, obs->e_position().x()); + addData(pinIndex, i++, obs->e_position().y()); + addData(pinIndex, i++, obs->e_position().z()); + addData(pinIndex, i++, obs->e_velocity().norm()); + addData(pinIndex, i++, obs->e_velocity().x()); + addData(pinIndex, i++, obs->e_velocity().y()); + addData(pinIndex, i++, obs->e_velocity().z()); + addData(pinIndex, i++, obs->n_velocity().x()); + addData(pinIndex, i++, obs->n_velocity().y()); + addData(pinIndex, i++, obs->n_velocity().z()); + // WiFiPositionSolution + addData(pinIndex, i++, obs->e_positionStdev()(0)); // X-ECEF StDev [m] + addData(pinIndex, i++, obs->e_positionStdev()(1)); // Y-ECEF StDev [m] + addData(pinIndex, i++, obs->e_positionStdev()(2)); // Z-ECEF StDev [m] + addData(pinIndex, i++, obs->e_CovarianceMatrix()(0, 1)); // XY-ECEF StDev [m] + addData(pinIndex, i++, obs->e_CovarianceMatrix()(0, 2)); // XZ-ECEF StDev [m] + addData(pinIndex, i++, obs->e_CovarianceMatrix()(1, 2)); // YZ-ECEF StDev [m] + addData(pinIndex, i++, obs->n_positionStdev()(0)); // North StDev [m] + addData(pinIndex, i++, obs->n_positionStdev()(1)); // East StDev [m] + addData(pinIndex, i++, obs->n_positionStdev()(2)); // Down StDev [m] + addData(pinIndex, i++, obs->n_CovarianceMatrix()(0, 1)); // NE-ECEF StDev [m] + addData(pinIndex, i++, obs->n_CovarianceMatrix()(0, 2)); // ND-ECEF StDev [m] + addData(pinIndex, i++, obs->n_CovarianceMatrix()(1, 2)); // ED-ECEF StDev [m] + addData(pinIndex, i++, obs->e_velocityStdev()(0)); // X velocity ECEF StDev [m/s] + addData(pinIndex, i++, obs->e_velocityStdev()(1)); // Y velocity ECEF StDev [m/s] + addData(pinIndex, i++, obs->e_velocityStdev()(2)); // Z velocity ECEF StDev [m/s] + addData(pinIndex, i++, obs->e_CovarianceMatrix().rows() >= 4 ? obs->e_CovarianceMatrix()(3, 4) : std::nan("")); // XY velocity StDev [m] + addData(pinIndex, i++, obs->e_CovarianceMatrix().rows() >= 4 ? obs->e_CovarianceMatrix()(3, 5) : std::nan("")); // XZ velocity StDev [m] + addData(pinIndex, i++, obs->e_CovarianceMatrix().rows() >= 4 ? obs->e_CovarianceMatrix()(4, 5) : std::nan("")); // YZ velocity StDev [m] + addData(pinIndex, i++, obs->n_velocityStdev()(0)); // North velocity StDev [m/s] + addData(pinIndex, i++, obs->n_velocityStdev()(1)); // East velocity StDev [m/s] + addData(pinIndex, i++, obs->n_velocityStdev()(2)); // Down velocity StDev [m/s] + addData(pinIndex, i++, obs->n_CovarianceMatrix().rows() >= 4 ? obs->n_CovarianceMatrix()(3, 4) : std::nan("")); // NE velocity StDev [m] + addData(pinIndex, i++, obs->n_CovarianceMatrix().rows() >= 4 ? obs->n_CovarianceMatrix()(3, 5) : std::nan("")); // ND velocity StDev [m] + addData(pinIndex, i++, obs->n_CovarianceMatrix().rows() >= 4 ? obs->n_CovarianceMatrix()(4, 5) : std::nan("")); // ED velocity StDev [m] } \ No newline at end of file diff --git a/src/Nodes/Plotting/Plot.hpp b/src/Nodes/Plotting/Plot.hpp index 876db74b0..73b71bd90 100644 --- a/src/Nodes/Plotting/Plot.hpp +++ b/src/Nodes/Plotting/Plot.hpp @@ -37,6 +37,7 @@ #include "NodeData/IMU/KvhObs.hpp" #include "NodeData/IMU/ImuObsWDelta.hpp" #include "NodeData/IMU/VectorNavBinaryOutput.hpp" +#include "NodeData/WiFi/WiFiPositioningSolution.hpp" namespace NAV { @@ -497,6 +498,11 @@ class Plot : public Node /// @param[in] pinIndex Index of the input pin where the data was received void plotVectorNavBinaryObs(const std::shared_ptr& obs, size_t pinIndex); + /// @brief Plot the data + /// @param[in] obs Observation to plot + /// @param[in] pinIndex Index of the input pin where the data was received + void plotWiFiPositioningSolution(const std::shared_ptr& obs, size_t pinIndex); + /// Data storage for each pin std::vector _pinData; diff --git a/src/util/Vendor/Espressif/EspressifUartSensor.hpp b/src/util/Vendor/Espressif/EspressifUartSensor.hpp index 74fff7f6f..4af064138 100644 --- a/src/util/Vendor/Espressif/EspressifUartSensor.hpp +++ b/src/util/Vendor/Espressif/EspressifUartSensor.hpp @@ -8,8 +8,9 @@ /// @file EspressifUartSensor.hpp /// @brief Class to read out Espressif Sensors +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) /// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2020-07-22 +/// @date 2024-01-08 #pragma once diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 47a6a8a4c..50d24e40d 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -18,7 +18,7 @@ /// Speed of light in air [m/s] constexpr double cAir = 299702547.0; -void NAV::vendor::espressif::decryptSingleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) +bool NAV::vendor::espressif::decryptWiFiObs(const std::shared_ptr& obs, uart::protocol::Packet& packet, [[maybe_unused]] const std::string& nameId) { obs->insTime = util::time::GetCurrentInsTime(); if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) @@ -28,8 +28,8 @@ void NAV::vendor::espressif::decryptSingleWiFiObsDeviceTime(const std::shared_pt std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase // Distance - int rtt = packet.extractInt32(); - double measuredDistance = static_cast(rtt) * cAir * 1e-9 / 2; + int rtt = packet.extractInt32(); // TODO check if ps or ns + double measuredDistance = static_cast(rtt) * cAir * 1e-12 / 2; // Time of measurement InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); @@ -38,87 +38,14 @@ void NAV::vendor::espressif::decryptSingleWiFiObsDeviceTime(const std::shared_pt obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); // Log the measurement details LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); + LOG_DEBUG("WiFiObs mac Address: {}, measured distance: {}, time of measurement: {}", macAddress, measuredDistance, timeOfMeasurement); } else { LOG_DEBUG("Received non-binary packet. Ignoring."); + return false; } -} - -void NAV::vendor::espressif::decryptMultipleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) -{ - obs->insTime = util::time::GetCurrentInsTime(); - if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) - { - obs->payloadLength = packet.extractUint16(); // TODO remove - size_t measurementLength = 14; // TODO irgendwo anders definieren - size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U - for (size_t i = 0; i < numOfMeasurement; i++) - { - std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); - std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase - double measuredDistance = packet.extractDouble(); - obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); - // Log the measurement details - LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); - } - } - else - { - LOG_DEBUG("Received non-binary packet. Ignoring."); - } -} - -void NAV::vendor::espressif::decryptSingleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) -{ - obs->insTime = util::time::GetCurrentInsTime(); - obs->insTime = util::time::GetCurrentInsTime(); - if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) - { - obs->payloadLength = packet.extractUint16(); // TODO remove - // Mac address - std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); - std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase - // Distance - int rtt = packet.extractInt32(); - double measuredDistance = static_cast(rtt) * cAir * 1e-9 / 2; - // Time of measurement - InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); - InsTime timeOfMeasurement(yearMonthDayHMS, UTC); - [[maybe_unused]] int ms = packet.extractInt32(); - // Add the measurement to the WiFiObs - obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); - // Log the measurement details - LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); - } - else - { - LOG_DEBUG("Received non-binary packet. Ignoring."); - } -} - -void NAV::vendor::espressif::decryptMultipleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet) -{ - obs->insTime = util::time::GetCurrentInsTime(); - if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) - { - obs->payloadLength = packet.extractUint16(); // TODO remove - size_t measurementLength = 14; // TODO irgendwo anders definieren - size_t numOfMeasurement = static_cast(obs->payloadLength) / measurementLength; // TODO checksum length = 2U - for (size_t i = 0; i < numOfMeasurement; i++) - { - std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); - std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase - double measuredDistance = packet.extractDouble(); - obs->data.push_back({ macAddress, util::time::GetCurrentInsTime(), measuredDistance }); - // Log the measurement details - LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); - } - } - else - { - LOG_DEBUG("Received non-binary packet. Ignoring."); - } + return true; } std::pair NAV::vendor::espressif::checksumUBX(const std::vector& data) diff --git a/src/util/Vendor/Espressif/EspressifUtilities.hpp b/src/util/Vendor/Espressif/EspressifUtilities.hpp index 58af6dc83..01f30b0be 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.hpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.hpp @@ -8,8 +8,9 @@ /// @file EspressifUtilities.hpp /// @brief Helper Functions to work with Espressif Sensors +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) /// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2020-07-22 +/// @date 2024-01-08 #pragma once @@ -23,25 +24,10 @@ namespace NAV::vendor::espressif { -/// @brief Decrypts the provided Espressif observation with device time -/// @param[in] obs Espressif Observation to decrypt -/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) -void decryptSingleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); - -/// @brief Decrypts the provided multiple Espressif observation with device time -/// @param[in] obs Espressif Observation to decrypt -/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) -void decryptMultipleWiFiObsDeviceTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); - /// @brief Decrypts the provided Espressif observation /// @param[in] obs Espressif Observation to decrypt /// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) -void decryptSingleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); - -/// @brief Decrypts the provided multiple Espressif observation -/// @param[in] obs Espressif Observation to decrypt -/// @param[in, out] packet Uart packet with the data (content gets changed because data gets extracted) -void decryptMultipleWiFiObsInstinctTime(const std::shared_ptr& obs, uart::protocol::Packet& packet); +bool decryptWiFiObs(const std::shared_ptr& obs, uart::protocol::Packet& packet, [[maybe_unused]] const std::string& nameId); /// @brief Calculates the two UBX checksums for the provided data vector /// @param[in] data Data Vector for which the checksum should be calculated From 6a234b57b4e20808264b831182f67fbf416adf44 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Mon, 18 Mar 2024 23:26:08 +0100 Subject: [PATCH 05/27] Add WiFiObsLogger and WiFiFile --- src/NodeData/WiFi/WiFiObs.hpp | 6 + src/NodeRegistry.cpp | 6 +- .../Converter/GNSS/UartPacketConverter.cpp | 26 ++ .../Converter/GNSS/UartPacketConverter.hpp | 4 + src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp | 184 +++++++++++++ src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp | 77 ++++++ .../DataProcessor/WiFi/WiFiPositioning.cpp | 11 + .../{Sensors => FileReader}/EspressifFile.cpp | 48 ---- .../{Sensors => FileReader}/EspressifFile.hpp | 2 +- .../WiFi/FileReader/WiFiObsFile.cpp | 253 ++++++++++++++++++ .../WiFi/FileReader/WiFiObsFile.hpp | 77 ++++++ .../DataProvider/WiFi/Sensors/ArubaSensor.cpp | 2 +- .../WiFi/Sensors/EspressifSensor.cpp | 1 - .../Vendor/Espressif/EspressifUtilities.cpp | 5 + 14 files changed, 650 insertions(+), 52 deletions(-) create mode 100644 src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp create mode 100644 src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp rename src/Nodes/DataProvider/WiFi/{Sensors => FileReader}/EspressifFile.cpp (78%) rename src/Nodes/DataProvider/WiFi/{Sensors => FileReader}/EspressifFile.hpp (99%) create mode 100644 src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp create mode 100644 src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp diff --git a/src/NodeData/WiFi/WiFiObs.hpp b/src/NodeData/WiFi/WiFiObs.hpp index 79c13b568..05670eb3c 100644 --- a/src/NodeData/WiFi/WiFiObs.hpp +++ b/src/NodeData/WiFi/WiFiObs.hpp @@ -14,6 +14,7 @@ #pragma once #include "NodeData/NodeData.hpp" +#include "util/Vendor/VectorNav/BinaryOutputs/TimeOutputs.hpp" namespace NAV { @@ -40,8 +41,13 @@ class WiFiObs : public NodeData struct FtmObs { + /// MAC address of the device std::string macAddress; + /// Time of the measurement InsTime time; + /// TODO + // NAV::vendor::vectornav::TimeOutputs timeOutputs; + /// Distance to the device double distance; }; diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index a0d680e0e..80398bd26 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -190,6 +190,7 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataLogger/IMU/VectorNavDataLogger.hpp" #include "Nodes/DataLogger/State/LcKfInsGnssErrorLogger.hpp" #include "Nodes/DataLogger/State/PosVelAttLogger.hpp" +#include "Nodes/DataLogger/WiFi/WiFiObsLogger.hpp" // Data Processor #include "Nodes/DataProcessor/ErrorModel/ErrorModel.hpp" #include "Nodes/DataProcessor/GNSS/SinglePointPositioning.hpp" @@ -219,7 +220,8 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataProvider/IMU/FileReader/MultiImuFile.hpp" #include "Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp" #include "Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp" -#include "Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp" +#include "Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp" +#include "Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp" // Data Simulator #include "Nodes/DataProvider/IMU/Simulators/ImuSimulator.hpp" // Plotting @@ -259,6 +261,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); // Data Processor registerNodeType(); registerNodeType(); @@ -289,6 +292,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); // Data Simulator registerNodeType(); // Experimental diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index afaae76a6..56d3e61f7 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -97,6 +97,32 @@ void NAV::UartPacketConverter::guiConfig() } flow::ApplyChanges(); } + + // if (_outputType == OutputType_WiFiObs) + // { + ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); + if (ImGui::TreeNode(fmt::format("Synchronization Control##{}", size_t(id)).c_str())) + { + ImGui::TextUnformatted("Contains parameters which allow the timing of the VN-310E to be\n" + "synchronized with external devices."); + + if (ImGui::Checkbox(fmt::format("Show SyncIn Pin##{}", size_t(id)).c_str(), &_syncInPin)) + { + LOG_DEBUG("{}: syncInPin changed to {}", nameId(), _syncInPin); + flow::ApplyChanges(); + if (_syncInPin) + { + nm::CreateInputPin(this, "SyncIn", Pin::Type::Object, { "TimeSync" }); + } + else if (!_syncInPin) + { + nm::DeleteInputPin(inputPins.at(INPUT_PORT_INDEX_SYNC_IN)); + } + } + ImGui::TreePop(); + } + flow::ApplyChanges(); + // } } [[nodiscard]] json NAV::UartPacketConverter::save() const diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp index eb5b21544..2fc8575d9 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp @@ -66,6 +66,7 @@ class UartPacketConverter : public Node private: constexpr static size_t OUTPUT_PORT_INDEX_CONVERTED = 0; ///< @brief Flow constexpr static size_t INPUT_PORT_INDEX_UART_PACKET = 0; ///< @brief Flow (UartPacket) + constexpr static size_t INPUT_PORT_INDEX_SYNC_IN = 1; ///< @brief Flow (SyncIn) /// Enum specifying the type of the output message enum OutputType @@ -78,6 +79,9 @@ class UartPacketConverter : public Node /// The selected output type in the GUI OutputType _outputType = OutputType_UbloxObs; + /// Show the SyncIn Pin + bool _syncInPin = false; + /// @brief Initialize the node bool initialize() override; diff --git a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp new file mode 100644 index 000000000..f623cd43f --- /dev/null +++ b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp @@ -0,0 +1,184 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "WiFiObsLogger.hpp" + +#include "NodeData/WiFi/WiFiObs.hpp" + +#include "Navigation/Transformations/Units.hpp" + +#include "util/Logger.hpp" + +#include // std::setprecision + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +NAV::WiFiObsLogger::WiFiObsLogger() + : Node(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _fileType = FileType::ASCII; + + _hasConfig = true; + _guiConfigDefaultWindowSize = { 380, 70 }; + + nm::CreateInputPin(this, "writeObservation", Pin::Type::Flow, { WiFiObs::type() }, &WiFiObsLogger::writeObservation); +} + +NAV::WiFiObsLogger::~WiFiObsLogger() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::WiFiObsLogger::typeStatic() +{ + return "WiFiObsLogger"; +} + +std::string NAV::WiFiObsLogger::type() const +{ + return typeStatic(); +} + +std::string NAV::WiFiObsLogger::category() +{ + return "Data Logger"; +} + +void NAV::WiFiObsLogger::guiConfig() +{ + if (FileWriter::guiConfig(".csv", { ".csv" }, size_t(id), nameId())) + { + flow::ApplyChanges(); + doDeinitialize(); + } +} + +[[nodiscard]] json NAV::WiFiObsLogger::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["FileWriter"] = FileWriter::save(); + + return j; +} + +void NAV::WiFiObsLogger::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("FileWriter")) + { + FileWriter::restore(j.at("FileWriter")); + } +} + +void NAV::WiFiObsLogger::flush() +{ + _filestream.flush(); +} + +bool NAV::WiFiObsLogger::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + if (!FileWriter::initialize()) + { + return false; + } + + CommonLog::initialize(); + + _filestream << "Time [s],GpsCycle,GpsWeek,GpsToW [s]," + << "MacAddress," + << "Distance [m]" << std::endl; + + return true; +} + +void NAV::WiFiObsLogger::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); + + FileWriter::deinitialize(); +} + +void NAV::WiFiObsLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, size_t pinIdx) +{ + if (auto* sourcePin = inputPins[pinIdx].link.getConnectedPin()) + { + constexpr int gpsCyclePrecision = 3; + constexpr int gpsTimePrecision = 12; + constexpr int valuePrecision = 15; + + auto nodeData = queue.extract_front(); + + // -------------------------------------------------------- Time ----------------------------------------------------------- + auto wifiObs = std::static_pointer_cast(nodeData); + for (auto const& obs : wifiObs->data) + { + if (!obs.time.empty()) + { + _filestream << std::setprecision(valuePrecision) << std::round(calcTimeIntoRun(obs.time) * 1e9) / 1e9; + } + _filestream << ","; + if (!obs.time.empty()) + { + _filestream << std::fixed << std::setprecision(gpsCyclePrecision) << obs.time.toGPSweekTow().gpsCycle; + } + _filestream << ","; + if (!obs.time.empty()) + { + _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs.time.toGPSweekTow().gpsWeek; + } + _filestream << ","; + if (!obs.time.empty()) + { + _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs.time.toGPSweekTow().tow; + } + _filestream << "," << std::setprecision(valuePrecision); + + // { + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.year), sizeof(obs->timeOutputs->timeUtc.year)); + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.month), sizeof(obs->timeOutputs->timeUtc.month)); + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.day), sizeof(obs->timeOutputs->timeUtc.day)); + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.hour), sizeof(obs->timeOutputs->timeUtc.hour)); + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.min), sizeof(obs->timeOutputs->timeUtc.min)); + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.sec), sizeof(obs->timeOutputs->timeUtc.sec)); + // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.ms), sizeof(obs->timeOutputs->timeUtc.ms)); + // } + + // ------------------------------------------------------ MacAddress ---------------------------------------------------------- + if (!obs.macAddress.empty()) + { + _filestream << obs.macAddress; + } + else + { + _filestream << ","; + } + _filestream << ","; + + // ------------------------------------------------------- Distance ----------------------------------------------------------- + if (!(obs.distance < 0.0)) + { + _filestream << obs.distance; + } + else + { + _filestream << ","; + } + } + _filestream << std::endl; + } +} diff --git a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp new file mode 100644 index 000000000..ac3c27c96 --- /dev/null +++ b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp @@ -0,0 +1,77 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file WiFiObsLogger.hpp +/// @brief Data Logger for WiFi observations +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @date 2024-03-14 + +#pragma once + +#include "internal/Node/Node.hpp" +#include "Nodes/DataLogger/Protocol/FileWriter.hpp" +#include "Nodes/DataLogger/Protocol/CommonLog.hpp" + +namespace NAV +{ +class NodeData; + +/// Data Logger for WiFiObs observations +class WiFiObsLogger : public Node, public FileWriter, public CommonLog +{ + public: + /// @brief Default constructor + WiFiObsLogger(); + /// @brief Destructor + ~WiFiObsLogger() override; + /// @brief Copy constructor + WiFiObsLogger(const WiFiObsLogger&) = delete; + /// @brief Move constructor + WiFiObsLogger(WiFiObsLogger&&) = delete; + /// @brief Copy assignment operator + WiFiObsLogger& operator=(const WiFiObsLogger&) = delete; + /// @brief Move assignment operator + WiFiObsLogger& operator=(WiFiObsLogger&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + /// @brief Function called by the flow executer after finishing to flush out remaining data + void flush() override; + + private: + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Write Observation to the file + /// @param[in] queue Queue with all the received data messages + /// @param[in] pinIdx Index of the pin the data is received on + void writeObservation(InputPin::NodeDataQueue& queue, size_t pinIdx); +}; + +} // namespace NAV diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index f6ff78efd..a10cab808 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -563,6 +563,7 @@ bool NAV::WiFiPositioning::initialize() _kalmanFilter.P.diagonal() << variance_pos, variance_vel; _kalmanFilter.x << _state.e_position, _state.e_velocity; + std::cout << _kalmanFilter.x << std::endl; if (_measurementNoiseUnit == MeasurementNoiseUnit::meter2) { _kalmanFilter.R << _measurementNoise; @@ -751,6 +752,8 @@ void NAV::WiFiPositioning::kfSolution() F(1, 4) = 1; F(2, 5) = 1; _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, 1); + std::cout << F << std::endl; + std::cout << _kalmanFilter.Phi << std::endl; // Process noise covariance matrix Eigen::Matrix3d Q1 = Eigen::Matrix3d::Zero(); Q1.diagonal() = Eigen::Vector3d(std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0); @@ -768,6 +771,9 @@ void NAV::WiFiPositioning::kfSolution() _kalmanFilter.Q *= std::pow(_processNoise, 2); } // Predict + std::cout << _kalmanFilter.Q << std::endl; + std::cout << _kalmanFilter.x << std::endl; + std::cout << _kalmanFilter.P << std::endl; _kalmanFilter.predict(); } @@ -782,6 +788,11 @@ void NAV::WiFiPositioning::kfSolution() H.block<1, 3>(0, 0) = -e_calcLineOfSightUnitVector(_kalmanFilter.x.block<3, 1>(0, 0), _devices.at(0).position); _kalmanFilter.H << H; // Correct + std::cout << _kalmanFilter.Q << std::endl; + std::cout << _kalmanFilter.x << std::endl; + std::cout << _kalmanFilter.P << std::endl; + std::cout << _kalmanFilter.z << std::endl; + std::cout << _kalmanFilter.H << std::endl; _kalmanFilter.correctWithMeasurementInnovation(); _devices.clear(); diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp b/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp similarity index 78% rename from src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp rename to src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp index 35306ec20..ccc5f142b 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.cpp +++ b/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp @@ -168,54 +168,6 @@ std::shared_ptr NAV::EspressifFile::pollData() return obs; } -// std::shared_ptr NAV::EspressifFile::pollData() -// { -// auto filestream = std::ifstream(getFilepath(), std::ios::binary); // TODO -// uint8_t i = 0; -// std::unique_ptr packet = nullptr; -// // while (filestream.read(reinterpret_cast(&i), 1)) -// while (readsome(reinterpret_cast(&i), 1)) // TODO Wieso funktioniert das nicht? -// { -// packet = _sensor.findPacket(i); - -// if (packet != nullptr) -// { -// break; -// } -// } - -// if (!packet) -// { -// return nullptr; -// } - -// // Check if package is empty -// if (packet->getRawDataLength() == 0) -// { -// return nullptr; -// } - -// auto obs = std::make_shared(); -// vendor::espressif::decryptSingleWiFiObsDeviceTime(obs, *packet); // TODO weitere neben single? - -// if (!obs->insTime.empty()) -// { -// if (util::time::GetMode() == util::time::Mode::REAL_TIME) -// { -// util::time::SetCurrentTime(obs->insTime); -// } -// } -// else if (auto currentTime = util::time::GetCurrentInsTime(); -// !currentTime.empty()) -// { -// obs->insTime = currentTime; -// } - -// invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); -// // filestream.close(); // TODO -// return obs; -// } - NAV::FileReader::FileType NAV::EspressifFile::determineFileType() { LOG_TRACE("called for {}", nameId()); diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp b/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp similarity index 99% rename from src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp rename to src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp index f7577d924..b9821309a 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifFile.hpp +++ b/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp @@ -10,7 +10,7 @@ /// @brief File Reader for ubx files /// @author R. Lintz (r-lintz@gmx.de) (master thesis) /// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2024-01-08 +/// @date 2024-03-07 #pragma once diff --git a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp new file mode 100644 index 000000000..ab06aede4 --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp @@ -0,0 +1,253 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "WiFiObsFile.hpp" + +#include "util/Logger.hpp" + +#include "util/Time/TimeBase.hpp" + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +#include "NodeData/WiFi/WiFiObs.hpp" + +NAV::WiFiObsFile::WiFiObsFile() + : Node(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _hasConfig = true; + _guiConfigDefaultWindowSize = { 488, 248 }; + + nm::CreateOutputPin(this, "WiFiObs", Pin::Type::Flow, { NAV::WiFiObs::type() }, &WiFiObsFile::pollData); + // nm::CreateOutputPin(this, "Header Columns", Pin::Type::Object, { "std::vector" }, &_headerColumns); +} + +NAV::WiFiObsFile::~WiFiObsFile() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::WiFiObsFile::typeStatic() +{ + return "WiFiObsFile"; +} + +std::string NAV::WiFiObsFile::type() const +{ + return typeStatic(); +} + +std::string NAV::WiFiObsFile::category() +{ + return "Data Provider"; +} + +void NAV::WiFiObsFile::guiConfig() +{ + if (auto res = FileReader::guiConfig(".csv,.*", { ".csv" }, size_t(id), nameId())) + { + LOG_DEBUG("{}: Path changed to {}", nameId(), _path); + flow::ApplyChanges(); + if (res == FileReader::PATH_CHANGED) + { + doReinitialize(); + } + else + { + doDeinitialize(); + } + } + + // // Header info + // if (ImGui::BeginTable(fmt::format("##PvaHeaders ({})", id.AsPointer()).c_str(), 4, + // ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg)) + // { + // ImGui::TableSetupColumn("Time", ImGuiTableColumnFlags_WidthFixed); + // ImGui::TableSetupColumn("Position", ImGuiTableColumnFlags_WidthFixed); + // ImGui::TableSetupColumn("Velocity", ImGuiTableColumnFlags_WidthFixed); + // ImGui::TableSetupColumn("Attitude", ImGuiTableColumnFlags_WidthFixed); + // ImGui::TableHeadersRow(); + + // auto TextColoredIfExists = [this](int index, const char* text) { + // ImGui::TableSetColumnIndex(index); + // if (std::find(_headerColumns.begin(), _headerColumns.end(), text) != _headerColumns.end()) + // { + // ImGui::TextUnformatted(text); + // } + // else + // { + // ImGui::TextDisabled("%s", text); + // } + // }; + + // ImGui::TableNextRow(); + // TextColoredIfExists(0, "GpsCycle"); + // TextColoredIfExists(1, "Pos ECEF X [m]"); + // TextColoredIfExists(2, "Vel ECEF X [m/s]"); + // TextColoredIfExists(3, "n_Quat_b w"); + // ImGui::TableNextRow(); + // TextColoredIfExists(0, "GpsWeek"); + // TextColoredIfExists(1, "Pos ECEF Y [m]"); + // TextColoredIfExists(2, "Vel ECEF Y [m/s]"); + // TextColoredIfExists(3, "n_Quat_b x"); + // ImGui::TableNextRow(); + // TextColoredIfExists(0, "GpsToW [s]"); + // TextColoredIfExists(1, "Pos ECEF Z [m]"); + // TextColoredIfExists(2, "Vel ECEF Z [m/s]"); + // TextColoredIfExists(3, "n_Quat_b y"); + // ImGui::TableNextRow(); + // TextColoredIfExists(1, "Latitude [deg]"); + // TextColoredIfExists(2, "Vel N [m/s]"); + // TextColoredIfExists(3, "n_Quat_b z"); + // ImGui::TableNextRow(); + // TextColoredIfExists(1, "Longitude [deg]"); + // TextColoredIfExists(2, "Vel E [m/s]"); + // TextColoredIfExists(3, "Roll [deg]"); + // ImGui::TableNextRow(); + // TextColoredIfExists(1, "Altitude [m]"); + // TextColoredIfExists(2, "Vel D [m/s]"); + // TextColoredIfExists(3, "Pitch [deg]"); + // ImGui::TableNextRow(); + // TextColoredIfExists(3, "Yaw [deg]"); + + // ImGui::EndTable(); + // } +} + +[[nodiscard]] json NAV::WiFiObsFile::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["FileReader"] = FileReader::save(); + + return j; +} + +void NAV::WiFiObsFile::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("FileReader")) + { + FileReader::restore(j.at("FileReader")); + } +} + +bool NAV::WiFiObsFile::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + return FileReader::initialize(); +} + +void NAV::WiFiObsFile::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); + + FileReader::deinitialize(); +} + +bool NAV::WiFiObsFile::resetNode() +{ + FileReader::resetReader(); + + return true; +} + +std::shared_ptr NAV::WiFiObsFile::pollData() +{ + auto obs = std::make_shared(); + + // Read line + std::string line; + getline(line); + // Remove any starting non text characters + line.erase(line.begin(), std::find_if(line.begin(), line.end(), [](int ch) { return std::isgraph(ch); })); + + if (line.empty()) + { + return nullptr; + } + + // Convert line into stream + std::stringstream lineStream(line); + std::string cell; + + uint16_t gpsCycle = 0; + uint16_t gpsWeek; + long double gpsToW; + InsTime time; + std::string macAddress; + double distance; + + bool gpsCycleSet = false; + bool gpsWeekSet = false; + bool gpsToWSet = false; + bool macAddressSet = false; + bool distanceSet = false; + + // Split line at comma + for (const auto& column : _headerColumns) + { + if (std::getline(lineStream, cell, ',')) + { + // Remove any trailing non text characters + cell.erase(std::find_if(cell.begin(), cell.end(), [](int ch) { return std::iscntrl(ch); }), cell.end()); + if (cell.empty()) + { + continue; + } + + if (column == "GpsCycle") + { + gpsCycle = static_cast(std::stoul(cell)); + gpsCycleSet = true; + } + else if (column == "GpsWeek") + { + gpsWeek = static_cast(std::stoul(cell)); + gpsWeekSet = true; + } + else if (column == "GpsToW [s]") + { + gpsToW = std::stold(cell); + gpsToWSet = true; + } + else if (column == "MacAddress") + { + macAddress = cell; + macAddressSet = true; + } + else if (column == "Distance [m]") + { + distance = std::stod(cell); + distanceSet = true; + } + } + } + + if (!gpsCycleSet || !gpsWeekSet || !gpsToWSet || !macAddressSet || !distanceSet) + { + LOG_ERROR("{}: Not all columns are set", nameId()); + return nullptr; + } + else + { + time = InsTime(gpsCycle, gpsWeek, gpsToW); + } + obs->data.push_back({ macAddress, time, distance }); // TODO: Add timeOutputs + obs->insTime = time; + + invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); + return obs; +} \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp new file mode 100644 index 000000000..c196ebbca --- /dev/null +++ b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp @@ -0,0 +1,77 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file WiFiObsFile.hpp +/// @brief WiFiObs File Reader +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2023-05-23 + +#pragma once + +#include "internal/Node/Node.hpp" +#include "Nodes/DataProvider/Protocol/FileReader.hpp" + +namespace NAV +{ +/// File Reader for Imu log files +class WiFiObsFile : public Node, public FileReader +{ + public: + /// @brief Default constructor + WiFiObsFile(); + /// @brief Destructor + ~WiFiObsFile() override; + /// @brief Copy constructor + WiFiObsFile(const WiFiObsFile&) = delete; + /// @brief Move constructor + WiFiObsFile(WiFiObsFile&&) = delete; + /// @brief Copy assignment operator + WiFiObsFile& operator=(const WiFiObsFile&) = delete; + /// @brief Move assignment operator + WiFiObsFile& operator=(WiFiObsFile&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + /// @brief Resets the node. Moves the read cursor to the start + bool resetNode() override; + + private: + constexpr static size_t OUTPUT_PORT_INDEX_WiFiObs_OBS = 0; ///< @brief Flow (WiFiObs) + constexpr static size_t OUTPUT_PORT_INDEX_HEADER_COLUMNS = 1; ///< @brief Object (std::vector) + + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Polls data from the file + /// @return The read observation + [[nodiscard]] std::shared_ptr pollData(); +}; + +} // namespace NAV diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp index 716024dea..97f3fb96b 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -336,7 +336,7 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) InsTime_YMDHMS yearMonthDayHMS(std::stoi(timeStamp1.substr(0, 4)), std::stoi(timeStamp1.substr(5, 2)), std::stoi(timeStamp1.substr(8, 2)), std::stoi(timeStamp2.substr(0, 2)), std::stoi(timeStamp2.substr(3, 2)), std::stoi(timeStamp2.substr(6, 2))); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase - obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); + obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); // TODO: Add time outputs } } diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp index d13ea21c8..fbadc6d81 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -30,7 +30,6 @@ NAV::EspressifSensor::EspressifSensor() _guiConfigDefaultWindowSize = { 360, 70 }; // TODO: Update the library to handle different baudrates - // _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_9600); _selectedBaudrate = baudrate2Selection(Baudrate::BAUDRATE_115200); _sensorPort = "/dev/ttyACM0"; diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 50d24e40d..72be9a739 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -14,6 +14,7 @@ #include #include "util/Time/TimeBase.hpp" +#include "util/Vendor/VectorNav/BinaryOutputs/TimeOutputs.hpp" /// Speed of light in air [m/s] constexpr double cAir = 299702547.0; @@ -34,6 +35,10 @@ bool NAV::vendor::espressif::decryptWiFiObs(const std::shared_ptr& InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); [[maybe_unused]] int ms = packet.extractInt32(); + // Time outputs + std::shared_ptr timeOutputs; + timeOutputs->syncInCnt = packet.extractUint32(); + timeOutputs->timeSyncIn = packet.extractUint64(); // Add the measurement to the WiFiObs obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); // Log the measurement details From e37169b56b2e01b9de7d10402e532f5a944b6cf2 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Wed, 20 Mar 2024 17:30:00 +0100 Subject: [PATCH 06/27] Wifiobs without time and add wifisolutionlogger and add syncPin --- src/NodeData/WiFi/WiFiObs.hpp | 19 +- src/NodeRegistry.cpp | 2 + .../Converter/GNSS/UartPacketConverter.cpp | 54 +++- .../Converter/GNSS/UartPacketConverter.hpp | 16 +- src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp | 90 +++---- .../WiFi/WiFiPositioningSolutionLogger.cpp | 246 ++++++++++++++++++ .../WiFi/WiFiPositioningSolutionLogger.hpp | 78 ++++++ .../DataProcessor/WiFi/WiFiPositioning.cpp | 131 +++++----- .../WiFi/FileReader/WiFiObsFile.cpp | 8 +- .../DataProvider/WiFi/Sensors/ArubaSensor.cpp | 7 +- .../Vendor/Espressif/EspressifUtilities.cpp | 21 +- 11 files changed, 506 insertions(+), 166 deletions(-) create mode 100644 src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp create mode 100644 src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp diff --git a/src/NodeData/WiFi/WiFiObs.hpp b/src/NodeData/WiFi/WiFiObs.hpp index 05670eb3c..ba80bed97 100644 --- a/src/NodeData/WiFi/WiFiObs.hpp +++ b/src/NodeData/WiFi/WiFiObs.hpp @@ -39,19 +39,12 @@ class WiFiObs : public NodeData /// Payload length in bytes uint16_t payloadLength = 0; - struct FtmObs - { - /// MAC address of the device - std::string macAddress; - /// Time of the measurement - InsTime time; - /// TODO - // NAV::vendor::vectornav::TimeOutputs timeOutputs; - /// Distance to the device - double distance; - }; - - std::vector data; + /// MAC address of the device + std::string macAddress; + /// Distance to the device + double distance; + /// TODO + NAV::vendor::vectornav::TimeOutputs timeOutputs; }; } // namespace NAV diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index 80398bd26..2f49d4db0 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -191,6 +191,7 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataLogger/State/LcKfInsGnssErrorLogger.hpp" #include "Nodes/DataLogger/State/PosVelAttLogger.hpp" #include "Nodes/DataLogger/WiFi/WiFiObsLogger.hpp" +#include "Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp" // Data Processor #include "Nodes/DataProcessor/ErrorModel/ErrorModel.hpp" #include "Nodes/DataProcessor/GNSS/SinglePointPositioning.hpp" @@ -262,6 +263,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); // Data Processor registerNodeType(); registerNodeType(); diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index 56d3e61f7..4810c67a4 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -21,6 +21,8 @@ namespace nm = NAV::NodeManager; #include "util/Vendor/Emlid/EmlidUtilities.hpp" #include "util/Vendor/Espressif/EspressifUtilities.hpp" +#include "Nodes/DataProvider/IMU/Sensors/VectorNavSensor.hpp" + NAV::UartPacketConverter::UartPacketConverter() : Node(typeStatic()) { @@ -98,19 +100,13 @@ void NAV::UartPacketConverter::guiConfig() flow::ApplyChanges(); } - // if (_outputType == OutputType_WiFiObs) - // { - ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); - if (ImGui::TreeNode(fmt::format("Synchronization Control##{}", size_t(id)).c_str())) + if (_outputType == OutputType_WiFiObs) { - ImGui::TextUnformatted("Contains parameters which allow the timing of the VN-310E to be\n" - "synchronized with external devices."); - if (ImGui::Checkbox(fmt::format("Show SyncIn Pin##{}", size_t(id)).c_str(), &_syncInPin)) { LOG_DEBUG("{}: syncInPin changed to {}", nameId(), _syncInPin); flow::ApplyChanges(); - if (_syncInPin) + if (_syncInPin && (inputPins.size() <= 1)) { nm::CreateInputPin(this, "SyncIn", Pin::Type::Object, { "TimeSync" }); } @@ -119,10 +115,8 @@ void NAV::UartPacketConverter::guiConfig() nm::DeleteInputPin(inputPins.at(INPUT_PORT_INDEX_SYNC_IN)); } } - ImGui::TreePop(); + // Remove the extra flow::ApplyChanges() statement here } - flow::ApplyChanges(); - // } } [[nodiscard]] json NAV::UartPacketConverter::save() const @@ -132,6 +126,7 @@ void NAV::UartPacketConverter::guiConfig() json j; j["outputType"] = _outputType; + j["syncInPin"] = _syncInPin; return j; } @@ -163,6 +158,14 @@ void NAV::UartPacketConverter::restore(json const& j) } } } + if (j.contains("syncInPin")) + { + j.at("syncInPin").get_to(_syncInPin); + if (_syncInPin && inputPins.size() <= 1) + { + nm::CreateInputPin(this, "SyncIn", Pin::Type::Object, { "TimeSync" }); + } + } } bool NAV::UartPacketConverter::initialize() @@ -172,11 +175,13 @@ bool NAV::UartPacketConverter::initialize() return true; } -void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, [[maybe_unused]] size_t pinIdx) { auto uartPacket = std::static_pointer_cast(queue.extract_front()); + // auto timeSyncMaster = std::static_pointer_cast(queue.extract_front()); - std::shared_ptr convertedData = nullptr; + std::shared_ptr + convertedData = nullptr; if (_outputType == OutputType_UbloxObs) { @@ -189,7 +194,30 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, s { auto obs = std::make_shared(); auto packet = uartPacket->raw; + vendor::espressif::decryptWiFiObs(obs, packet, nameId()); + if (_syncInPin && inputPins.at(1).isPinLinked()) + { + auto timeSyncMaster = getInputValue(1); + if (_lastSyncInCnt > obs->timeOutputs.syncInCnt) // reset of the slave + { + _syncInCntCorr = timeSyncMaster->syncOutCnt; + } + else if (_lastSyncOutCnt > timeSyncMaster->syncOutCnt) // reset of the master + { + _syncOutCntCorr = obs->timeOutputs.syncInCnt; + } + else if (_lastSyncInCnt == 0 && _lastSyncOutCnt > 1) // slave started later + { + _syncInCntCorr = timeSyncMaster->syncOutCnt; + } + int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->syncOutCnt - _syncOutCntCorr; + obs->insTime = timeSyncMaster->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn) + + std::chrono::seconds(syncCntDiff); + LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}", + vnSensor->nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(), + timeSyncMaster->syncOutCnt, obs->timeOutputs->syncInCnt, syncCntDiff); + } convertedData = obs; } else /* if (_outputType == OutputType_EmlidObs) */ diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp index 2fc8575d9..adf48a198 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.hpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.hpp @@ -16,6 +16,7 @@ #include "internal/Node/Node.hpp" +#include "vn/sensors.h" #include "NodeData/General/UartPacket.hpp" #include "NodeData/GNSS/UbloxObs.hpp" #include "NodeData/GNSS/EmlidObs.hpp" @@ -82,8 +83,21 @@ class UartPacketConverter : public Node /// Show the SyncIn Pin bool _syncInPin = false; + /// Last received syncInCnt + int64_t _lastSyncInCnt = 0; + + /// Last received syncOutCnt + int64_t _lastSyncOutCnt = 0; + + /// Corrected SyncOut counter in case of a reset (master) + int64_t _syncOutCntCorr = 0; + + /// Corrected SyncIn counter in case of a reset (slave) + int64_t _syncInCntCorr = 0; + /// @brief Initialize the node - bool initialize() override; + bool + initialize() override; /// @brief Converts the UartPacket to the selected message type /// @param[in] queue Queue with all the received data messages diff --git a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp index f623cd43f..057f15268 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp +++ b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp @@ -124,61 +124,49 @@ void NAV::WiFiObsLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, s auto nodeData = queue.extract_front(); // -------------------------------------------------------- Time ----------------------------------------------------------- - auto wifiObs = std::static_pointer_cast(nodeData); - for (auto const& obs : wifiObs->data) + auto obs = std::static_pointer_cast(nodeData); + if (!obs->insTime.empty()) + { + _filestream << std::setprecision(valuePrecision) << std::round(calcTimeIntoRun(obs->insTime) * 1e9) / 1e9; + } + _filestream << ","; + if (!obs->insTime.empty()) + { + _filestream << std::fixed << std::setprecision(gpsCyclePrecision) << obs->insTime.toGPSweekTow().gpsCycle; + } + _filestream << ","; + if (!obs->insTime.empty()) + { + _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs->insTime.toGPSweekTow().gpsWeek; + } + _filestream << ","; + if (!obs->insTime.empty()) + { + _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs->insTime.toGPSweekTow().tow; + } + _filestream << "," << std::setprecision(valuePrecision); + + // ------------------------------------------------------ MacAddress ---------------------------------------------------------- + if (!obs->macAddress.empty()) + { + _filestream << obs->macAddress; + } + else { - if (!obs.time.empty()) - { - _filestream << std::setprecision(valuePrecision) << std::round(calcTimeIntoRun(obs.time) * 1e9) / 1e9; - } - _filestream << ","; - if (!obs.time.empty()) - { - _filestream << std::fixed << std::setprecision(gpsCyclePrecision) << obs.time.toGPSweekTow().gpsCycle; - } - _filestream << ","; - if (!obs.time.empty()) - { - _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs.time.toGPSweekTow().gpsWeek; - } - _filestream << ","; - if (!obs.time.empty()) - { - _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs.time.toGPSweekTow().tow; - } - _filestream << "," << std::setprecision(valuePrecision); - - // { - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.year), sizeof(obs->timeOutputs->timeUtc.year)); - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.month), sizeof(obs->timeOutputs->timeUtc.month)); - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.day), sizeof(obs->timeOutputs->timeUtc.day)); - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.hour), sizeof(obs->timeOutputs->timeUtc.hour)); - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.min), sizeof(obs->timeOutputs->timeUtc.min)); - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.sec), sizeof(obs->timeOutputs->timeUtc.sec)); - // _filestream.write(reinterpret_cast(&obs->timeOutputs->timeUtc.ms), sizeof(obs->timeOutputs->timeUtc.ms)); - // } - - // ------------------------------------------------------ MacAddress ---------------------------------------------------------- - if (!obs.macAddress.empty()) - { - _filestream << obs.macAddress; - } - else - { - _filestream << ","; - } _filestream << ","; + } + _filestream << ","; - // ------------------------------------------------------- Distance ----------------------------------------------------------- - if (!(obs.distance < 0.0)) - { - _filestream << obs.distance; - } - else - { - _filestream << ","; - } + // ------------------------------------------------------- Distance ----------------------------------------------------------- + if (!(obs->distance < 0.0)) + { + _filestream << obs->distance; } + else + { + _filestream << ","; + } + _filestream << std::endl; } } diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp new file mode 100644 index 000000000..8002ee0e5 --- /dev/null +++ b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp @@ -0,0 +1,246 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "WiFiPositioningSolutionLogger.hpp" + +#include "Navigation/Transformations/Units.hpp" + +#include "util/Logger.hpp" + +#include // std::setprecision + +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/FlowManager.hpp" + +#include "NodeData/WiFi/WiFiPositioningSolution.hpp" + +NAV::WiFiPositioningSolutionLogger::WiFiPositioningSolutionLogger() + : Node(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _fileType = FileType::ASCII; + + _hasConfig = true; + _guiConfigDefaultWindowSize = { 380, 70 }; + + nm::CreateInputPin(this, "writeObservation", Pin::Type::Flow, { WiFiPositioningSolution::type() }, &WiFiPositioningSolutionLogger::writeObservation); +} + +NAV::WiFiPositioningSolutionLogger::~WiFiPositioningSolutionLogger() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::WiFiPositioningSolutionLogger::typeStatic() +{ + return "WiFiPositioningSolutionLogger"; +} + +std::string NAV::WiFiPositioningSolutionLogger::type() const +{ + return typeStatic(); +} + +std::string NAV::WiFiPositioningSolutionLogger::category() +{ + return "Data Logger"; +} + +void NAV::WiFiPositioningSolutionLogger::guiConfig() +{ + if (FileWriter::guiConfig(".csv", { ".csv" }, size_t(id), nameId())) + { + flow::ApplyChanges(); + doDeinitialize(); + } +} + +[[nodiscard]] json NAV::WiFiPositioningSolutionLogger::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + j["FileWriter"] = FileWriter::save(); + + return j; +} + +void NAV::WiFiPositioningSolutionLogger::restore(json const& j) +{ + LOG_TRACE("{}: called", nameId()); + + if (j.contains("FileWriter")) + { + FileWriter::restore(j.at("FileWriter")); + } +} + +void NAV::WiFiPositioningSolutionLogger::flush() +{ + _filestream.flush(); +} + +bool NAV::WiFiPositioningSolutionLogger::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + if (!FileWriter::initialize()) + { + return false; + } + + CommonLog::initialize(); + + _filestream << "Time [s],GpsCycle,GpsWeek,GpsTow [s]," + << "Pos ECEF X [m],Pos ECEF Y [m],Pos ECEF Z [m],Latitude [deg],Longitude [deg],Altitude [m]," + << "North/South [m],East/West [m]," + << "Vel ECEF X [m/s],Vel ECEF Y [m/s],Vel ECEF Z [m/s],Vel N [m/s],Vel E [m/s],Vel D [m/s]," + << "X-ECEF StDev [m],Y-ECEF StDev [m],Z-ECEF StDev [m]," + // << "XY-ECEF StDev [m],XZ-ECEF StDev [m],YZ-ECEF StDev [m]," + << "North StDev [m],East StDev [m],Down StDev [m]," + // << "NE-ECEF StDev [m],ND-ECEF StDev [m],ED-ECEF StDev [m]," + << "X velocity ECEF StDev [m/s],Y velocity ECEF StDev [m/s],Z velocity ECEF StDev [m/s]," + // << "XY velocity StDev [m],XZ velocity StDev [m],YZ velocity StDev [m]," + << "North velocity StDev [m/s],East velocity StDev [m/s],Down velocity StDev [m/s]," + // << "NE velocity StDev [m],ND velocity StDev [m],ED velocity StDev [m]," + << std::endl; + + return true; +} + +void NAV::WiFiPositioningSolutionLogger::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); + + FileWriter::deinitialize(); +} + +void NAV::WiFiPositioningSolutionLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +{ + auto obs = std::static_pointer_cast(queue.extract_front()); + + constexpr int gpsCyclePrecision = 3; + constexpr int gpsTimePrecision = 12; + constexpr int valuePrecision = 12; + + if (!obs->insTime.empty()) + { + _filestream << std::setprecision(valuePrecision) << std::round(calcTimeIntoRun(obs->insTime) * 1e9) / 1e9; + } + _filestream << ","; + if (!obs->insTime.empty()) + { + _filestream << std::fixed << std::setprecision(gpsCyclePrecision) << obs->insTime.toGPSweekTow().gpsCycle; + } + _filestream << ","; + if (!obs->insTime.empty()) + { + _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs->insTime.toGPSweekTow().gpsWeek; + } + _filestream << ","; + if (!obs->insTime.empty()) + { + _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs->insTime.toGPSweekTow().tow; + } + _filestream << "," << std::setprecision(valuePrecision); + + // -------------------------------------------------------- Position ----------------------------------------------------------- + + if (!std::isnan(obs->e_position().x())) { _filestream << obs->e_position().x(); } // Pos ECEF X [m] + _filestream << ","; + if (!std::isnan(obs->e_position().y())) { _filestream << obs->e_position().y(); } // Pos ECEF Y [m] + _filestream << ","; + if (!std::isnan(obs->e_position().z())) { _filestream << obs->e_position().z(); } // Pos ECEF Z [m] + _filestream << ","; + if (!std::isnan(obs->lla_position().x())) { _filestream << rad2deg(obs->lla_position().x()); } // Latitude [deg] + _filestream << ","; + if (!std::isnan(obs->lla_position().y())) { _filestream << rad2deg(obs->lla_position().y()); } // Longitude [deg] + _filestream << ","; + if (!std::isnan(obs->lla_position().z())) { _filestream << obs->lla_position().z(); } // Altitude [m] + _filestream << ","; + if (!std::isnan(obs->lla_position().x()) && !std::isnan(obs->lla_position().y())) + { + auto localPosition = calcLocalPosition(obs->lla_position()); + _filestream << localPosition.northSouth << ","; // North/South [m] + _filestream << localPosition.eastWest << ","; // East/West [m] + } + else + { + _filestream << ",,"; + } + + // -------------------------------------------------------- Velocity ----------------------------------------------------------- + + if (!std::isnan(obs->e_velocity().x())) { _filestream << obs->e_velocity().x(); } // Vel ECEF X [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocity().y())) { _filestream << obs->e_velocity().y(); } // Vel ECEF Y [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocity().z())) { _filestream << obs->e_velocity().z(); } // Vel ECEF Z [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocity().x())) { _filestream << obs->n_velocity().x(); } // Vel N [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocity().y())) { _filestream << obs->n_velocity().y(); } // Vel E [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocity().z())) { _filestream << obs->n_velocity().z(); } // Vel D [m/s] + _filestream << ","; + + // -------------------------------------------------------- Standard Deviation ----------------------------------------------------------- + if (!std::isnan(obs->e_positionStdev()(0))) { _filestream << obs->e_positionStdev()(0); }; // X-ECEF StDev [m] + _filestream << ","; + if (!std::isnan(obs->e_positionStdev()(1))) { _filestream << obs->e_positionStdev()(1); }; // Y-ECEF StDev [m] + _filestream << ","; + if (!std::isnan(obs->e_positionStdev()(2))) { _filestream << obs->e_positionStdev()(2); }; // Z-ECEF StDev [m] + _filestream << ","; + // if (!std::isnan(obs->e_positionStdev()(0, 1))) { _filestream << obs->e_positionStdev()(0, 1); }; // XY-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_positionStdev()(0, 2))) { _filestream << obs->e_positionStdev()(0, 2); }; // XZ-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_positionStdev()(1, 2))) { _filestream << obs->e_positionStdev()(1, 2); }; // YZ-ECEF StDev [m] + // _filestream << ","; + if (!std::isnan(obs->n_positionStdev()(0))) { _filestream << obs->n_positionStdev()(0); }; // North StDev [m] + _filestream << ","; + if (!std::isnan(obs->n_positionStdev()(1))) { _filestream << obs->n_positionStdev()(1); }; // East StDev [m] + _filestream << ","; + if (!std::isnan(obs->n_positionStdev()(2))) { _filestream << obs->n_positionStdev()(2); }; // Down StDev [m] + _filestream << ","; + // if (!std::isnan(obs->n_positionStdev()(0, 1))) { _filestream << obs->n_positionStdev()(0, 1); }; // NE-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_positionStdev()(0, 2))) { _filestream << obs->n_positionStdev()(0, 2); }; // ND-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_positionStdev()(1, 2))) { _filestream << obs->n_positionStdev()(1, 2); }; // ED-ECEF StDev [m] + // _filestream << ","; + if (!std::isnan(obs->e_velocityStdev()(0))) { _filestream << obs->e_velocityStdev()(0); }; // X velocity ECEF StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocityStdev()(1))) { _filestream << obs->e_velocityStdev()(1); }; // Y velocity ECEF StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocityStdev()(2))) { _filestream << obs->e_velocityStdev()(2); }; // Z velocity ECEF StDev [m/s] + _filestream << ","; + // if (!std::isnan(obs->e_velocityStdev()(0, 1))) { _filestream << obs->e_velocityStdev()(0, 1); }; // XY velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_velocityStdev()(0, 2))) { _filestream << obs->e_velocityStdev()(0, 2); }; // XZ velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_velocityStdev()(1, 2))) { _filestream << obs->e_velocityStdev()(1, 2); }; // YZ velocity StDev [m] + // _filestream << ","; + if (!std::isnan(obs->n_velocityStdev()(0))) { _filestream << obs->n_velocityStdev()(0); }; // North velocity StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocityStdev()(1))) { _filestream << obs->n_velocityStdev()(1); }; // East velocity StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocityStdev()(2))) { _filestream << obs->n_velocityStdev()(2); }; // Down velocity StDev [m/s] + _filestream << ","; + // if (!std::isnan(obs->n_velocityStdev()(0, 1))) { _filestream << obs->n_velocityStdev()(0, 1); }; // NE velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_velocityStdev()(0, 2))) { _filestream << obs->n_velocityStdev()(0, 2); }; // ND velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_velocityStdev()(1, 2))) { _filestream << obs->n_velocityStdev()(1, 2); }; // ED velocity StDev [m] + // _filestream << ","; + + _filestream << '\n'; +} \ No newline at end of file diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp new file mode 100644 index 000000000..a82a2889e --- /dev/null +++ b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp @@ -0,0 +1,78 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file WiFiPositioningSolutionLogger.hpp +/// @brief Data Logger for SppSolution observations +/// @author R. Lintz (r-lintz@gmx.de) (master thesis) +/// @author T. Topp (topp@ins.uni-stuttgart.de) +/// @date 2024-03-20 + +#pragma once + +#include "internal/Node/Node.hpp" +#include "Nodes/DataLogger/Protocol/FileWriter.hpp" +#include "Nodes/DataLogger/Protocol/CommonLog.hpp" + +namespace NAV +{ +class NodeData; + +/// Data Logger for SppSolution observations +class WiFiPositioningSolutionLogger : public Node, public FileWriter, public CommonLog +{ + public: + /// @brief Default constructor + WiFiPositioningSolutionLogger(); + /// @brief Destructor + ~WiFiPositioningSolutionLogger() override; + /// @brief Copy constructor + WiFiPositioningSolutionLogger(const WiFiPositioningSolutionLogger&) = delete; + /// @brief Move constructor + WiFiPositioningSolutionLogger(WiFiPositioningSolutionLogger&&) = delete; + /// @brief Copy assignment operator + WiFiPositioningSolutionLogger& operator=(const WiFiPositioningSolutionLogger&) = delete; + /// @brief Move assignment operator + WiFiPositioningSolutionLogger& operator=(WiFiPositioningSolutionLogger&&) = delete; + + /// @brief String representation of the Class Type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the Class Type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the Class Category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + /// @brief Function called by the flow executer after finishing to flush out remaining data + void flush() override; + + private: + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Write Observation to the file + /// @param[in] queue Queue with all the received data messages + /// @param[in] pinIdx Index of the pin the data is received on + void writeObservation(InputPin::NodeDataQueue& queue, size_t pinIdx); +}; + +} // namespace NAV diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index a10cab808..837ba9b66 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -460,9 +460,9 @@ void NAV::WiFiPositioning::restore(json const& j) { LOG_TRACE("{}: called", nameId()); - if (j.contains("nNavInfoPins")) + if (j.contains("nWifiInputPins")) { - j.at("nNavInfoPins").get_to(_nWifiInputPins); + j.at("nWifiInputPins").get_to(_nWifiInputPins); updateNumberOfInputPins(); } if (j.contains("frame")) @@ -563,7 +563,6 @@ bool NAV::WiFiPositioning::initialize() _kalmanFilter.P.diagonal() << variance_pos, variance_vel; _kalmanFilter.x << _state.e_position, _state.e_velocity; - std::cout << _kalmanFilter.x << std::endl; if (_measurementNoiseUnit == MeasurementNoiseUnit::meter2) { _kalmanFilter.R << _measurementNoise; @@ -597,74 +596,70 @@ void NAV::WiFiPositioning::updateNumberOfInputPins() void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) { - auto wifiObs = std::static_pointer_cast(queue.extract_front()); - for (auto const& obs : wifiObs->data) + auto obs = std::static_pointer_cast(queue.extract_front()); + auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs->macAddress); + if (it != _deviceMacAddresses.end()) // Device exists { - auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs.macAddress); - if (it != _deviceMacAddresses.end()) // Device exists - { - // Get the index of the found element - size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); + // Get the index of the found element + size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); - // Check if a device with the same position already exists and update the distance - bool deviceExists = false; - for (auto& device : _devices) + // Check if a device with the same position already exists and update the distance + bool deviceExists = false; + for (auto& device : _devices) + { + if (device.position == _devicePositions.at(index)) { - if (device.position == _devicePositions.at(index)) - { - deviceExists = true; - device.distance = obs.distance * _deviceScale.at(index) + _deviceBias.at(index); - device.time = obs.time; - break; - } + deviceExists = true; + device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index); + device.time = obs->insTime; + break; } + } - // If the device does not exist, add it to the list - if (!deviceExists) + // If the device does not exist, add it to the list + if (!deviceExists) + { + if (_frame == Frame::LLA) { - if (_frame == Frame::LLA) - { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); - } - else if (_frame == Frame::ECEF) - { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); - } - else if (_frame == Frame::ENU) - { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); - } - else if (_frame == Frame::NED) - { - _devices.push_back({ _devicePositions.at(index), obs.time, obs.distance * _deviceScale.at(index) + _deviceBias.at(index) }); - } + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); } - - auto wifiPositioningSolution = std::make_shared(); - wifiPositioningSolution->insTime = obs.time; - if (_solutionMode == SolutionMode::LSQ) + else if (_frame == Frame::ECEF) { - if (_devices.size() == _numOfDevices) - { - LeastSquaresResult lsqSolution = WiFiPositioning::lsqSolution(); - wifiPositioningSolution->setPositionAndStdDev_e(_state.e_position, lsqSolution.variance.cwiseSqrt()); - std::cout << lsqSolution.solution << std::endl; - wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance); - invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); - } + _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); } - else if (_solutionMode == SolutionMode::KF) + else if (_frame == Frame::ENU) { - WiFiPositioning::kfSolution(); - wifiPositioningSolution->setPositionAndStdDev_e(_kalmanFilter.x.block<3, 1>(0, 0), _kalmanFilter.P.block<3, 3>(0, 0).cwiseSqrt()); - wifiPositioningSolution->setVelocityAndStdDev_e(_kalmanFilter.x.block<3, 1>(3, 0), _kalmanFilter.P.block<3, 3>(3, 3).cwiseSqrt()); - wifiPositioningSolution->setCovarianceMatrix(_kalmanFilter.P); - invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); + _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); } + else if (_frame == Frame::NED) + { + _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); + } + } - // print // TODO delete - LOG_DEBUG("{}: Received distance to device {} at position {} with distance {}", nameId(), obs.macAddress, _devicePositions.at(index).transpose(), obs.distance); + auto wifiPositioningSolution = std::make_shared(); + wifiPositioningSolution->insTime = obs->insTime; + if (_solutionMode == SolutionMode::LSQ) + { + if (_devices.size() == _numOfDevices) + { + LeastSquaresResult lsqSolution = WiFiPositioning::lsqSolution(); + wifiPositioningSolution->setPositionAndStdDev_e(_state.e_position, lsqSolution.variance.cwiseSqrt()); + wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance); + invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); + } } + else if (_solutionMode == SolutionMode::KF) + { + WiFiPositioning::kfSolution(); + wifiPositioningSolution->setPositionAndStdDev_e(_kalmanFilter.x.block<3, 1>(0, 0), _kalmanFilter.P.block<3, 3>(0, 0).cwiseSqrt()); + wifiPositioningSolution->setVelocityAndStdDev_e(_kalmanFilter.x.block<3, 1>(3, 0), _kalmanFilter.P.block<3, 3>(3, 3).cwiseSqrt()); + wifiPositioningSolution->setCovarianceMatrix(_kalmanFilter.P); + invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); + } + + // print // TODO delete + LOG_DEBUG("{}: Received distance to device {} at position {} with distance {}", nameId(), obs->macAddress, _devicePositions.at(index).transpose(), obs->distance); } } @@ -752,8 +747,8 @@ void NAV::WiFiPositioning::kfSolution() F(1, 4) = 1; F(2, 5) = 1; _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, 1); - std::cout << F << std::endl; - std::cout << _kalmanFilter.Phi << std::endl; + // std::cout << F << std::endl; // TODO delete + // std::cout << _kalmanFilter.Phi << std::endl; // Process noise covariance matrix Eigen::Matrix3d Q1 = Eigen::Matrix3d::Zero(); Q1.diagonal() = Eigen::Vector3d(std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0); @@ -771,9 +766,9 @@ void NAV::WiFiPositioning::kfSolution() _kalmanFilter.Q *= std::pow(_processNoise, 2); } // Predict - std::cout << _kalmanFilter.Q << std::endl; - std::cout << _kalmanFilter.x << std::endl; - std::cout << _kalmanFilter.P << std::endl; + // std::cout << _kalmanFilter.Q << std::endl; // TODO delete + // std::cout << _kalmanFilter.x << std::endl; + // std::cout << _kalmanFilter.P << std::endl; _kalmanFilter.predict(); } @@ -788,11 +783,11 @@ void NAV::WiFiPositioning::kfSolution() H.block<1, 3>(0, 0) = -e_calcLineOfSightUnitVector(_kalmanFilter.x.block<3, 1>(0, 0), _devices.at(0).position); _kalmanFilter.H << H; // Correct - std::cout << _kalmanFilter.Q << std::endl; - std::cout << _kalmanFilter.x << std::endl; - std::cout << _kalmanFilter.P << std::endl; - std::cout << _kalmanFilter.z << std::endl; - std::cout << _kalmanFilter.H << std::endl; + // std::cout << _kalmanFilter.Q << std::endl; // TODO delete + // std::cout << _kalmanFilter.x << std::endl; + // std::cout << _kalmanFilter.P << std::endl; + // std::cout << _kalmanFilter.z << std::endl; + // std::cout << _kalmanFilter.H << std::endl; _kalmanFilter.correctWithMeasurementInnovation(); _devices.clear(); diff --git a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp index ab06aede4..10a990568 100644 --- a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp +++ b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp @@ -187,8 +187,6 @@ std::shared_ptr NAV::WiFiObsFile::pollData() uint16_t gpsWeek; long double gpsToW; InsTime time; - std::string macAddress; - double distance; bool gpsCycleSet = false; bool gpsWeekSet = false; @@ -225,12 +223,12 @@ std::shared_ptr NAV::WiFiObsFile::pollData() } else if (column == "MacAddress") { - macAddress = cell; + obs->macAddress = cell; macAddressSet = true; } else if (column == "Distance [m]") { - distance = std::stod(cell); + obs->distance = std::stod(cell); distanceSet = true; } } @@ -245,9 +243,7 @@ std::shared_ptr NAV::WiFiObsFile::pollData() { time = InsTime(gpsCycle, gpsWeek, gpsToW); } - obs->data.push_back({ macAddress, time, distance }); // TODO: Add timeOutputs obs->insTime = time; - invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); return obs; } \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp index 97f3fb96b..0e8bda856 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -336,9 +336,10 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) InsTime_YMDHMS yearMonthDayHMS(std::stoi(timeStamp1.substr(0, 4)), std::stoi(timeStamp1.substr(5, 2)), std::stoi(timeStamp1.substr(8, 2)), std::stoi(timeStamp2.substr(0, 2)), std::stoi(timeStamp2.substr(3, 2)), std::stoi(timeStamp2.substr(6, 2))); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase - obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); // TODO: Add time outputs + obs->distance = measuredDistance; + obs->macAddress = macAddress; + obs->insTime = timeOfMeasurement; } + node->invokeCallbacks(OUTPUT_PORT_INDEX_WIFI_OBS, obs); } - - node->invokeCallbacks(OUTPUT_PORT_INDEX_WIFI_OBS, obs); } \ No newline at end of file diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 72be9a739..6c0a0b28c 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -26,24 +26,23 @@ bool NAV::vendor::espressif::decryptWiFiObs(const std::shared_ptr& { obs->payloadLength = packet.extractUint16(); // TODO remove // Mac address - std::string macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); - std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase + obs->macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); + std::transform(obs->macAddress.begin(), obs->macAddress.end(), obs->macAddress.begin(), ::toupper); // Convert to uppercase // Distance int rtt = packet.extractInt32(); // TODO check if ps or ns - double measuredDistance = static_cast(rtt) * cAir * 1e-12 / 2; + obs->distance = static_cast(rtt) * cAir * 1e-12 / 2; // Time of measurement InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); - [[maybe_unused]] int ms = packet.extractInt32(); + [[maybe_unused]] uint32_t microseconds = packet.extractUint32(); + obs->insTime = timeOfMeasurement + std::chrono::microseconds(microseconds); // Time outputs - std::shared_ptr timeOutputs; - timeOutputs->syncInCnt = packet.extractUint32(); - timeOutputs->timeSyncIn = packet.extractUint64(); - // Add the measurement to the WiFiObs - obs->data.push_back({ macAddress, timeOfMeasurement, measuredDistance }); + std::shared_ptr timeOutputs = std::make_shared(); + obs->timeOutputs.syncInCnt = packet.extractUint32(); + obs->timeOutputs.timeSyncIn = packet.extractUint64(); // Log the measurement details - LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", macAddress, measuredDistance); - LOG_DEBUG("WiFiObs mac Address: {}, measured distance: {}, time of measurement: {}", macAddress, measuredDistance, timeOfMeasurement); + LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", obs->macAddress, obs->distance); + LOG_DEBUG("WiFiObs mac Address: {}, measured distance: {}, time of measurement: {}", obs->macAddress, obs->distance, obs->insTime); } else { From 95dd625497000663353484cf743008d9e3899c3f Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Sun, 28 Apr 2024 13:44:44 +0200 Subject: [PATCH 07/27] Temporary final version --- conanfile.txt | 4 +- src/NodeData/WiFi/WiFiObs.hpp | 4 +- src/NodeData/WiFi/WiFiPositioningSolution.hpp | 20 + .../Converter/GNSS/UartPacketConverter.cpp | 2 +- src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp | 18 +- .../WiFi/WiFiPositioningSolutionLogger.cpp | 7 + .../DataProcessor/WiFi/WiFiPositioning.cpp | 395 ++++++++++++------ .../DataProcessor/WiFi/WiFiPositioning.hpp | 57 ++- .../WiFi/FileReader/WiFiObsFile.cpp | 63 +-- .../DataProvider/WiFi/Sensors/ArubaSensor.cpp | 28 +- .../WiFi/Sensors/EspressifSensor.cpp | 8 +- src/Nodes/Plotting/Plot.cpp | 4 + .../Vendor/Espressif/EspressifUtilities.cpp | 4 +- 13 files changed, 401 insertions(+), 213 deletions(-) diff --git a/conanfile.txt b/conanfile.txt index 785b97f17..6aa75d725 100644 --- a/conanfile.txt +++ b/conanfile.txt @@ -1,6 +1,6 @@ [requires] -spdlog/1.12.0 -fmt/[<=10.1.1] +spdlog/1.13.0 +fmt/[<=10.2.1] boost/1.83.0 eigen/3.4.0 catch2/3.4.0 diff --git a/src/NodeData/WiFi/WiFiObs.hpp b/src/NodeData/WiFi/WiFiObs.hpp index ba80bed97..70acb55a4 100644 --- a/src/NodeData/WiFi/WiFiObs.hpp +++ b/src/NodeData/WiFi/WiFiObs.hpp @@ -43,7 +43,9 @@ class WiFiObs : public NodeData std::string macAddress; /// Distance to the device double distance; - /// TODO + /// Standard deviation of the distance + double distanceStd; + /// Time of observation NAV::vendor::vectornav::TimeOutputs timeOutputs; }; diff --git a/src/NodeData/WiFi/WiFiPositioningSolution.hpp b/src/NodeData/WiFi/WiFiPositioningSolution.hpp index 8f76df82a..850844c14 100644 --- a/src/NodeData/WiFi/WiFiPositioningSolution.hpp +++ b/src/NodeData/WiFi/WiFiPositioningSolution.hpp @@ -46,6 +46,12 @@ class WiFiPositioningSolution : public PosVel /// Returns the standard deviation of the position in local navigation frame coordinates in [m] [[nodiscard]] const Eigen::Vector3d& n_positionStdev() const { return _n_positionStdev; } + /// Returns the Bias in [m] + [[nodiscard]] double bias() const { return _bias; } + + /// Returns the standard deviation of the Bias in [m] + [[nodiscard]] double biasStdev() const { return _biasStdev; } + /// Returns the standard deviation of the velocity in [m/s], in earth coordinates [[nodiscard]] const Eigen::Vector3d& e_velocityStdev() const { return _e_velocityStdev; } @@ -70,6 +76,15 @@ class WiFiPositioningSolution : public PosVel _n_positionStdev = (n_Quat_e().toRotationMatrix() * e_PositionCovarianceMatrix * n_Quat_e().conjugate().toRotationMatrix()).diagonal().cwiseSqrt(); } + /// @brief Set the Bias and its standard deviation + /// @param[in] bias New Bias [m] + /// @param[in] biasStdev Standard deviation of Bias [m] + void setBiasAndStdDev(double bias, double biasStdev) + { + _bias = bias; + _biasStdev = biasStdev; + } + /// @brief Set the Velocity in ECEF coordinates and its standard deviation /// @param[in] e_velocity New Velocity in ECEF coordinates [m/s] /// @param[in] e_velocityCovarianceMatrix Covariance matrix of Velocity in earth coordinates [m/s] @@ -108,6 +123,11 @@ class WiFiPositioningSolution : public PosVel /// Standard deviation of Position in local navigation frame coordinates [m] Eigen::Vector3d _n_positionStdev = Eigen::Vector3d::Zero() * std::nan(""); + /// Bias [m] + double _bias = std::nan(""); + /// Standard deviation of Bias [m] + double _biasStdev = std::nan(""); + /// Standard deviation of Velocity in earth coordinates [m/s] Eigen::Vector3d _e_velocityStdev = Eigen::Vector3d::Zero() * std::nan(""); /// Standard deviation of Velocity in navigation coordinates [m/s] diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index 4810c67a4..600a49c6e 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -207,7 +207,7 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, [ { _syncOutCntCorr = obs->timeOutputs.syncInCnt; } - else if (_lastSyncInCnt == 0 && _lastSyncOutCnt > 1) // slave started later + else if (_lastSyncInCnt == 0 && _lastSyncOutCnt > 1) // slave counter started later { _syncInCntCorr = timeSyncMaster->syncOutCnt; } diff --git a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp index 057f15268..b7da3dbb0 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp +++ b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp @@ -101,7 +101,9 @@ bool NAV::WiFiObsLogger::initialize() _filestream << "Time [s],GpsCycle,GpsWeek,GpsToW [s]," << "MacAddress," - << "Distance [m]" << std::endl; + << "Distance [m]," + << "DistanceStd [m]" + << std::endl; return true; } @@ -158,15 +160,11 @@ void NAV::WiFiObsLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, s _filestream << ","; // ------------------------------------------------------- Distance ----------------------------------------------------------- - if (!(obs->distance < 0.0)) - { - _filestream << obs->distance; - } - else - { - _filestream << ","; - } + _filestream << obs->distance; + _filestream << ","; + // --------------------------------------------------------- Standard deviation --------------------------------------------------- + _filestream << obs->distanceStd; _filestream << std::endl; } -} +} \ No newline at end of file diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp index 8002ee0e5..07f86936e 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp +++ b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp @@ -102,6 +102,7 @@ bool NAV::WiFiPositioningSolutionLogger::initialize() _filestream << "Time [s],GpsCycle,GpsWeek,GpsTow [s]," << "Pos ECEF X [m],Pos ECEF Y [m],Pos ECEF Z [m],Latitude [deg],Longitude [deg],Altitude [m]," << "North/South [m],East/West [m]," + << "Bias [m],Bias StDev [m]," << "Vel ECEF X [m/s],Vel ECEF Y [m/s],Vel ECEF Z [m/s],Vel N [m/s],Vel E [m/s],Vel D [m/s]," << "X-ECEF StDev [m],Y-ECEF StDev [m],Z-ECEF StDev [m]," // << "XY-ECEF StDev [m],XZ-ECEF StDev [m],YZ-ECEF StDev [m]," @@ -177,6 +178,12 @@ void NAV::WiFiPositioningSolutionLogger::writeObservation(NAV::InputPin::NodeDat _filestream << ",,"; } + // -------------------------------------------------------- Bias ----------------------------------------------------------- + if (!std::isnan(obs->bias())) { _filestream << obs->bias(); } // Bias [m] + _filestream << ","; + if (!std::isnan(obs->biasStdev())) { _filestream << obs->biasStdev(); } // Bias StDev [m] + _filestream << ","; + // -------------------------------------------------------- Velocity ----------------------------------------------------------- if (!std::isnan(obs->e_velocity().x())) { _filestream << obs->e_velocity().x(); } // Vel ECEF X [m/s] diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index 837ba9b66..234220a63 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -43,7 +43,6 @@ NAV::WiFiPositioning::WiFiPositioning() updateNumberOfInputPins(); - // updateOutputPin(); nm::CreateOutputPin(this, NAV::WiFiPositioningSolution::type().c_str(), Pin::Type::Flow, { NAV::WiFiPositioningSolution::type() }); } @@ -91,18 +90,15 @@ void NAV::WiFiPositioning::guiConfig() ImGui::SetNextItemWidth(250 * gui::NodeEditorApplication::windowFontRatio()); + // ########################################################################################################### + // Frames + // ########################################################################################################### if (_numOfDevices == 0) { - if (ImGui::Combo(fmt::format("Frame##{}", size_t(id)).c_str(), reinterpret_cast(&_frame), "ENU\0NED\0ECEF\0LLA\0\0")) + if (ImGui::Combo(fmt::format("Frame##{}", size_t(id)).c_str(), reinterpret_cast(&_frame), "ECEF\0LLA\0\0")) { switch (_frame) { - case Frame::ENU: - LOG_DEBUG("{}: Frame changed to ENU", nameId()); - break; - case Frame::NED: - LOG_DEBUG("{}: Frame changed to NED", nameId()); - break; case Frame::ECEF: LOG_DEBUG("{}: Frame changed to ECEF", nameId()); break; @@ -115,23 +111,11 @@ void NAV::WiFiPositioning::guiConfig() flow::ApplyChanges(); } - if (ImGui::BeginTable("RouterInput", 6, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) + if (ImGui::BeginTable("AccessPointInput", 6, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) { // Column headers ImGui::TableSetupColumn("MAC address", ImGuiTableColumnFlags_WidthFixed, columnWidth); - if (_frame == Frame::ENU) - { - ImGui::TableSetupColumn("East", ImGuiTableColumnFlags_WidthFixed, columnWidth); - ImGui::TableSetupColumn("North", ImGuiTableColumnFlags_WidthFixed, columnWidth); - ImGui::TableSetupColumn("Up", ImGuiTableColumnFlags_WidthFixed, columnWidth); - } - else if (_frame == Frame::NED) - { - ImGui::TableSetupColumn("North", ImGuiTableColumnFlags_WidthFixed, columnWidth); - ImGui::TableSetupColumn("East", ImGuiTableColumnFlags_WidthFixed, columnWidth); - ImGui::TableSetupColumn("Down", ImGuiTableColumnFlags_WidthFixed, columnWidth); - } - else if (_frame == Frame::ECEF) + if (_frame == Frame::ECEF) { ImGui::TableSetupColumn("X", ImGuiTableColumnFlags_WidthFixed, columnWidth); ImGui::TableSetupColumn("Y", ImGuiTableColumnFlags_WidthFixed, columnWidth); @@ -171,49 +155,7 @@ void NAV::WiFiPositioning::guiConfig() flow::ApplyChanges(); } } - if (_frame == Frame::ENU) - { - ImGui::TableNextColumn(); - ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputEast{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) - { - flow::ApplyChanges(); - } - ImGui::TableNextColumn(); - ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputNorth{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) - { - flow::ApplyChanges(); - } - ImGui::TableNextColumn(); - ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputUp{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) - { - flow::ApplyChanges(); - } - } - else if (_frame == Frame::NED) - { - ImGui::TableNextColumn(); - ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputNorth{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) - { - flow::ApplyChanges(); - } - ImGui::TableNextColumn(); - ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputEast{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) - { - flow::ApplyChanges(); - } - ImGui::TableNextColumn(); - ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputDown{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) - { - flow::ApplyChanges(); - } - } - else if (_frame == Frame::ECEF) + if (_frame == Frame::ECEF) { ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); @@ -306,8 +248,44 @@ void NAV::WiFiPositioning::guiConfig() } } flow::ApplyChanges(); - // updateOutputPin(); + // ########################################################################################################### + // Least Squares + // ########################################################################################################### + if (_solutionMode == SolutionMode::LSQ) + { + ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); + if (ImGui::TreeNode(fmt::format("x0 - Initial State##{}", size_t(id)).c_str())) + { + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble3(fmt::format("Position (m)##{}", "m", + size_t(id)) + .c_str(), + _initialState.e_position.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position); + flow::ApplyChanges(); + } + + if (_estimateBias) + { + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble(fmt::format("Bias (m)##{}", "m", + size_t(id)) + .c_str(), + &_initialState.bias, 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: bias changed to {}", nameId(), _initialState.bias); + flow::ApplyChanges(); + } + } + + ImGui::TreePop(); + } + } + // ########################################################################################################### + // Kalman Filter + // ########################################################################################################### if (_solutionMode == SolutionMode::KF) { // ########################################################################################################### @@ -367,9 +345,9 @@ void NAV::WiFiPositioning::guiConfig() if (ImGui::InputDouble3(fmt::format("Position (m)##{}", "m", size_t(id)) .c_str(), - _state.e_position.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) + _initialState.e_position.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) { - LOG_DEBUG("{}: e_position changed to {}", nameId(), _state.e_position); + LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position); flow::ApplyChanges(); } @@ -383,6 +361,19 @@ void NAV::WiFiPositioning::guiConfig() flow::ApplyChanges(); } + if (_estimateBias) + { + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble(fmt::format("Bias (m)##{}", "m", + size_t(id)) + .c_str(), + &_state.bias, 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: bias changed to {}", nameId(), _state.bias); + flow::ApplyChanges(); + } + } + ImGui::TreePop(); } @@ -423,9 +414,60 @@ void NAV::WiFiPositioning::guiConfig() flow::ApplyChanges(); } + if (_estimateBias) + { + if (gui::widgets::InputDoubleWithUnit(fmt::format("Bias covariance ({})##{}", + _initCovarianceBiasUnit == InitCovarianceBiasUnit::meter2 + ? "Variance σ²" + : "Standard deviation σ", + size_t(id)) + .c_str(), + configWidth, unitWidth, &_initCovarianceBias, reinterpret_cast(&_initCovarianceBiasUnit), "m^2\0" + "m\0\0", + 0, 0, "%.2e", ImGuiInputTextFlags_CharsScientific)) + { + LOG_DEBUG("{}: initCovarianceBias changed to {}", nameId(), _initCovarianceBias); + LOG_DEBUG("{}: initCovarianceBiasUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasUnit)); + flow::ApplyChanges(); + } + } + ImGui::TreePop(); } } + ImGui::Separator(); + // ########################################################################################################### + // Estimate Bias + // ########################################################################################################### + if (ImGui::Checkbox(fmt::format("Estimate Bias##{}", size_t(id)).c_str(), &_estimateBias)) + { + if (_estimateBias) + { + LOG_DEBUG("{}: Estimate Bias changed to Yes", nameId()); + _numStates = 7; + } + else + { + LOG_DEBUG("{}: Estimate Bias changed to No", nameId()); + _numStates = 6; + } + } + flow::ApplyChanges(); + // ########################################################################################################### + // Weighted Solution + // ########################################################################################################### + if (ImGui::Checkbox(fmt::format("Weighted Solution##{}", size_t(id)).c_str(), &_weightedSolution)) + { + if (_weightedSolution) + { + LOG_DEBUG("{}: Weighted Solution changed to Yes", nameId()); + } + else + { + LOG_DEBUG("{}: Weighted Solution changed to No", nameId()); + } + } + flow::ApplyChanges(); } [[nodiscard]] json NAV::WiFiPositioning::save() const @@ -435,7 +477,11 @@ void NAV::WiFiPositioning::guiConfig() json j; j["nWifiInputPins"] = _nWifiInputPins; + j["numStates"] = _numStates; + j["numMeasurements"] = _numMeasurements; j["frame"] = _frame; + j["estimateBias"] = _estimateBias; + j["weightedSolution"] = _weightedSolution; j["deviceMacAddresses"] = _deviceMacAddresses; j["devicePositions"] = _devicePositions; j["deviceBias"] = _deviceBias; @@ -444,10 +490,16 @@ void NAV::WiFiPositioning::guiConfig() j["solutionMode"] = _solutionMode; j["e_position"] = _state.e_position; j["e_velocity"] = _state.e_velocity; + j["bias"] = _state.bias; + j["intialStatePosition"] = _initialState.e_position; + j["initialStateVelocity"] = _initialState.e_velocity; + j["initialStateBias"] = _initialState.bias; j["initCovariancePosition"] = _initCovariancePosition; j["initCovariancePositionUnit"] = _initCovariancePositionUnit; j["initCovarianceVelocity"] = _initCovarianceVelocity; j["initCovarianceVelocityUnit"] = _initCovarianceVelocityUnit; + j["initCovarianceBias"] = _initCovarianceBias; + j["initCovarianceBiasUnit"] = _initCovarianceBiasUnit; j["measurementNoise"] = _measurementNoise; j["measurementNoiseUnit"] = _measurementNoiseUnit; j["processNoise"] = _processNoise; @@ -465,10 +517,26 @@ void NAV::WiFiPositioning::restore(json const& j) j.at("nWifiInputPins").get_to(_nWifiInputPins); updateNumberOfInputPins(); } + if (j.contains("numStates")) + { + j.at("numStates").get_to(_numStates); + } + if (j.contains("numMeasurements")) + { + j.at("numMeasurements").get_to(_numMeasurements); + } if (j.contains("frame")) { j.at("frame").get_to(_frame); } + if (j.contains("estimateBias")) + { + j.at("estimateBias").get_to(_estimateBias); + } + if (j.contains("weightedSolution")) + { + j.at("weightedSolution").get_to(_weightedSolution); + } if (j.contains("deviceMacAddresses")) { j.at("deviceMacAddresses").get_to(_deviceMacAddresses); @@ -501,6 +569,22 @@ void NAV::WiFiPositioning::restore(json const& j) { j.at("e_velocity").get_to(_state.e_velocity); } + if (j.contains("bias")) + { + j.at("bias").get_to(_state.bias); + } + if (j.contains("intialStatePosition")) + { + j.at("intialStatePosition").get_to(_initialState.e_position); + } + if (j.contains("initialStateVelocity")) + { + j.at("initialStateVelocity").get_to(_initialState.e_velocity); + } + if (j.contains("initialStateBias")) + { + j.at("initialStateBias").get_to(_initialState.bias); + } if (j.contains("initCovariancePosition")) { j.at("initCovariancePosition").get_to(_initCovariancePosition); @@ -517,6 +601,14 @@ void NAV::WiFiPositioning::restore(json const& j) { j.at("initCovarianceVelocityUnit").get_to(_initCovarianceVelocityUnit); } + if (j.contains("initCovarianceBias")) + { + j.at("initCovarianceBias").get_to(_initCovarianceBias); + } + if (j.contains("initCovarianceBiasUnit")) + { + j.at("initCovarianceBiasUnit").get_to(_initCovarianceBiasUnit); + } if (j.contains("measurementNoise")) { j.at("measurementNoise").get_to(_measurementNoise); @@ -539,6 +631,16 @@ bool NAV::WiFiPositioning::initialize() { LOG_TRACE("{}: called", nameId()); + _kalmanFilter = KalmanFilter{ _numStates, _numMeasurements }; + + // Initial state + _state.e_position = _initialState.e_position; + _state.e_velocity = _initialState.e_velocity; + if (_estimateBias) + { + _state.bias = _initialState.bias; + } + // Initial Covariance of the velocity in [m²/s²] Eigen::Vector3d variance_vel = Eigen::Vector3d::Zero(); if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2) @@ -561,8 +663,32 @@ bool NAV::WiFiPositioning::initialize() variance_pos = _initCovariancePosition.array().pow(2); } - _kalmanFilter.P.diagonal() << variance_pos, variance_vel; - _kalmanFilter.x << _state.e_position, _state.e_velocity; + // Initial Covariance of the bias in [m²] + double variance_bias = 0.0; + if (_initCovarianceBiasUnit == InitCovarianceBiasUnit::meter2) + { + variance_bias = _initCovarianceBias; + } + else if (_initCovarianceBiasUnit == InitCovarianceBiasUnit::meter) + { + variance_bias = std::pow(_initCovarianceBias, 2); + } + if (_estimateBias) + { + _kalmanFilter.P.diagonal() << variance_pos, variance_vel, variance_bias; + } + else + { + _kalmanFilter.P.diagonal() << variance_pos, variance_vel; + } + if (_estimateBias) + { + _kalmanFilter.x << _state.e_position, _state.e_velocity, _state.bias; + } + else + { + _kalmanFilter.x << _state.e_position, _state.e_velocity; + } if (_measurementNoiseUnit == MeasurementNoiseUnit::meter2) { _kalmanFilter.R << _measurementNoise; @@ -611,6 +737,7 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size { deviceExists = true; device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index); + device.distanceStd = obs->distanceStd * _deviceScale.at(index); device.time = obs->insTime; break; } @@ -621,19 +748,11 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size { if (_frame == Frame::LLA) { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); + _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index), obs->distanceStd * _deviceScale.at(index) }); } else if (_frame == Frame::ECEF) { - _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); - } - else if (_frame == Frame::ENU) - { - _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); - } - else if (_frame == Frame::NED) - { - _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index) }); + _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index), obs->distanceStd * _deviceScale.at(index) }); } } @@ -644,8 +763,12 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size if (_devices.size() == _numOfDevices) { LeastSquaresResult lsqSolution = WiFiPositioning::lsqSolution(); - wifiPositioningSolution->setPositionAndStdDev_e(_state.e_position, lsqSolution.variance.cwiseSqrt()); - wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance); + wifiPositioningSolution->setPositionAndStdDev_e(_state.e_position.block<3, 1>(0, 0), lsqSolution.variance.block<3, 3>(0, 0).cwiseSqrt()); + wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance.block<3, 3>(0, 0)); + if (_estimateBias) + { + wifiPositioningSolution->setBiasAndStdDev(_state.bias, lsqSolution.variance(3, 3)); + } invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); } } @@ -653,12 +776,15 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size { WiFiPositioning::kfSolution(); wifiPositioningSolution->setPositionAndStdDev_e(_kalmanFilter.x.block<3, 1>(0, 0), _kalmanFilter.P.block<3, 3>(0, 0).cwiseSqrt()); + if (_estimateBias) + { + wifiPositioningSolution->setBiasAndStdDev(_kalmanFilter.x(6), _kalmanFilter.P(6, 6)); + } wifiPositioningSolution->setVelocityAndStdDev_e(_kalmanFilter.x.block<3, 1>(3, 0), _kalmanFilter.P.block<3, 3>(3, 3).cwiseSqrt()); wifiPositioningSolution->setCovarianceMatrix(_kalmanFilter.P); invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); } - // print // TODO delete LOG_DEBUG("{}: Received distance to device {} at position {} with distance {}", nameId(), obs->macAddress, _devicePositions.at(index).transpose(), obs->distance); } } @@ -666,31 +792,32 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size NAV::LeastSquaresResult NAV::WiFiPositioning::lsqSolution() { LeastSquaresResult lsq; + int n = (_estimateBias) ? 4 : 3; // Number of unknowns - if (_devices.size() < 4) + if ((_estimateBias && _devices.size() < 5) || (!_estimateBias && _devices.size() < 4)) { - LOG_DEBUG("{}: Received less than 4 observations. Can't compute position", nameId()); + LOG_DEBUG("{}: Received less than {} observations. Can't compute position", nameId(), (_estimateBias ? 5 : 4)); return lsq; } else { LOG_DEBUG("{}: Received {} observations", nameId(), _devices.size()); } - _state.e_position = { 10.0, 10.0, 10.0 }; // TODO Initialwerte - - // calculate the centroid of device positions - Eigen::Vector3d centroid = Eigen::Vector3d::Zero(); - for (const auto& device : _devices) - { - centroid += device.position; - } - centroid /= _devices.size(); - _state.e_position = centroid; - Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(_devices.size()), static_cast(3)); + Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast(_devices.size()), n); + Eigen::MatrixXd W = Eigen::MatrixXd::Identity(static_cast(_devices.size()), static_cast(_devices.size())); Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_devices.size())); size_t numMeasurements = _devices.size(); + if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2))) + { + _state.e_position << _initialState.e_position; + if (_estimateBias) + { + _state.bias = _initialState.bias; + } + } + for (size_t o = 0; o < 10; o++) { LOG_DATA("{}: Iteration {}", nameId(), o); @@ -698,6 +825,10 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: { // calculate the distance between the device and the estimated position double distEst = (_devices.at(i).position - _state.e_position).norm(); + if (_estimateBias) + { + distEst += _state.bias; + } // calculate the residual vector ddist(static_cast(i)) = _devices.at(i).distance - distEst; @@ -708,14 +839,34 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: } // calculate the design matrix e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; + if (_estimateBias) + { + e_H(static_cast(i), 3) = 1; + } + W(static_cast(i), static_cast(i)) = 1 / std::pow(_devices.at(i).distanceStd, 2); } // solve the linear least squares problem - lsq = solveLinearLeastSquaresUncertainties(e_H, ddist); - LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); - LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); + + lsq = solveWeightedLinearLeastSquaresUncertainties(e_H, W, ddist); + + if (_estimateBias) + { + LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2), lsq.solution(3)); + LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); + } + else + { + LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2)); + LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); + } // update the estimated position - _state.e_position += lsq.solution; + _state.e_position += lsq.solution.block<3, 1>(0, 0); + // update the estimated bias + if (_estimateBias) + { + _state.bias += lsq.solution(3); + } bool solInaccurate = lsq.solution.norm() > 1e-3; if (!solInaccurate) // Solution is accurate enough @@ -723,6 +874,7 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: break; } } + _devices.clear(); LOG_DEBUG("{}: Position: {}", nameId(), _state.e_position.transpose()); @@ -742,21 +894,20 @@ void NAV::WiFiPositioning::kfSolution() if (tau_i > 0) { // Transition matrix - Eigen::MatrixXd F = Eigen::MatrixXd::Zero(6, 6); + Eigen::MatrixXd F = Eigen::MatrixXd::Zero(_numStates, _numStates); F(0, 3) = 1; F(1, 4) = 1; F(2, 5) = 1; _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, 1); - // std::cout << F << std::endl; // TODO delete - // std::cout << _kalmanFilter.Phi << std::endl; // Process noise covariance matrix - Eigen::Matrix3d Q1 = Eigen::Matrix3d::Zero(); - Q1.diagonal() = Eigen::Vector3d(std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0, std::pow(tau_i, 3) / 3.0); - Eigen::Matrix3d Q2 = Eigen::Matrix3d::Zero(); - Q2.diagonal() = Eigen::Vector3d(std::pow(tau_i, 2) / 2.0, std::pow(tau_i, 2) / 2.0, std::pow(tau_i, 2) / 2.0); - Eigen::Matrix3d Q4 = Eigen::Matrix3d::Zero(); - Q4.diagonal() = Eigen::Vector3d(tau_i, tau_i, tau_i); - _kalmanFilter.Q << Q1, Q2, Q2, Q4; + _kalmanFilter.Q.block(0, 0, 3, 3) = std::pow(tau_i, 3) / 3.0 * Eigen::Matrix3d::Identity(); + _kalmanFilter.Q.block(3, 0, 3, 3) = std::pow(tau_i, 2) / 2.0 * Eigen::Matrix3d::Identity(); + _kalmanFilter.Q.block(0, 3, 3, 3) = std::pow(tau_i, 2) / 2.0 * Eigen::Matrix3d::Identity(); + _kalmanFilter.Q.block(3, 3, 3, 3) = tau_i * Eigen::Matrix3d::Identity(); + if (_estimateBias) + { + _kalmanFilter.Q(6, 6) = tau_i; + } if (_processNoiseUnit == ProcessNoiseUnit::meter2) { _kalmanFilter.Q *= _processNoise; @@ -766,9 +917,6 @@ void NAV::WiFiPositioning::kfSolution() _kalmanFilter.Q *= std::pow(_processNoise, 2); } // Predict - // std::cout << _kalmanFilter.Q << std::endl; // TODO delete - // std::cout << _kalmanFilter.x << std::endl; - // std::cout << _kalmanFilter.P << std::endl; _kalmanFilter.predict(); } @@ -777,17 +925,26 @@ void NAV::WiFiPositioning::kfSolution() // ########################################################################################################### // Measurement double estimatedDistance = (_devices.at(0).position - _kalmanFilter.x.block<3, 1>(0, 0)).norm(); + if (_estimateBias) + { + estimatedDistance += _kalmanFilter.x(6); + } _kalmanFilter.z << _devices.at(0).distance - estimatedDistance; // Design matrix - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 6); + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, _numStates); H.block<1, 3>(0, 0) = -e_calcLineOfSightUnitVector(_kalmanFilter.x.block<3, 1>(0, 0), _devices.at(0).position); + if (_estimateBias) + { + H(0, 6) = 1; + } _kalmanFilter.H << H; // Correct - // std::cout << _kalmanFilter.Q << std::endl; // TODO delete - // std::cout << _kalmanFilter.x << std::endl; - // std::cout << _kalmanFilter.P << std::endl; - // std::cout << _kalmanFilter.z << std::endl; - // std::cout << _kalmanFilter.H << std::endl; + std::cout << _kalmanFilter.R << std::endl; + if (_weightedSolution) + { + _kalmanFilter.R << std::pow(_devices.at(0).distanceStd, 2); + } + std::cout << _kalmanFilter.R << std::endl; _kalmanFilter.correctWithMeasurementInnovation(); _devices.clear(); diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp index 04fd1b8ec..bdb76f245 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp @@ -68,8 +68,14 @@ class WiFiPositioning : public Node constexpr static size_t INPUT_PORT_INDEX_WIFI_OBS = 0; ///< @brief WiFiObs constexpr static size_t OUTPUT_PORT_INDEX_WIFISOL = 0; ///< @brief WiFiPositioningSolution - /// Kalman Filter representation - States: 3xVel, 3xPos - Measurements: 1xDist - KalmanFilter _kalmanFilter{ 6, 1 }; + /// @brief Number of states + uint8_t _numStates = 6; + + /// @brief Number of measurements + uint8_t _numMeasurements = 1; + + /// Kalman Filter representation - States: 3xVel, 3xPos, (1xBias) - Measurements: 1xDist + KalmanFilter _kalmanFilter{ _numStates, _numMeasurements }; // --------------------------------------------------------------- Gui ----------------------------------------------------------------- @@ -90,10 +96,8 @@ class WiFiPositioning : public Node /// @brief Available Frames enum class Frame : int { - ENU, ///< East-North-Up frame - NED, ///< North-East-Down frame - ECEF, ///< Earth-Centered Earth-Fixed frame // TODO - LLA, ///< Latitude-Longitude-Altitude frame // TODO + ECEF, ///< Earth-Centered Earth-Fixed frame + LLA, ///< Latitude-Longitude-Altitude frame }; /// Frame to calculate the position in Frame _frame = Frame::ECEF; @@ -107,6 +111,12 @@ class WiFiPositioning : public Node /// Solution Mode SolutionMode _solutionMode = SolutionMode::LSQ; + /// Selection of whether the bias will be additionally estimated + bool _estimateBias = false; + + /// Selection of whether the solution will be weighted + bool _weightedSolution = false; + /// @brief State estimated by the positioning algorithm struct State { @@ -114,24 +124,40 @@ class WiFiPositioning : public Node Eigen::Vector3d e_position = Eigen::Vector3d::Zero(); /// Estimated velocity in ECEF frame [m/s] Eigen::Vector3d e_velocity = Eigen::Vector3d::Zero(); + /// Estimated bias [m] + double bias = 0; }; /// State estimated by the algorithm State _state; - std::vector - _deviceMacAddresses; + /// Initial state + State _initialState; + + /// Input of mac addresses + std::vector _deviceMacAddresses; + + /// Input of positions std::vector _devicePositions; + + /// Input of biases std::vector _deviceBias; + + /// Input of scales std::vector _deviceScale; + + /// Number of devices size_t _numOfDevices; + /// @brief Device struct struct Device { Eigen::Vector3d position; InsTime time; double distance; + double distanceStd; }; + /// Devices which are used for the positioning std::vector _devices; /// Time when the last prediction was triggered @@ -204,6 +230,19 @@ class WiFiPositioning : public Node /// GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Variance σ²) Eigen::Vector3d _initCovarianceVelocity{ 10, 10, 10 }; -}; + // ########################################################################################################### + + /// Possible Units for the initial covariance for the bias (standard deviation σ or Variance σ²) + enum class InitCovarianceBiasUnit + { + meter2, ///< Variance [m^2] + meter, ///< Standard deviation [m] + }; + /// Gui selection for the Unit of the initial covariance for the bias + InitCovarianceBiasUnit _initCovarianceBiasUnit = InitCovarianceBiasUnit::meter; + + /// GUI selection of the initial covariance diagonal values for bias (standard deviation σ or Variance σ²) + double _initCovarianceBias = 10; +}; } // namespace NAV \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp index 10a990568..f597d487b 100644 --- a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp +++ b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp @@ -65,61 +65,6 @@ void NAV::WiFiObsFile::guiConfig() doDeinitialize(); } } - - // // Header info - // if (ImGui::BeginTable(fmt::format("##PvaHeaders ({})", id.AsPointer()).c_str(), 4, - // ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg)) - // { - // ImGui::TableSetupColumn("Time", ImGuiTableColumnFlags_WidthFixed); - // ImGui::TableSetupColumn("Position", ImGuiTableColumnFlags_WidthFixed); - // ImGui::TableSetupColumn("Velocity", ImGuiTableColumnFlags_WidthFixed); - // ImGui::TableSetupColumn("Attitude", ImGuiTableColumnFlags_WidthFixed); - // ImGui::TableHeadersRow(); - - // auto TextColoredIfExists = [this](int index, const char* text) { - // ImGui::TableSetColumnIndex(index); - // if (std::find(_headerColumns.begin(), _headerColumns.end(), text) != _headerColumns.end()) - // { - // ImGui::TextUnformatted(text); - // } - // else - // { - // ImGui::TextDisabled("%s", text); - // } - // }; - - // ImGui::TableNextRow(); - // TextColoredIfExists(0, "GpsCycle"); - // TextColoredIfExists(1, "Pos ECEF X [m]"); - // TextColoredIfExists(2, "Vel ECEF X [m/s]"); - // TextColoredIfExists(3, "n_Quat_b w"); - // ImGui::TableNextRow(); - // TextColoredIfExists(0, "GpsWeek"); - // TextColoredIfExists(1, "Pos ECEF Y [m]"); - // TextColoredIfExists(2, "Vel ECEF Y [m/s]"); - // TextColoredIfExists(3, "n_Quat_b x"); - // ImGui::TableNextRow(); - // TextColoredIfExists(0, "GpsToW [s]"); - // TextColoredIfExists(1, "Pos ECEF Z [m]"); - // TextColoredIfExists(2, "Vel ECEF Z [m/s]"); - // TextColoredIfExists(3, "n_Quat_b y"); - // ImGui::TableNextRow(); - // TextColoredIfExists(1, "Latitude [deg]"); - // TextColoredIfExists(2, "Vel N [m/s]"); - // TextColoredIfExists(3, "n_Quat_b z"); - // ImGui::TableNextRow(); - // TextColoredIfExists(1, "Longitude [deg]"); - // TextColoredIfExists(2, "Vel E [m/s]"); - // TextColoredIfExists(3, "Roll [deg]"); - // ImGui::TableNextRow(); - // TextColoredIfExists(1, "Altitude [m]"); - // TextColoredIfExists(2, "Vel D [m/s]"); - // TextColoredIfExists(3, "Pitch [deg]"); - // ImGui::TableNextRow(); - // TextColoredIfExists(3, "Yaw [deg]"); - - // ImGui::EndTable(); - // } } [[nodiscard]] json NAV::WiFiObsFile::save() const @@ -193,6 +138,7 @@ std::shared_ptr NAV::WiFiObsFile::pollData() bool gpsToWSet = false; bool macAddressSet = false; bool distanceSet = false; + bool distanceStdSet = false; // Split line at comma for (const auto& column : _headerColumns) @@ -231,10 +177,15 @@ std::shared_ptr NAV::WiFiObsFile::pollData() obs->distance = std::stod(cell); distanceSet = true; } + else if (column == "DistanceStd [m]") + { + obs->distanceStd = std::stod(cell); + distanceStdSet = true; + } } } - if (!gpsCycleSet || !gpsWeekSet || !gpsToWSet || !macAddressSet || !distanceSet) + if (!gpsCycleSet || !gpsWeekSet || !gpsToWSet || !macAddressSet || !distanceSet || !distanceStdSet) { LOG_ERROR("{}: Not all columns are set", nameId()); return nullptr; diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp index 0e8bda856..a6b054212 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -272,7 +272,7 @@ void NAV::ArubaSensor::deinitialize() _timer.stop(); } - // To Show the Deinitialization in the GUI // TODO Wieso? + // To Show the Deinitialization in the GUI std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } @@ -281,8 +281,6 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) auto* node = static_cast(userData); auto obs = std::make_shared(); - obs->insTime = util::time::GetCurrentInsTime(); - char buffer[1024]; std::string receivedData; size_t bytesRead; @@ -316,30 +314,40 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) while (std::getline(iss, line) && !line.empty()) { std::istringstream lineStream(line); - std::string dummy; + std::string value; std::string macAddress; lineStream >> macAddress; int rtt, rssi, stdValue; lineStream >> rtt >> rssi >> stdValue; - for (int i = 0; i < 17; ++i) - { - lineStream >> dummy; - } + + std::regex timeRegex("\\d{4}-\\d{2}-\\d{2}"); // Time format: YYYY-MM-DD std::string timeStamp1, timeStamp2; lineStream >> timeStamp1; lineStream >> timeStamp2; - + while (lineStream >> value) + { + if (std::regex_match(value, timeRegex)) // Check if the value is a time stamp + { + timeStamp1 = value; + break; + } + } + lineStream >> value; + timeStamp2 = value; double measuredDistance = static_cast(rtt) * 1e-9 / 2 * cAir; + double measuredDistanceStd = static_cast(stdValue) * 1e-9 / 2 * cAir; if (std::regex_match(macAddress, macRegex)) // Check if the MAC address is valid { InsTime_YMDHMS yearMonthDayHMS(std::stoi(timeStamp1.substr(0, 4)), std::stoi(timeStamp1.substr(5, 2)), std::stoi(timeStamp1.substr(8, 2)), std::stoi(timeStamp2.substr(0, 2)), std::stoi(timeStamp2.substr(3, 2)), std::stoi(timeStamp2.substr(6, 2))); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); std::transform(macAddress.begin(), macAddress.end(), macAddress.begin(), ::toupper); // Convert to uppercase obs->distance = measuredDistance; + obs->distanceStd = measuredDistanceStd; obs->macAddress = macAddress; obs->insTime = timeOfMeasurement; + // obs->insTime = util::time::GetCurrentInsTime(); // if you want to use the instinct time instead + node->invokeCallbacks(OUTPUT_PORT_INDEX_WIFI_OBS, obs); } - node->invokeCallbacks(OUTPUT_PORT_INDEX_WIFI_OBS, obs); } } \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp index fbadc6d81..3e99ee6c8 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -125,10 +125,10 @@ void NAV::EspressifSensor::deinitialize() { LOG_TRACE("{}: called", nameId()); - if (!isInitialized()) - { - return; - } + // if (!isInitialized()) // TODO: Sensor is not correctly initialized + // { + // return; + // } if (_sensor->isConnected()) { diff --git a/src/Nodes/Plotting/Plot.cpp b/src/Nodes/Plotting/Plot.cpp index 6a5994e90..87807945d 100644 --- a/src/Nodes/Plotting/Plot.cpp +++ b/src/Nodes/Plotting/Plot.cpp @@ -2251,6 +2251,7 @@ void NAV::Plot::afterCreateLink(OutputPin& startPin, InputPin& endPin) _pinData.at(pinIndex).addPlotDataItem(i++, "X-ECEF [m]"); _pinData.at(pinIndex).addPlotDataItem(i++, "Y-ECEF [m]"); _pinData.at(pinIndex).addPlotDataItem(i++, "Z-ECEF [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Bias [m]"); _pinData.at(pinIndex).addPlotDataItem(i++, "Velocity norm [m/s]"); _pinData.at(pinIndex).addPlotDataItem(i++, "X velocity ECEF [m/s]"); _pinData.at(pinIndex).addPlotDataItem(i++, "Y velocity ECEF [m/s]"); @@ -2271,6 +2272,7 @@ void NAV::Plot::afterCreateLink(OutputPin& startPin, InputPin& endPin) _pinData.at(pinIndex).addPlotDataItem(i++, "NE-ECEF StDev [m]"); _pinData.at(pinIndex).addPlotDataItem(i++, "ND-ECEF StDev [m]"); _pinData.at(pinIndex).addPlotDataItem(i++, "ED-ECEF StDev [m]"); + _pinData.at(pinIndex).addPlotDataItem(i++, "Bias StDev [m]"); _pinData.at(pinIndex).addPlotDataItem(i++, "X velocity ECEF StDev [m/s]"); _pinData.at(pinIndex).addPlotDataItem(i++, "Y velocity ECEF StDev [m/s]"); _pinData.at(pinIndex).addPlotDataItem(i++, "Z velocity ECEF StDev [m/s]"); @@ -3641,6 +3643,7 @@ void NAV::Plot::plotWiFiPositioningSolution(const std::shared_ptre_position().x()); addData(pinIndex, i++, obs->e_position().y()); addData(pinIndex, i++, obs->e_position().z()); + addData(pinIndex, i++, obs->bias()); addData(pinIndex, i++, obs->e_velocity().norm()); addData(pinIndex, i++, obs->e_velocity().x()); addData(pinIndex, i++, obs->e_velocity().y()); @@ -3661,6 +3664,7 @@ void NAV::Plot::plotWiFiPositioningSolution(const std::shared_ptrn_CovarianceMatrix()(0, 1)); // NE-ECEF StDev [m] addData(pinIndex, i++, obs->n_CovarianceMatrix()(0, 2)); // ND-ECEF StDev [m] addData(pinIndex, i++, obs->n_CovarianceMatrix()(1, 2)); // ED-ECEF StDev [m] + addData(pinIndex, i++, obs->biasStdev()); // Bias StDev [m] addData(pinIndex, i++, obs->e_velocityStdev()(0)); // X velocity ECEF StDev [m/s] addData(pinIndex, i++, obs->e_velocityStdev()(1)); // Y velocity ECEF StDev [m/s] addData(pinIndex, i++, obs->e_velocityStdev()(2)); // Z velocity ECEF StDev [m/s] diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 6c0a0b28c..2f90c2c10 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -29,8 +29,10 @@ bool NAV::vendor::espressif::decryptWiFiObs(const std::shared_ptr& obs->macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); std::transform(obs->macAddress.begin(), obs->macAddress.end(), obs->macAddress.begin(), ::toupper); // Convert to uppercase // Distance - int rtt = packet.extractInt32(); // TODO check if ps or ns + int rtt = packet.extractInt32(); // Round trip time in picoseconds obs->distance = static_cast(rtt) * cAir * 1e-12 / 2; + int rttStd = packet.extractInt32(); // Standard deviation of the round trip time in picoseconds + obs->distanceStd = static_cast(rttStd) * cAir * 1e-12 / 2; // Time of measurement InsTime_YMDHMS yearMonthDayHMS(packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32(), packet.extractInt32()); InsTime timeOfMeasurement(yearMonthDayHMS, UTC); From 5d97717e20428e344ceb8984fb975c7d764bca7a Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Mon, 13 May 2024 15:31:45 +0200 Subject: [PATCH 08/27] Fixed some bugs --- .../WiFi/WiFiPositioningSolutionLogger.cpp | 281 ++++++++++++------ .../WiFi/WiFiPositioningSolutionLogger.hpp | 3 + .../DataProcessor/WiFi/WiFiPositioning.cpp | 148 +++++++-- .../DataProcessor/WiFi/WiFiPositioning.hpp | 3 + 4 files changed, 323 insertions(+), 112 deletions(-) diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp index 07f86936e..fa412d742 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp +++ b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp @@ -60,6 +60,11 @@ void NAV::WiFiPositioningSolutionLogger::guiConfig() flow::ApplyChanges(); doDeinitialize(); } + + if (ImGui::Checkbox(fmt::format("Log NaN values##{}", nameId()).c_str(), &_logNanValues)) + { + flow::ApplyChanges(); + } } [[nodiscard]] json NAV::WiFiPositioningSolutionLogger::save() const @@ -69,6 +74,7 @@ void NAV::WiFiPositioningSolutionLogger::guiConfig() json j; j["FileWriter"] = FileWriter::save(); + j["logNanValues"] = _logNanValues; return j; } @@ -81,6 +87,10 @@ void NAV::WiFiPositioningSolutionLogger::restore(json const& j) { FileWriter::restore(j.at("FileWriter")); } + if (j.contains("logNanValues")) + { + _logNanValues = j.at("logNanValues"); + } } void NAV::WiFiPositioningSolutionLogger::flush() @@ -153,101 +163,194 @@ void NAV::WiFiPositioningSolutionLogger::writeObservation(NAV::InputPin::NodeDat } _filestream << "," << std::setprecision(valuePrecision); - // -------------------------------------------------------- Position ----------------------------------------------------------- - - if (!std::isnan(obs->e_position().x())) { _filestream << obs->e_position().x(); } // Pos ECEF X [m] - _filestream << ","; - if (!std::isnan(obs->e_position().y())) { _filestream << obs->e_position().y(); } // Pos ECEF Y [m] - _filestream << ","; - if (!std::isnan(obs->e_position().z())) { _filestream << obs->e_position().z(); } // Pos ECEF Z [m] - _filestream << ","; - if (!std::isnan(obs->lla_position().x())) { _filestream << rad2deg(obs->lla_position().x()); } // Latitude [deg] - _filestream << ","; - if (!std::isnan(obs->lla_position().y())) { _filestream << rad2deg(obs->lla_position().y()); } // Longitude [deg] - _filestream << ","; - if (!std::isnan(obs->lla_position().z())) { _filestream << obs->lla_position().z(); } // Altitude [m] - _filestream << ","; - if (!std::isnan(obs->lla_position().x()) && !std::isnan(obs->lla_position().y())) + if (_logNanValues) { + // -------------------------------------------------------- Position ----------------------------------------------------------- + + _filestream << obs->e_position().x(); // Pos ECEF X [m] + _filestream << ","; + _filestream << obs->e_position().y(); // Pos ECEF Y [m] + _filestream << ","; + _filestream << obs->e_position().z(); // Pos ECEF Z [m] + _filestream << ","; + _filestream << rad2deg(obs->lla_position().x()); // Latitude [deg] + _filestream << ","; + _filestream << rad2deg(obs->lla_position().y()); // Longitude [deg] + _filestream << ","; + _filestream << obs->lla_position().z(); // Altitude [m] + _filestream << ","; auto localPosition = calcLocalPosition(obs->lla_position()); _filestream << localPosition.northSouth << ","; // North/South [m] _filestream << localPosition.eastWest << ","; // East/West [m] + + // -------------------------------------------------------- Bias ----------------------------------------------------------- + _filestream << obs->bias(); // Bias [m] + _filestream << ","; + _filestream << obs->biasStdev(); // Bias StDev [m] + _filestream << ","; + // -------------------------------------------------------- Velocity ----------------------------------------------------------- + + _filestream << obs->e_velocity().x(); // Vel ECEF X [m/s] + _filestream << ","; + _filestream << obs->e_velocity().y(); // Vel ECEF Y [m/s] + _filestream << ","; + _filestream << obs->e_velocity().z(); // Vel ECEF Z [m/s] + _filestream << ","; + _filestream << obs->n_velocity().x(); // Vel N [m/s] + _filestream << ","; + _filestream << obs->n_velocity().y(); // Vel E [m/s] + _filestream << ","; + _filestream << obs->n_velocity().z(); // Vel D [m/s] + _filestream << ","; + // -------------------------------------------------------- Standard Deviation ----------------------------------------------------------- + _filestream << obs->e_positionStdev()(0); // X-ECEF StDev [m] + _filestream << ","; + _filestream << obs->e_positionStdev()(1); // Y-ECEF StDev [m] + _filestream << ","; + _filestream << obs->e_positionStdev()(2); // Z-ECEF StDev [m] + _filestream << ","; + // _filestream << obs->e_positionStdev()(0, 1); // XY-ECEF StDev [m] + // _filestream << ","; + // _filestream << obs->e_positionStdev()(0, 2); // XZ-ECEF StDev [m] + // _filestream << ","; + // _filestream << obs->e_positionStdev()(1, 2); // YZ-ECEF StDev [m] + // _filestream << ","; + _filestream << obs->n_positionStdev()(0); // North StDev [m] + _filestream << ","; + _filestream << obs->n_positionStdev()(1); // East StDev [m] + _filestream << ","; + _filestream << obs->n_positionStdev()(2); // Down StDev [m] + _filestream << ","; + // _filestream << obs->n_positionStdev()(0, 1); // NE-ECEF StDev [m] + // _filestream << ","; + // _filestream << obs->n_positionStdev()(0, 2); // ND-ECEF StDev [m] + // _filestream << ","; + // _filestream << obs->n_positionStdev()(1, 2); // ED-ECEF StDev [m] + // _filestream << ","; + _filestream << obs->e_velocityStdev()(0); // X velocity ECEF StDev [m/s] + _filestream << ","; + _filestream << obs->e_velocityStdev()(1); // Y velocity ECEF StDev [m/s] + _filestream << ","; + _filestream << obs->e_velocityStdev()(2); // Z velocity ECEF StDev [m/s] + _filestream << ","; + // _filestream << obs->e_velocityStdev()(0, 1); // XY velocity StDev [m] + // _filestream << ","; + // _filestream << obs->e_velocityStdev()(0, 2); // XZ velocity StDev [m] + // _filestream << ","; + // _filestream << obs->e_velocityStdev()(1, 2); // YZ velocity StDev [m] + // _filestream << ","; + _filestream << obs->n_velocityStdev()(0); // North velocity StDev [m/s] + _filestream << ","; + _filestream << obs->n_velocityStdev()(1); // East velocity StDev [m/s] + _filestream << ","; + _filestream << obs->n_velocityStdev()(2); // Down velocity StDev [m/s] + _filestream << ","; + // _filestream << obs->n_velocityStdev()(0, 1); // NE velocity StDev [m] + // _filestream << ","; + // _filestream << obs->n_velocityStdev()(0, 2); // ND velocity StDev [m] + // _filestream << ","; + // _filestream << obs->n_velocityStdev()(1, 2); // ED velocity StDev [m] + // _filestream << ","; + _filestream << '\n'; } else { - _filestream << ",,"; + // -------------------------------------------------------- Position ----------------------------------------------------------- + + if (!std::isnan(obs->e_position().x())) { _filestream << obs->e_position().x(); } // Pos ECEF X [m] + _filestream << ","; + if (!std::isnan(obs->e_position().y())) { _filestream << obs->e_position().y(); } // Pos ECEF Y [m] + _filestream << ","; + if (!std::isnan(obs->e_position().z())) { _filestream << obs->e_position().z(); } // Pos ECEF Z [m] + _filestream << ","; + if (!std::isnan(obs->lla_position().x())) { _filestream << rad2deg(obs->lla_position().x()); } // Latitude [deg] + _filestream << ","; + if (!std::isnan(obs->lla_position().y())) { _filestream << rad2deg(obs->lla_position().y()); } // Longitude [deg] + _filestream << ","; + if (!std::isnan(obs->lla_position().z())) { _filestream << obs->lla_position().z(); } // Altitude [m] + _filestream << ","; + if (!std::isnan(obs->lla_position().x()) && !std::isnan(obs->lla_position().y())) + { + auto localPosition = calcLocalPosition(obs->lla_position()); + _filestream << localPosition.northSouth << ","; // North/South [m] + _filestream << localPosition.eastWest << ","; // East/West [m] + } + else + { + _filestream << ",,"; + } + + // -------------------------------------------------------- Bias ----------------------------------------------------------- + if (!std::isnan(obs->bias())) { _filestream << obs->bias(); } // Bias [m] + _filestream << ","; + if (!std::isnan(obs->biasStdev())) { _filestream << obs->biasStdev(); } // Bias StDev [m] + _filestream << ","; + + // -------------------------------------------------------- Velocity ----------------------------------------------------------- + + if (!std::isnan(obs->e_velocity().x())) { _filestream << obs->e_velocity().x(); } // Vel ECEF X [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocity().y())) { _filestream << obs->e_velocity().y(); } // Vel ECEF Y [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocity().z())) { _filestream << obs->e_velocity().z(); } // Vel ECEF Z [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocity().x())) { _filestream << obs->n_velocity().x(); } // Vel N [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocity().y())) { _filestream << obs->n_velocity().y(); } // Vel E [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocity().z())) { _filestream << obs->n_velocity().z(); } // Vel D [m/s] + _filestream << ","; + + // -------------------------------------------------------- Standard Deviation ----------------------------------------------------------- + if (!std::isnan(obs->e_positionStdev()(0))) { _filestream << obs->e_positionStdev()(0); }; // X-ECEF StDev [m] + _filestream << ","; + if (!std::isnan(obs->e_positionStdev()(1))) { _filestream << obs->e_positionStdev()(1); }; // Y-ECEF StDev [m] + _filestream << ","; + if (!std::isnan(obs->e_positionStdev()(2))) { _filestream << obs->e_positionStdev()(2); }; // Z-ECEF StDev [m] + _filestream << ","; + // if (!std::isnan(obs->e_positionStdev()(0, 1))) { _filestream << obs->e_positionStdev()(0, 1); }; // XY-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_positionStdev()(0, 2))) { _filestream << obs->e_positionStdev()(0, 2); }; // XZ-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_positionStdev()(1, 2))) { _filestream << obs->e_positionStdev()(1, 2); }; // YZ-ECEF StDev [m] + // _filestream << ","; + if (!std::isnan(obs->n_positionStdev()(0))) { _filestream << obs->n_positionStdev()(0); }; // North StDev [m] + _filestream << ","; + if (!std::isnan(obs->n_positionStdev()(1))) { _filestream << obs->n_positionStdev()(1); }; // East StDev [m] + _filestream << ","; + if (!std::isnan(obs->n_positionStdev()(2))) { _filestream << obs->n_positionStdev()(2); }; // Down StDev [m] + _filestream << ","; + // if (!std::isnan(obs->n_positionStdev()(0, 1))) { _filestream << obs->n_positionStdev()(0, 1); }; // NE-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_positionStdev()(0, 2))) { _filestream << obs->n_positionStdev()(0, 2); }; // ND-ECEF StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_positionStdev()(1, 2))) { _filestream << obs->n_positionStdev()(1, 2); }; // ED-ECEF StDev [m] + // _filestream << ","; + if (!std::isnan(obs->e_velocityStdev()(0))) { _filestream << obs->e_velocityStdev()(0); }; // X velocity ECEF StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocityStdev()(1))) { _filestream << obs->e_velocityStdev()(1); }; // Y velocity ECEF StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->e_velocityStdev()(2))) { _filestream << obs->e_velocityStdev()(2); }; // Z velocity ECEF StDev [m/s] + _filestream << ","; + // if (!std::isnan(obs->e_velocityStdev()(0, 1))) { _filestream << obs->e_velocityStdev()(0, 1); }; // XY velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_velocityStdev()(0, 2))) { _filestream << obs->e_velocityStdev()(0, 2); }; // XZ velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->e_velocityStdev()(1, 2))) { _filestream << obs->e_velocityStdev()(1, 2); }; // YZ velocity StDev [m] + // _filestream << ","; + if (!std::isnan(obs->n_velocityStdev()(0))) { _filestream << obs->n_velocityStdev()(0); }; // North velocity StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocityStdev()(1))) { _filestream << obs->n_velocityStdev()(1); }; // East velocity StDev [m/s] + _filestream << ","; + if (!std::isnan(obs->n_velocityStdev()(2))) { _filestream << obs->n_velocityStdev()(2); }; // Down velocity StDev [m/s] + _filestream << ","; + // if (!std::isnan(obs->n_velocityStdev()(0, 1))) { _filestream << obs->n_velocityStdev()(0, 1); }; // NE velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_velocityStdev()(0, 2))) { _filestream << obs->n_velocityStdev()(0, 2); }; // ND velocity StDev [m] + // _filestream << ","; + // if (!std::isnan(obs->n_velocityStdev()(1, 2))) { _filestream << obs->n_velocityStdev()(1, 2); }; // ED velocity StDev [m] + // _filestream << ","; + + _filestream << '\n'; } - - // -------------------------------------------------------- Bias ----------------------------------------------------------- - if (!std::isnan(obs->bias())) { _filestream << obs->bias(); } // Bias [m] - _filestream << ","; - if (!std::isnan(obs->biasStdev())) { _filestream << obs->biasStdev(); } // Bias StDev [m] - _filestream << ","; - - // -------------------------------------------------------- Velocity ----------------------------------------------------------- - - if (!std::isnan(obs->e_velocity().x())) { _filestream << obs->e_velocity().x(); } // Vel ECEF X [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocity().y())) { _filestream << obs->e_velocity().y(); } // Vel ECEF Y [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocity().z())) { _filestream << obs->e_velocity().z(); } // Vel ECEF Z [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocity().x())) { _filestream << obs->n_velocity().x(); } // Vel N [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocity().y())) { _filestream << obs->n_velocity().y(); } // Vel E [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocity().z())) { _filestream << obs->n_velocity().z(); } // Vel D [m/s] - _filestream << ","; - - // -------------------------------------------------------- Standard Deviation ----------------------------------------------------------- - if (!std::isnan(obs->e_positionStdev()(0))) { _filestream << obs->e_positionStdev()(0); }; // X-ECEF StDev [m] - _filestream << ","; - if (!std::isnan(obs->e_positionStdev()(1))) { _filestream << obs->e_positionStdev()(1); }; // Y-ECEF StDev [m] - _filestream << ","; - if (!std::isnan(obs->e_positionStdev()(2))) { _filestream << obs->e_positionStdev()(2); }; // Z-ECEF StDev [m] - _filestream << ","; - // if (!std::isnan(obs->e_positionStdev()(0, 1))) { _filestream << obs->e_positionStdev()(0, 1); }; // XY-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_positionStdev()(0, 2))) { _filestream << obs->e_positionStdev()(0, 2); }; // XZ-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_positionStdev()(1, 2))) { _filestream << obs->e_positionStdev()(1, 2); }; // YZ-ECEF StDev [m] - // _filestream << ","; - if (!std::isnan(obs->n_positionStdev()(0))) { _filestream << obs->n_positionStdev()(0); }; // North StDev [m] - _filestream << ","; - if (!std::isnan(obs->n_positionStdev()(1))) { _filestream << obs->n_positionStdev()(1); }; // East StDev [m] - _filestream << ","; - if (!std::isnan(obs->n_positionStdev()(2))) { _filestream << obs->n_positionStdev()(2); }; // Down StDev [m] - _filestream << ","; - // if (!std::isnan(obs->n_positionStdev()(0, 1))) { _filestream << obs->n_positionStdev()(0, 1); }; // NE-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_positionStdev()(0, 2))) { _filestream << obs->n_positionStdev()(0, 2); }; // ND-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_positionStdev()(1, 2))) { _filestream << obs->n_positionStdev()(1, 2); }; // ED-ECEF StDev [m] - // _filestream << ","; - if (!std::isnan(obs->e_velocityStdev()(0))) { _filestream << obs->e_velocityStdev()(0); }; // X velocity ECEF StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocityStdev()(1))) { _filestream << obs->e_velocityStdev()(1); }; // Y velocity ECEF StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocityStdev()(2))) { _filestream << obs->e_velocityStdev()(2); }; // Z velocity ECEF StDev [m/s] - _filestream << ","; - // if (!std::isnan(obs->e_velocityStdev()(0, 1))) { _filestream << obs->e_velocityStdev()(0, 1); }; // XY velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_velocityStdev()(0, 2))) { _filestream << obs->e_velocityStdev()(0, 2); }; // XZ velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_velocityStdev()(1, 2))) { _filestream << obs->e_velocityStdev()(1, 2); }; // YZ velocity StDev [m] - // _filestream << ","; - if (!std::isnan(obs->n_velocityStdev()(0))) { _filestream << obs->n_velocityStdev()(0); }; // North velocity StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocityStdev()(1))) { _filestream << obs->n_velocityStdev()(1); }; // East velocity StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocityStdev()(2))) { _filestream << obs->n_velocityStdev()(2); }; // Down velocity StDev [m/s] - _filestream << ","; - // if (!std::isnan(obs->n_velocityStdev()(0, 1))) { _filestream << obs->n_velocityStdev()(0, 1); }; // NE velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_velocityStdev()(0, 2))) { _filestream << obs->n_velocityStdev()(0, 2); }; // ND velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_velocityStdev()(1, 2))) { _filestream << obs->n_velocityStdev()(1, 2); }; // ED velocity StDev [m] - // _filestream << ","; - - _filestream << '\n'; } \ No newline at end of file diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp index a82a2889e..b7c813d22 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp +++ b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp @@ -69,6 +69,9 @@ class WiFiPositioningSolutionLogger : public Node, public FileWriter, public Com /// @brief Deinitialize the node void deinitialize() override; + /// @brief Selection weather to log nan values or not + bool _logNanValues = false; + /// @brief Write Observation to the file /// @param[in] queue Queue with all the received data messages /// @param[in] pinIdx Index of the pin the data is received on diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index 234220a63..96fed4d0a 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -30,6 +30,7 @@ namespace nm = NAV::NodeManager; #include "NodeData/WiFi/WiFiPositioningSolution.hpp" #include "Navigation/GNSS/Functions.hpp" #include "Navigation/Transformations/CoordinateFrames.hpp" +#include "Navigation/Transformations/Units.hpp" #include "Navigation/Math/LeastSquares.hpp" @@ -257,16 +258,31 @@ void NAV::WiFiPositioning::guiConfig() ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); if (ImGui::TreeNode(fmt::format("x0 - Initial State##{}", size_t(id)).c_str())) { + Eigen::Vector3d llaPos = trafo::ecef2lla_WGS84(_initialState.e_position); + llaPos.block<2, 1>(0, 0) = rad2deg(llaPos.block<2, 1>(0, 0)); + ImGui::SetNextItemWidth(configWidth); - if (ImGui::InputDouble3(fmt::format("Position (m)##{}", "m", + if (ImGui::InputDouble3(fmt::format("Position ECEF (m)##{}", "m", size_t(id)) .c_str(), - _initialState.e_position.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) + _initialState.e_position.data(), "%.4f", ImGuiInputTextFlags_CharsScientific)) { LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position); flow::ApplyChanges(); } + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble3(fmt::format("Position LLA (°,°,m)##{}", "(°,°,m)", + size_t(id)) + .c_str(), + llaPos.data(), "%.8f", ImGuiInputTextFlags_CharsScientific)) + { + llaPos.block<2, 1>(0, 0) = deg2rad(llaPos.block<2, 1>(0, 0)); + _initialState.e_position = trafo::lla2ecef_WGS84(llaPos); + LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position); + flow::ApplyChanges(); + } + if (_estimateBias) { ImGui::SetNextItemWidth(configWidth); @@ -341,16 +357,31 @@ void NAV::WiFiPositioning::guiConfig() ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); if (ImGui::TreeNode(fmt::format("x0 - Initial State##{}", size_t(id)).c_str())) { + Eigen::Vector3d llaPos = trafo::ecef2lla_WGS84(_initialState.e_position); + llaPos.block<2, 1>(0, 0) = rad2deg(llaPos.block<2, 1>(0, 0)); + ImGui::SetNextItemWidth(configWidth); - if (ImGui::InputDouble3(fmt::format("Position (m)##{}", "m", + if (ImGui::InputDouble3(fmt::format("Position ECEF (m)##{}", "m", size_t(id)) .c_str(), - _initialState.e_position.data(), "%.3e", ImGuiInputTextFlags_CharsScientific)) + _initialState.e_position.data(), "%.4f", ImGuiInputTextFlags_CharsScientific)) { LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position); flow::ApplyChanges(); } + ImGui::SetNextItemWidth(configWidth); + if (ImGui::InputDouble3(fmt::format("Position LLA ##{}", "m", + size_t(id)) + .c_str(), + llaPos.data(), "%.8f°", ImGuiInputTextFlags_CharsScientific)) + { + llaPos.block<2, 1>(0, 0) = deg2rad(llaPos.block<2, 1>(0, 0)); + _initialState.e_position = trafo::lla2ecef_WGS84(llaPos); + LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position); + flow::ApplyChanges(); + } + ImGui::SetNextItemWidth(configWidth); if (ImGui::InputDouble3(fmt::format("Velocity (m/s)##{}", "m", size_t(id)) @@ -468,6 +499,25 @@ void NAV::WiFiPositioning::guiConfig() } } flow::ApplyChanges(); + + // ########################################################################################################### + // Use Initial Values + // ########################################################################################################### + if (_solutionMode == SolutionMode::LSQ) + { + if (ImGui::Checkbox(fmt::format("Use Initial Values##{}", size_t(id)).c_str(), &_useInitialValues)) + { + if (_useInitialValues) + { + LOG_DEBUG("{}: Use Initial Values changed to Yes", nameId()); + } + else + { + LOG_DEBUG("{}: Use Initial Values changed to No", nameId()); + } + } + } + flow::ApplyChanges(); } [[nodiscard]] json NAV::WiFiPositioning::save() const @@ -482,6 +532,7 @@ void NAV::WiFiPositioning::guiConfig() j["frame"] = _frame; j["estimateBias"] = _estimateBias; j["weightedSolution"] = _weightedSolution; + j["useInitialValues"] = _useInitialValues; j["deviceMacAddresses"] = _deviceMacAddresses; j["devicePositions"] = _devicePositions; j["deviceBias"] = _deviceBias; @@ -537,6 +588,10 @@ void NAV::WiFiPositioning::restore(json const& j) { j.at("weightedSolution").get_to(_weightedSolution); } + if (j.contains("useInitialValues")) + { + j.at("useInitialValues").get_to(_useInitialValues); + } if (j.contains("deviceMacAddresses")) { j.at("deviceMacAddresses").get_to(_deviceMacAddresses); @@ -724,7 +779,7 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size { auto obs = std::static_pointer_cast(queue.extract_front()); auto it = std::find(_deviceMacAddresses.begin(), _deviceMacAddresses.end(), obs->macAddress); - if (it != _deviceMacAddresses.end()) // Device exists + if (it != _deviceMacAddresses.end()) // Check if the MAC address is in the list { // Get the index of the found element size_t index = static_cast(std::distance(_deviceMacAddresses.begin(), it)); @@ -733,13 +788,30 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size bool deviceExists = false; for (auto& device : _devices) { - if (device.position == _devicePositions.at(index)) + if (_frame == Frame::ECEF) { - deviceExists = true; - device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index); - device.distanceStd = obs->distanceStd * _deviceScale.at(index); - device.time = obs->insTime; - break; + if (device.position == _devicePositions.at(index)) + { + deviceExists = true; + device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index); + device.distanceStd = obs->distanceStd * _deviceScale.at(index); + device.time = obs->insTime; + break; + } + } + else if (_frame == Frame::LLA) + { + Eigen::Vector3d ecefPos = _devicePositions.at(index); + ecefPos.block<2, 1>(0, 0) = deg2rad(ecefPos.block<2, 1>(0, 0)); + ecefPos = trafo::lla2ecef_WGS84(ecefPos); + if (device.position == ecefPos) + { + deviceExists = true; + device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index); + device.distanceStd = obs->distanceStd * _deviceScale.at(index); + device.time = obs->insTime; + break; + } } } @@ -748,7 +820,9 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size { if (_frame == Frame::LLA) { - _devices.push_back({ trafo::lla2ecef_WGS84(_devicePositions.at(index)), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index), obs->distanceStd * _deviceScale.at(index) }); + Eigen::Vector3d llaPos = _devicePositions.at(index); + llaPos.block<2, 1>(0, 0) = deg2rad(llaPos.block<2, 1>(0, 0)); + _devices.push_back({ trafo::lla2ecef_WGS84(llaPos), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index), obs->distanceStd * _deviceScale.at(index) }); } else if (_frame == Frame::ECEF) { @@ -763,7 +837,7 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size if (_devices.size() == _numOfDevices) { LeastSquaresResult lsqSolution = WiFiPositioning::lsqSolution(); - wifiPositioningSolution->setPositionAndStdDev_e(_state.e_position.block<3, 1>(0, 0), lsqSolution.variance.block<3, 3>(0, 0).cwiseSqrt()); + wifiPositioningSolution->setPositionAndStdDev_e(lsqSolution.solution.block<3, 1>(0, 0), lsqSolution.variance.block<3, 3>(0, 0).cwiseSqrt()); wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance.block<3, 3>(0, 0)); if (_estimateBias) { @@ -809,14 +883,14 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_devices.size())); size_t numMeasurements = _devices.size(); - if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2))) - { - _state.e_position << _initialState.e_position; - if (_estimateBias) - { - _state.bias = _initialState.bias; - } - } + // if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2)) || _useInitialValues) + // { + // _state.e_position << _initialState.e_position; + // if (_estimateBias) + // { + // _state.bias = _initialState.bias; + // } + // } for (size_t o = 0; o < 10; o++) { @@ -839,15 +913,27 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: } // calculate the design matrix e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; + if (_estimateBias) { e_H(static_cast(i), 3) = 1; } - W(static_cast(i), static_cast(i)) = 1 / std::pow(_devices.at(i).distanceStd, 2); + if (_weightedSolution) + { + W(static_cast(i), static_cast(i)) = 1 / std::pow(_devices.at(i).distanceStd, 2); + // W /= W.sum(); // normalize the weights + } } // solve the linear least squares problem + // std::cout << "e_h" << std::endl; // TODO delete + // std::cout << e_H << std::endl; + // std::cout << "o" << o << std::endl; + // std::cout << ddist << std::endl; + // std::cout << "Gewichtung" << std::endl; + // std::cout << W << std::endl; lsq = solveWeightedLinearLeastSquaresUncertainties(e_H, W, ddist); + // std::cout << lsq.solution << std::endl; // TODO delete if (_estimateBias) { @@ -868,11 +954,27 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: _state.bias += lsq.solution(3); } - bool solInaccurate = lsq.solution.norm() > 1e-3; + bool solInaccurate = pow(lsq.solution.norm(), 2) > 1e-3; if (!solInaccurate) // Solution is accurate enough { + lsq.solution.block<3, 1>(0, 0) = _state.e_position; break; } + if (o == 9) + { + LOG_DEBUG("{}: Solution did not converge", nameId()); + lsq.solution.setConstant(std::numeric_limits::quiet_NaN()); + lsq.variance.setConstant(std::numeric_limits::quiet_NaN()); + if (_estimateBias) + { + _state.bias = std::numeric_limits::quiet_NaN(); + } + _state.e_position = _initialState.e_position; + if (_estimateBias) + { + _state.bias = _initialState.bias; + } + } } _devices.clear(); diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp index bdb76f245..432a940ce 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.hpp @@ -117,6 +117,9 @@ class WiFiPositioning : public Node /// Selection of whether the solution will be weighted bool _weightedSolution = false; + /// Selection of whether the initial values should always be used or those of the last position + bool _useInitialValues = false; + /// @brief State estimated by the positioning algorithm struct State { From d595eb9c5ca6d8d36ce407d5d1f06f7608d1a7ec Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Mon, 13 May 2024 16:09:53 +0200 Subject: [PATCH 09/27] Update Readme --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 2d8334d4d..fff32f899 100644 --- a/README.md +++ b/README.md @@ -32,6 +32,7 @@ If no GUI is required, the application can be run in ```--nogui``` mode and a `. ##### Build & run the main program ```shell +sudo apt-get install libssh-dev conan install . --build=missing -s build_type=Release -s compiler.cppstd=20 cmake -Bbuild/Release -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=ON -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=OFF -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=INFO cmake --build build/Release --parallel8 From 5b9b72c83ce2e7a2e54ab2593c8b22a5877eef20 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Tue, 14 May 2024 15:12:41 +0200 Subject: [PATCH 10/27] Fixed some warnings --- src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp | 2 +- .../DataProcessor/WiFi/WiFiPositioning.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp index b7da3dbb0..200a0fcd8 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp +++ b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.cpp @@ -117,7 +117,7 @@ void NAV::WiFiObsLogger::deinitialize() void NAV::WiFiObsLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, size_t pinIdx) { - if (auto* sourcePin = inputPins[pinIdx].link.getConnectedPin()) + if ([[maybe_unused]] auto* sourcePin = inputPins[pinIdx].link.getConnectedPin()) { constexpr int gpsCyclePrecision = 3; constexpr int gpsTimePrecision = 12; diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index 96fed4d0a..dd80b554e 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -143,7 +143,7 @@ void NAV::WiFiPositioning::guiConfig() std::regex macRegex("^([0-9A-Fa-f]{2}[:-]){5}([0-9A-Fa-f]{2})$"); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputText(fmt::format("##Mac{}", size_t(rowIndex)).c_str(), &_deviceMacAddresses.at(rowIndex), ImGuiInputTextFlags_None)) + if (ImGui::InputText(fmt::format("##Mac{}", rowIndex).c_str(), &_deviceMacAddresses.at(rowIndex), ImGuiInputTextFlags_None)) { std::transform(_deviceMacAddresses.at(rowIndex).begin(), _deviceMacAddresses.at(rowIndex).end(), _deviceMacAddresses.at(rowIndex).begin(), ::toupper); // Convert to uppercase if (!std::regex_match(_deviceMacAddresses.at(rowIndex), macRegex)) @@ -160,19 +160,19 @@ void NAV::WiFiPositioning::guiConfig() { ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputX{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputX{}", rowIndex).c_str(), &_devicePositions.at(rowIndex)[0], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputY{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputY{}", rowIndex).c_str(), &_devicePositions.at(rowIndex)[1], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputZ{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputZ{}", rowIndex).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } @@ -181,32 +181,32 @@ void NAV::WiFiPositioning::guiConfig() { ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDoubleL(fmt::format("##InputLat{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[0], -180, 180, 0.0, 0.0, "%.8f°")) + if (ImGui::InputDoubleL(fmt::format("##InputLat{}", rowIndex).c_str(), &_devicePositions.at(rowIndex)[0], -180, 180, 0.0, 0.0, "%.8f°")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDoubleL(fmt::format("##InputLon{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[1], -180, 180, 0.0, 0.0, "%.8f°")) + if (ImGui::InputDoubleL(fmt::format("##InputLon{}", rowIndex).c_str(), &_devicePositions.at(rowIndex)[1], -180, 180, 0.0, 0.0, "%.8f°")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputHeight{}", size_t(rowIndex)).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputHeight{}", rowIndex).c_str(), &_devicePositions.at(rowIndex)[2], 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputBias{}", size_t(rowIndex)).c_str(), &_deviceBias.at(rowIndex), 0.0, 0.0, "%.4fm")) + if (ImGui::InputDouble(fmt::format("##InputBias{}", rowIndex).c_str(), &_deviceBias.at(rowIndex), 0.0, 0.0, "%.4fm")) { flow::ApplyChanges(); } ImGui::TableNextColumn(); ImGui::SetNextItemWidth(columnWidth); - if (ImGui::InputDouble(fmt::format("##InputScale{}", size_t(rowIndex)).c_str(), &_deviceScale.at(rowIndex), 0.0, 0.0, "%.4f")) + if (ImGui::InputDouble(fmt::format("##InputScale{}", rowIndex).c_str(), &_deviceScale.at(rowIndex), 0.0, 0.0, "%.4f")) { flow::ApplyChanges(); } From 63cfb85bc09168a587e7d4d2b356be2747d6a1e8 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Tue, 14 May 2024 15:45:00 +0200 Subject: [PATCH 11/27] More bugs fixed --- src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp | 9 +++------ src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp | 5 ++--- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp index f597d487b..cf566197f 100644 --- a/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp +++ b/src/Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.cpp @@ -129,8 +129,8 @@ std::shared_ptr NAV::WiFiObsFile::pollData() std::string cell; uint16_t gpsCycle = 0; - uint16_t gpsWeek; - long double gpsToW; + uint16_t gpsWeek = 0; + long double gpsToW = 0.0; InsTime time; bool gpsCycleSet = false; @@ -190,10 +190,7 @@ std::shared_ptr NAV::WiFiObsFile::pollData() LOG_ERROR("{}: Not all columns are set", nameId()); return nullptr; } - else - { - time = InsTime(gpsCycle, gpsWeek, gpsToW); - } + time = InsTime(gpsCycle, gpsWeek, gpsToW); obs->insTime = time; invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); return obs; diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp index a6b054212..e9dd34602 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -291,7 +291,7 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) bytesRead = static_cast(ssh_channel_read_timeout(node->_channel, buffer, sizeof(buffer), 0, 10)); // timeout in ms if (bytesRead > 0) { - receivedData.append(buffer, static_cast(bytesRead)); + receivedData.append(buffer, bytesRead); } } while (bytesRead > 0); // Send command to clear output @@ -301,8 +301,7 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) std::istringstream iss(receivedData); std::string line; - while (std::getline(iss, line) && line.find("Peer-bssid") == std::string::npos) - ; // Skip lines until the header "Peer-bssid" is found + while (std::getline(iss, line) && line.find("Peer-bssid") == std::string::npos); // Skip lines until the header "Peer-bssid" is found // Skip the header lines std::getline(iss, line); From f39ac8318bbece1d8d739dba36cf651859108331 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Wed, 15 May 2024 15:36:19 +0200 Subject: [PATCH 12/27] Log level changes --- src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp | 2 +- src/util/Vendor/Espressif/EspressifUtilities.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp index 3e99ee6c8..7570d4021 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -145,7 +145,7 @@ void NAV::EspressifSensor::deinitialize() void NAV::EspressifSensor::binaryAsyncMessageReceived(void* userData, uart::protocol::Packet& p, [[maybe_unused]] size_t index) { - LOG_INFO("binaryAsyncMessageReceived"); + LOG_DATA("binaryAsyncMessageReceived"); auto* ubSensor = static_cast(userData); auto obs = std::make_shared(p); diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index 2f90c2c10..e7166f7a8 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -43,12 +43,11 @@ bool NAV::vendor::espressif::decryptWiFiObs(const std::shared_ptr& obs->timeOutputs.syncInCnt = packet.extractUint32(); obs->timeOutputs.timeSyncIn = packet.extractUint64(); // Log the measurement details - LOG_DATA("WiFiObs mac Address: {}, measured distance: {}", obs->macAddress, obs->distance); - LOG_DEBUG("WiFiObs mac Address: {}, measured distance: {}, time of measurement: {}", obs->macAddress, obs->distance, obs->insTime); + LOG_DATA("WiFiObs mac Address: {}, measured distance: {}, time of measurement: {}", obs->macAddress, obs->distance, obs->insTime); } else { - LOG_DEBUG("Received non-binary packet. Ignoring."); + LOG_DATA("Received non-binary packet. Ignoring."); return false; } return true; From a76194159375fcc05a520f76348bdd117e29d623 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Wed, 15 May 2024 16:11:47 +0200 Subject: [PATCH 13/27] Change securrenttime log leve --- src/util/Time/TimeBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/util/Time/TimeBase.cpp b/src/util/Time/TimeBase.cpp index 9508b7bc3..35617ba4c 100644 --- a/src/util/Time/TimeBase.cpp +++ b/src/util/Time/TimeBase.cpp @@ -76,7 +76,7 @@ void NAV::util::time::SetCurrentTime(const NAV::InsTime& insTime) { if (timeMode == Mode::REAL_TIME) { - LOG_INFO("Updating current Time [{}] to [{} ]", currentExactTime, insTime); + LOG_DATA("Updating current Time [{}] to [{} ]", currentExactTime, insTime); } currentTimeComputer = std::chrono::steady_clock::now(); currentTime = insTime; From 77d30baae5854d1b8bdb6b31a9a63822f3085111 Mon Sep 17 00:00:00 2001 From: rolandlintz Date: Tue, 4 Jun 2024 19:11:02 +0200 Subject: [PATCH 14/27] sync time bug fixed --- src/Nodes/Converter/GNSS/UartPacketConverter.cpp | 16 ++++++++++++---- .../Vendor/Espressif/EspressifUartSensor.cpp | 2 +- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index 600a49c6e..fc06e86ba 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -201,22 +201,30 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, [ auto timeSyncMaster = getInputValue(1); if (_lastSyncInCnt > obs->timeOutputs.syncInCnt) // reset of the slave { + _syncOutCntCorr = 0; _syncInCntCorr = timeSyncMaster->syncOutCnt; } else if (_lastSyncOutCnt > timeSyncMaster->syncOutCnt) // reset of the master { + _syncInCntCorr = 0; _syncOutCntCorr = obs->timeOutputs.syncInCnt; } - else if (_lastSyncInCnt == 0 && _lastSyncOutCnt > 1) // slave counter started later + else if (_lastSyncOutCnt == 0 && timeSyncMaster->syncOutCnt > 1) // slave counter started later { _syncInCntCorr = timeSyncMaster->syncOutCnt; } + else if (_lastSyncOutCnt == 0 && obs->timeOutputs.syncInCnt > 1) // master counter started later + { + _syncOutCntCorr = obs->timeOutputs.syncInCnt; + } + _lastSyncOutCnt = timeSyncMaster->syncOutCnt; + _lastSyncInCnt = obs->timeOutputs.syncInCnt; int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->syncOutCnt - _syncOutCntCorr; obs->insTime = timeSyncMaster->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn) + std::chrono::seconds(syncCntDiff); - LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}", - vnSensor->nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(), - timeSyncMaster->syncOutCnt, obs->timeOutputs->syncInCnt, syncCntDiff); + // LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}, syncInCntCorr {}, syncOutCntCorr {}", + // nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(), + // timeSyncMaster->syncOutCnt, obs->timeOutputs.syncInCnt, syncCntDiff, _syncInCntCorr, _syncOutCntCorr); } convertedData = obs; } diff --git a/src/util/Vendor/Espressif/EspressifUartSensor.cpp b/src/util/Vendor/Espressif/EspressifUartSensor.cpp index 375feb5d6..4d7d319d5 100644 --- a/src/util/Vendor/Espressif/EspressifUartSensor.cpp +++ b/src/util/Vendor/Espressif/EspressifUartSensor.cpp @@ -84,7 +84,7 @@ std::unique_ptr NAV::vendor::espressif::EspressifUartSen _binaryPayloadLength |= static_cast(static_cast(dataByte) << 8U); _binaryPayloadLength = uart::stoh(_binaryPayloadLength, ENDIANNESS); _numOfBytesRemainingForCompletePacket = _binaryPayloadLength + 2U; - LOG_DEBUG("{}: Binary packet: payload length={}", _name, _binaryPayloadLength); + LOG_DATA("{}: Binary packet: payload length={}", _name, _binaryPayloadLength); } else { From f329492038c2b01f07cb59e8c90d9c8d302117e0 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Fri, 7 Jun 2024 12:12:25 +0200 Subject: [PATCH 15/27] Some changes init values lsq --- .../DataProcessor/WiFi/WiFiPositioning.cpp | 21 +++++++++---------- .../Vendor/Espressif/EspressifUtilities.cpp | 2 +- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index dd80b554e..ad94a635e 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -883,16 +883,16 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_devices.size())); size_t numMeasurements = _devices.size(); - // if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2)) || _useInitialValues) - // { - // _state.e_position << _initialState.e_position; - // if (_estimateBias) - // { - // _state.bias = _initialState.bias; - // } - // } + if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2)) || _useInitialValues) + { + _state.e_position << _initialState.e_position; + if (_estimateBias) + { + _state.bias = _initialState.bias; + } + } - for (size_t o = 0; o < 10; o++) + for (size_t o = 0; o < 15; o++) { LOG_DATA("{}: Iteration {}", nameId(), o); for (size_t i = 0; i < numMeasurements; i++) @@ -921,7 +921,6 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: if (_weightedSolution) { W(static_cast(i), static_cast(i)) = 1 / std::pow(_devices.at(i).distanceStd, 2); - // W /= W.sum(); // normalize the weights } } // solve the linear least squares problem @@ -960,7 +959,7 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: lsq.solution.block<3, 1>(0, 0) = _state.e_position; break; } - if (o == 9) + if (o == 14) { LOG_DEBUG("{}: Solution did not converge", nameId()); lsq.solution.setConstant(std::numeric_limits::quiet_NaN()); diff --git a/src/util/Vendor/Espressif/EspressifUtilities.cpp b/src/util/Vendor/Espressif/EspressifUtilities.cpp index e7166f7a8..00c7f9107 100644 --- a/src/util/Vendor/Espressif/EspressifUtilities.cpp +++ b/src/util/Vendor/Espressif/EspressifUtilities.cpp @@ -24,7 +24,7 @@ bool NAV::vendor::espressif::decryptWiFiObs(const std::shared_ptr& obs->insTime = util::time::GetCurrentInsTime(); if (packet.type() == uart::protocol::Packet::Type::TYPE_BINARY) { - obs->payloadLength = packet.extractUint16(); // TODO remove + obs->payloadLength = packet.extractUint16(); // Mac address obs->macAddress = fmt::format("{:02X}:{:02X}:{:02X}:{:02X}:{:02X}:{:02X}", packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8(), packet.extractUint8()); std::transform(obs->macAddress.begin(), obs->macAddress.end(), obs->macAddress.begin(), ::toupper); // Convert to uppercase From 86651f830d43f768031505bafa175f1d5b5b9679 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Fri, 7 Jun 2024 12:16:12 +0200 Subject: [PATCH 16/27] workspace file (should be removed anyway) --- INSTINCT.code-workspace | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/INSTINCT.code-workspace b/INSTINCT.code-workspace index d1da74e32..63e6e7ca8 100644 --- a/INSTINCT.code-workspace +++ b/INSTINCT.code-workspace @@ -53,6 +53,10 @@ "mathover.forwardSkip": 9, "todo-tree.filtering.excludeGlobs": [ "**/lib" - ] + ], + "rainbow_csv.autodetect_separators": [ + "," + ], + "rainbow_csv.comment_prefix": "#" } } \ No newline at end of file From 1ebc58331cd08ae845cf6d011a3d8a09ed3175f1 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Tue, 18 Jun 2024 09:58:47 +0200 Subject: [PATCH 17/27] More comments --- src/NodeRegistry.cpp | 2 - .../DataProcessor/WiFi/WiFiPositioning.cpp | 49 +++-- .../WiFi/FileReader/EspressifFile.cpp | 198 ------------------ .../WiFi/FileReader/EspressifFile.hpp | 88 -------- .../WiFi/Sensors/EspressifSensor.cpp | 5 - 5 files changed, 27 insertions(+), 315 deletions(-) delete mode 100644 src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp delete mode 100644 src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index 2f49d4db0..687a4d67d 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -221,7 +221,6 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataProvider/IMU/FileReader/MultiImuFile.hpp" #include "Nodes/DataProvider/WiFi/Sensors/EspressifSensor.hpp" #include "Nodes/DataProvider/WiFi/Sensors/ArubaSensor.hpp" -#include "Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp" #include "Nodes/DataProvider/WiFi/FileReader/WiFiObsFile.hpp" // Data Simulator #include "Nodes/DataProvider/IMU/Simulators/ImuSimulator.hpp" @@ -293,7 +292,6 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); - registerNodeType(); registerNodeType(); // Data Simulator registerNodeType(); diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index ad94a635e..cf16278fe 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -830,8 +830,10 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size } } + // Calculate the solution auto wifiPositioningSolution = std::make_shared(); wifiPositioningSolution->insTime = obs->insTime; + // Least Squares if (_solutionMode == SolutionMode::LSQ) { if (_devices.size() == _numOfDevices) @@ -846,6 +848,7 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); } } + // Kalman Filter else if (_solutionMode == SolutionMode::KF) { WiFiPositioning::kfSolution(); @@ -868,6 +871,7 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: LeastSquaresResult lsq; int n = (_estimateBias) ? 4 : 3; // Number of unknowns + // Check if the number of devices is sufficient to compute the position if ((_estimateBias && _devices.size() < 5) || (!_estimateBias && _devices.size() < 4)) { LOG_DEBUG("{}: Received less than {} observations. Can't compute position", nameId(), (_estimateBias ? 5 : 4)); @@ -883,6 +887,7 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast(_devices.size())); size_t numMeasurements = _devices.size(); + // Check if the initial position is NaN if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2)) || _useInitialValues) { _state.e_position << _initialState.e_position; @@ -892,18 +897,20 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: } } + // Iteratively solve the linear least squares problem for (size_t o = 0; o < 15; o++) { LOG_DATA("{}: Iteration {}", nameId(), o); for (size_t i = 0; i < numMeasurements; i++) { - // calculate the distance between the device and the estimated position + // Calculate the distance between the device and the estimated position double distEst = (_devices.at(i).position - _state.e_position).norm(); if (_estimateBias) { distEst += _state.bias; } - // calculate the residual vector + + // Calculate the residual vector ddist(static_cast(i)) = _devices.at(i).distance - distEst; Eigen::Vector3d e_lineOfSightUnitVector = Eigen::Vector3d::Zero(); @@ -911,28 +918,22 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: { e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(_state.e_position, _devices.at(i).position); } - // calculate the design matrix - e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; + // Calculate the design matrix + e_H.block<1, 3>(static_cast(i), 0) = -e_lineOfSightUnitVector; if (_estimateBias) { e_H(static_cast(i), 3) = 1; } + + // Calculate the weight matrix if (_weightedSolution) { W(static_cast(i), static_cast(i)) = 1 / std::pow(_devices.at(i).distanceStd, 2); } } - // solve the linear least squares problem - // std::cout << "e_h" << std::endl; // TODO delete - // std::cout << e_H << std::endl; - // std::cout << "o" << o << std::endl; - // std::cout << ddist << std::endl; - // std::cout << "Gewichtung" << std::endl; - // std::cout << W << std::endl; - + // Solve the linear least squares problem lsq = solveWeightedLinearLeastSquaresUncertainties(e_H, W, ddist); - // std::cout << lsq.solution << std::endl; // TODO delete if (_estimateBias) { @@ -945,9 +946,10 @@ NAV::LeastSquaresResult NAV::WiFiPositioning:: LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt()); } - // update the estimated position + // Update the estimated position _state.e_position += lsq.solution.block<3, 1>(0, 0); - // update the estimated bias + + // Update the estimated bias if (_estimateBias) { _state.bias += lsq.solution(3); @@ -991,6 +993,7 @@ void NAV::WiFiPositioning::kfSolution() // ########################################################################################################### // Prediction // ########################################################################################################### + _lastPredictTime = _devices.at(0).time; if (tau_i > 0) { @@ -1000,6 +1003,7 @@ void NAV::WiFiPositioning::kfSolution() F(1, 4) = 1; F(2, 5) = 1; _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, 1); + // Process noise covariance matrix _kalmanFilter.Q.block(0, 0, 3, 3) = std::pow(tau_i, 3) / 3.0 * Eigen::Matrix3d::Identity(); _kalmanFilter.Q.block(3, 0, 3, 3) = std::pow(tau_i, 2) / 2.0 * Eigen::Matrix3d::Identity(); @@ -1024,6 +1028,7 @@ void NAV::WiFiPositioning::kfSolution() // ########################################################################################################### // Update // ########################################################################################################### + // Measurement double estimatedDistance = (_devices.at(0).position - _kalmanFilter.x.block<3, 1>(0, 0)).norm(); if (_estimateBias) @@ -1031,6 +1036,11 @@ void NAV::WiFiPositioning::kfSolution() estimatedDistance += _kalmanFilter.x(6); } _kalmanFilter.z << _devices.at(0).distance - estimatedDistance; + if (_weightedSolution) + { + _kalmanFilter.R << std::pow(_devices.at(0).distanceStd, 2); + } + // Design matrix Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, _numStates); H.block<1, 3>(0, 0) = -e_calcLineOfSightUnitVector(_kalmanFilter.x.block<3, 1>(0, 0), _devices.at(0).position); @@ -1039,13 +1049,8 @@ void NAV::WiFiPositioning::kfSolution() H(0, 6) = 1; } _kalmanFilter.H << H; - // Correct - std::cout << _kalmanFilter.R << std::endl; - if (_weightedSolution) - { - _kalmanFilter.R << std::pow(_devices.at(0).distanceStd, 2); - } - std::cout << _kalmanFilter.R << std::endl; + + // Update _kalmanFilter.correctWithMeasurementInnovation(); _devices.clear(); diff --git a/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp b/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp deleted file mode 100644 index ccc5f142b..000000000 --- a/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.cpp +++ /dev/null @@ -1,198 +0,0 @@ -// This file is part of INSTINCT, the INS Toolkit for Integrated -// Navigation Concepts and Training by the Institute of Navigation of -// the University of Stuttgart, Germany. -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License, v. 2.0. If a copy of the MPL was not distributed with this -// file, You can obtain one at https://mozilla.org/MPL/2.0/. - -#include "EspressifFile.hpp" - -#include "util/Logger.hpp" - -#include "internal/NodeManager.hpp" -namespace nm = NAV::NodeManager; -#include "internal/FlowManager.hpp" - -#include "util/Vendor/Espressif/EspressifUtilities.hpp" -#include "util/Time/TimeBase.hpp" - -#include "NodeData/WiFi/WiFiObs.hpp" - -NAV::EspressifFile::EspressifFile() - : Node(typeStatic()), _sensor(typeStatic()) -{ - LOG_TRACE("{}: called", name); - - _hasConfig = true; - _guiConfigDefaultWindowSize = { 380, 70 }; - - nm::CreateOutputPin(this, "WiFiObs", Pin::Type::Flow, { NAV::WiFiObs::type() }, &EspressifFile::pollData); -} - -NAV::EspressifFile::~EspressifFile() -{ - LOG_TRACE("{}: called", nameId()); -} - -std::string NAV::EspressifFile::typeStatic() -{ - return "EspressifFile"; -} - -std::string NAV::EspressifFile::type() const -{ - return typeStatic(); -} - -std::string NAV::EspressifFile::category() -{ - return "Data Provider"; -} - -void NAV::EspressifFile::guiConfig() -{ - if (auto res = FileReader::guiConfig(".bin,.*", { ".bin" }, size_t(id), nameId())) - { - LOG_DEBUG("{}: Path changed to {}", nameId(), _path); - flow::ApplyChanges(); - if (res == FileReader::PATH_CHANGED) - { - doReinitialize(); - } - else - { - doDeinitialize(); - } - } -} - -[[nodiscard]] json NAV::EspressifFile::save() const -{ - LOG_TRACE("{}: called", nameId()); - - json j; - - j["FileReader"] = FileReader::save(); - - return j; -} - -void NAV::EspressifFile::restore(json const& j) -{ - LOG_TRACE("{}: called", nameId()); - - if (j.contains("FileReader")) - { - FileReader::restore(j.at("FileReader")); - } -} - -bool NAV::EspressifFile::initialize() -{ - LOG_TRACE("{}: called", nameId()); - - return FileReader::initialize(); -} - -void NAV::EspressifFile::deinitialize() -{ - LOG_TRACE("{}: called", nameId()); - - FileReader::deinitialize(); -} - -bool NAV::EspressifFile::resetNode() -{ - FileReader::resetReader(); - - return true; -} - -std::shared_ptr NAV::EspressifFile::pollData() -{ - uint8_t i = 0; - std::unique_ptr packet = nullptr; - std::shared_ptr obs; - while (true) - { - while (!eof() && read(reinterpret_cast(&i), 1)) - { - packet = _sensor.findPacket(i); - - if (packet != nullptr) - { - break; - } - } - - if (!packet || eof()) - { - LOG_DEBUG("{}: End of file reached.", nameId()); - return nullptr; - } - - if (packet->getRawDataLength() == 0) - { - LOG_TRACE("{}: Packet has empty payload", nameId()); - return nullptr; - } - - obs = std::make_shared(); - if (!vendor::espressif::decryptWiFiObs(obs, *packet, nameId())) { continue; }; - if (packet->type() != uart::protocol::Packet::Type::TYPE_BINARY) { continue; }; - - if (!obs->insTime.empty()) - { - _lastObsTime = obs->insTime; - } - else - { - if (!_lastObsTime.empty()) - { - obs->insTime = _lastObsTime; - } - else - { - LOG_DATA("{}: Could not set valid time. Skipping package.", nameId()); - continue; - } - } - break; - } - - LOG_DATA("{}: [{}] Packet found [{}][{}]", nameId(), obs->insTime.toYMDHMS(GPST), - obs->msgClass, vendor::ublox::getStringFromMsgId(obs->msgClass, obs->msgId)); - - invokeCallbacks(OUTPUT_PORT_INDEX_WiFiObs_OBS, obs); - return obs; -} - -NAV::FileReader::FileType NAV::EspressifFile::determineFileType() -{ - LOG_TRACE("called for {}", nameId()); - - auto filestream = std::ifstream(getFilepath()); - - constexpr uint16_t BUFFER_SIZE = 10; - - std::array buffer{}; - if (filestream.good()) - { - filestream.read(buffer.data(), BUFFER_SIZE); - - if ((static_cast(buffer.at(0)) == vendor::espressif::EspressifUartSensor::BINARY_SYNC_CHAR_1 - && static_cast(buffer.at(1)) == vendor::espressif::EspressifUartSensor::BINARY_SYNC_CHAR_2)) - { - filestream.close(); - LOG_DEBUG("{} has the file type: Binary", nameId()); - return FileType::BINARY; - } - filestream.close(); - - LOG_ERROR("{} could not determine file type", nameId()); - return FileType::NONE; - } - LOG_ERROR("{} could not open file {}", nameId(), getFilepath()); - return FileType::NONE; -} \ No newline at end of file diff --git a/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp b/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp deleted file mode 100644 index b9821309a..000000000 --- a/src/Nodes/DataProvider/WiFi/FileReader/EspressifFile.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// This file is part of INSTINCT, the INS Toolkit for Integrated -// Navigation Concepts and Training by the Institute of Navigation of -// the University of Stuttgart, Germany. -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License, v. 2.0. If a copy of the MPL was not distributed with this -// file, You can obtain one at https://mozilla.org/MPL/2.0/. - -/// @file EspressifFile.hpp -/// @brief File Reader for ubx files -/// @author R. Lintz (r-lintz@gmx.de) (master thesis) -/// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2024-03-07 - -#pragma once - -#include "internal/Node/Node.hpp" -#include "Nodes/DataProvider/Protocol/FileReader.hpp" - -#include "util/Vendor/Espressif/EspressifUartSensor.hpp" - -namespace NAV -{ -/// File Reader for WiFiObs log files -class EspressifFile : public Node, public FileReader -{ - public: - /// @brief Default constructor - EspressifFile(); - /// @brief Destructor - ~EspressifFile() override; - /// @brief Copy constructor - EspressifFile(const EspressifFile&) = delete; - /// @brief Move constructor - EspressifFile(EspressifFile&&) = delete; - /// @brief Copy assignment operator - EspressifFile& operator=(const EspressifFile&) = delete; - /// @brief Move assignment operator - EspressifFile& operator=(EspressifFile&&) = delete; - - /// @brief String representation of the Class Type - [[nodiscard]] static std::string typeStatic(); - - /// @brief String representation of the Class Type - [[nodiscard]] std::string type() const override; - - /// @brief String representation of the Class Category - [[nodiscard]] static std::string category(); - - /// @brief ImGui config window which is shown on double click - /// @attention Don't forget to set _hasConfig to true in the constructor of the node - void guiConfig() override; - - /// @brief Saves the node into a json object - [[nodiscard]] json save() const override; - - /// @brief Restores the node from a json object - /// @param[in] j Json object with the node state - void restore(const json& j) override; - - /// @brief Resets the node. Moves the read cursor to the start - bool resetNode() override; - - private: - constexpr static size_t OUTPUT_PORT_INDEX_WiFiObs_OBS = 0; ///< @brief Flow (WiFiObs) - - /// @brief Initialize the node - bool initialize() override; - - /// @brief Deinitialize the node - void deinitialize() override; - - /// @brief Polls data from the file - /// @return The read observation - [[nodiscard]] std::shared_ptr pollData(); - - /// @brief Determines the type of the file - /// @return The File Type - [[nodiscard]] FileType determineFileType() override; - - /// - InsTime _lastObsTime; - - /// Sensor Object - vendor::espressif::EspressifUartSensor _sensor; -}; - -} // namespace NAV diff --git a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp index 7570d4021..c8e19326f 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/EspressifSensor.cpp @@ -125,11 +125,6 @@ void NAV::EspressifSensor::deinitialize() { LOG_TRACE("{}: called", nameId()); - // if (!isInitialized()) // TODO: Sensor is not correctly initialized - // { - // return; - // } - if (_sensor->isConnected()) { try From 5725c519d6afd1ca0ee71627d48880c0cb568c1b Mon Sep 17 00:00:00 2001 From: rolandlintz <130669222+rolandlintz@users.noreply.github.com> Date: Tue, 18 Jun 2024 12:19:01 +0200 Subject: [PATCH 18/27] Delete .vscode/launch.json --- .vscode/launch.json | 166 -------------------------------------------- 1 file changed, 166 deletions(-) delete mode 100644 .vscode/launch.json diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index a92c2474c..000000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,166 +0,0 @@ -{ - "version": "0.2.0", - "configurations": [ - { - "name": "Mac Debug", - "type": "cppdbg", - "request": "launch", - "preLaunchTask": "MAIN: Build project", - "program": "${workspaceFolder}/build/bin/Debug/instinct", - "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' - "--config=config.ini", - "--load=\"flow/Default.flow\"", - // "--load=\"test/flow/Nodes/DataProcessor/GNSS/SinglePointPositioning.flow\"", - // "--nogui", - // "--noinit", - "--console-log-level=trace", // trace/debug/info/warning/error/critical/off - "--file-log-level=trace", // trace/debug/info/warning/error/critical/off - // "--log-filter=SinglePointPositioning.cpp", - // ================ To debug flow tests ================ - // "--flow-path=test/flow", - // "--input-path=test/data", - // "--output-path=test/logs", - ], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "lldb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "Linux Debug Tests", - "type": "cppdbg", - "request": "launch", - "preLaunchTask": "TEST: Build project", - "program": "${workspaceFolder}/build/bin/Debug/tests", - "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' - "'[Debug]'", - ], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "LLDB Debug", - "type": "lldb", - "request": "launch", - "preLaunchTask": "MAIN: Build project", - "program": "${workspaceFolder}/build/bin/Debug/instinct", - "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' - "--config=config.ini", - "--load=\"flow/Default.flow\"", - // "--load=\"test/flow/Nodes/DataProcessor/GNSS/SinglePointPositioning.flow\"", - // "--nogui", - // "--noinit", - "--console-log-level=trace", // trace/debug/info/warning/error/critical/off - "--file-log-level=trace", // trace/debug/info/warning/error/critical/off - // "--log-filter=SinglePointPositioning.cpp", - // ================ To debug flow tests ================ - // "--flow-path=test/flow", - // "--input-path=test/data", - // "--output-path=test/logs", - ], - "cwd": "${workspaceFolder}" - }, - { - "name": "LLDB Debug Tests", - "type": "lldb", - "request": "launch", - "preLaunchTask": "TEST: Build project", - "program": "${workspaceFolder}/build/bin/Debug/tests", - "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' - "[Debug]", - ], - "cwd": "${workspaceFolder}", - }, - { - "name": "Linux Debug Attach", - "type": "cppdbg", - "request": "attach", - "program": "${workspaceFolder}/build/bin/Debug/instinct", - "MIMode": "gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - }, - { - "description": "Set Disassembly Flavor to Intel", - "text": "-gdb-set disassembly-flavor intel", - "ignoreFailures": true - } - ] - }, - { - "name": "Linux Debug Attach Tests", - "type": "cppdbg", - "request": "attach", - "preLaunchTask": "TEST: Build project", - "program": "${workspaceFolder}/build/bin/Debug/tests", - "MIMode": "gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - }, - { - "description": "Set Disassembly Flavor to Intel", - "text": "-gdb-set disassembly-flavor intel", - "ignoreFailures": true - } - ] - }, - { - "name": "Windows Debug", - "type": "cppvsdbg", - "request": "launch", - "preLaunchTask": "MAIN: Build project", - "program": "${workspaceFolder}/build/bin/Debug/instinct", - "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' - "--config=config.ini", - "--load=\"flow/Default.flow\"", - // "--nogui", - // "--noinit", - "--console-log-level=trace", // trace/debug/info/warning/error/critical/off - "--file-log-level=trace", // trace/debug/info/warning/error/critical/off - // "--log-filter=SinglePointPositioning.cpp", - ], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "console": "integratedTerminal", - }, - { - "name": "Windows Debug Tests", - "type": "cppvsdbg", - "request": "launch", - "preLaunchTask": "TEST: Build project", - "program": "${workspaceFolder}/build/bin/Debug/tests", - "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' - "[Debug]", - ], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "console": "integratedTerminal", - }, - ] -} \ No newline at end of file From 69b02d4df71c3fda8a784ca649a3b3acd1c51f13 Mon Sep 17 00:00:00 2001 From: rolandlintz <130669222+rolandlintz@users.noreply.github.com> Date: Tue, 18 Jun 2024 12:19:24 +0200 Subject: [PATCH 19/27] Delete .vscode/settings.json --- .vscode/settings.json | 192 ------------------------------------------ 1 file changed, 192 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 166077973..000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,192 +0,0 @@ -{ - "[cpp]": { - "editor.defaultFormatter": "ms-vscode.cpptools" - }, - "[c]": { - "editor.defaultFormatter": "ms-vscode.cpptools" - }, - "[cmake]": { - "editor.formatOnSave": false - }, - "[latex]": { - "editor.formatOnSave": false, - "editor.tabSize": 2, - }, - "C_Cpp.autoAddFileAssociations": false, // Otherwise the "files.associations" list will be filled everytime opening a file without extension (e.g. ) - "C_Cpp.default.cppStandard": "c++20", - "cSpell.words": [ - "cmake", - "coeffs", - "docopt", - "doppler", - "Doxygen", - "ECEF", - "firable", - "GNSS", - "gtest", - "hobiger", - "Kalman", - "Klobuchar", - "kutta", - "MappingFunction", - "mathbf", - "nodiscard", - "Orolia", - "pseudorange", - "Pseudorange", - "quaterniond", - "runge", - "Saastamoinen", - "Sagnac", - "Skydel", - "Spirent", - "troposphereModel", - "ublox" - ], - "editor.formatOnSave": true, - "editor.rulers": [ - 150 - ], - "doxdocgen.generic.paramTemplate": "@param[in, out] {param} ", - "doxdocgen.file.fileOrder": [ - "custom", - "file", - "brief", - "author", - "date", - ], - "doxdocgen.file.customTag": [ - "This file is part of INSTINCT, the INS Toolkit for Integrated", - "Navigation Concepts and Training by the Institute of Navigation of", - "the University of Stuttgart, Germany.", - "", - "This Source Code Form is subject to the terms of the Mozilla Public", - "License, v. 2.0. If a copy of the MPL was not distributed with this", - "file, You can obtain one at https://mozilla.org/MPL/2.0/.", - "", - "<-- KEEP ONE EMPTY LINE HERE AND MAKE LICENSE COMMENTS only 2 slashes '//'", - "", - ], - "doxdocgen.generic.order": [ - "brief", - "tparam", - "param", - "return" - ], - "doxdocgen.c.commentPrefix": "/// ", - "doxdocgen.c.firstLine": "", - "doxdocgen.c.lastLine": "", - "doxdocgen.c.triggerSequence": "///", - "doxdocgen.generic.boolReturnsTrueFalse": false, - "doxdocgen.generic.returnTemplate": "@return ", - "doxdocgen.cpp.ctorText": "Constructor", - "doxdocgen.cpp.dtorText": "Destructor", - "files.associations": { - "**/C++/**": "cpp", // <-- stl libraries - "**/tbb/**": "cpp", // <-- intel libraries - "**/boost/**": "cpp", // <-- boost libraries - "*.tpp": "cpp", // <-- Template files - "*.log.*": "log", // <-- log files - "*.flow": "json", // <-- nodeeditor save files - "*.dox": "latex", // <-- Doxygen files - }, - "logFileHighlighter.customPatterns": [ - { - "pattern": "(\\[T\\]|\\[TRACE\\]|\\[Trace\\]|\\[trace\\])", - "foreground": "white", - }, - { - "pattern": "(\\[D\\]|\\[DEBUG\\]|\\[Debug\\]|\\[debug\\])", - "foreground": "cyan", - }, - { - "pattern": "(\\[I\\]|\\[INFO\\]|\\[Info\\]|\\[info\\])", - "foreground": "green", - }, - { - "pattern": "(\\[W\\]|\\[WARN\\w*\\]|\\[Warn\\w*\\]|\\[warn\\w*\\])", - "foreground": "yellow", - }, - { - "pattern": "(\\[E\\]|\\[ERROR\\]|\\[Error\\]|\\[error\\])", - "foreground": "red", - }, - { - "pattern": "(\\[C\\]|\\[CRIT\\w*\\]|\\[Crit\\w*\\]|\\[crit\\w*\\])", - "foreground": "white", - "background": "red" - }, - ], - "vsicons.associations.folders": [ - { - "icon": "library", - "extensions": [ - "lib", - ".lib", - "library", - "libraries" - ], - "format": "FileFormat.svg" - }, - { - "icon": "blueprint", - "extensions": [ - "gui" - ], - "format": "FileFormat.svg" - } - ], - "clang-tidy.compilerArgs": [ - "-Wno-unknown-warning-option" - ], - "comment-divider.length": 110, - "comment-divider.lineFiller": "#", - "comment-divider.mainHeaderFiller": "#", - "comment-divider.subheaderFiller": "-", - "comment-divider.shouldLengthIncludeIndent": true, - "mathover.matchRegex": "\\/\\/\\sMath:\\s(.+)", - "mathover.forwardSkip": 9, - "search.exclude": { - "**/.git": true, - "**/.DS_Store": true, - ".vscode": true, - "bin": true, - "build": true, - "cmake": true, - "data": true, - "flow": true, - "logs": true, - "resources": true, - "tools": true, - }, - "files.watcherExclude": { - "**/.git/objects/**": true, - "**/.git/subtree-cache/**": true, - }, - "files.exclude": { - "**/.git": true, - "**/.DS_Store": true, - "**/tools": true, - ".cache": true, - // ".gitattributes": true, - ".github": true, - ".gitlab": true, - ".clang-format": true, - ".clang-tidy": true, - ".cmake-format.yaml": true, - ".cppcheck-suppressions-list": true, - ".gitlab-ci.yml": true, - ".gitmodules": true, - "CMakeUserPresets.json": true, - "INSTINCT.ini": true, - "compile_commands.json": true, - }, - "todo-tree.filtering.excludeGlobs": [ - "**/lib" - ], - "matched-line-dimmer.enabled": true, - "matched-line-dimmer.patterns": [ - "LOG_(DATA)", - ], - "matched-line-dimmer.opacity": 40, -} \ No newline at end of file From 34a4130ee03edf35d752abd48c0be37fe62dd3f9 Mon Sep 17 00:00:00 2001 From: rolandlintz <130669222+rolandlintz@users.noreply.github.com> Date: Tue, 18 Jun 2024 12:19:46 +0200 Subject: [PATCH 20/27] Delete INSTINCT.code-workspace --- INSTINCT.code-workspace | 62 ----------------------------------------- 1 file changed, 62 deletions(-) delete mode 100644 INSTINCT.code-workspace diff --git a/INSTINCT.code-workspace b/INSTINCT.code-workspace deleted file mode 100644 index 63e6e7ca8..000000000 --- a/INSTINCT.code-workspace +++ /dev/null @@ -1,62 +0,0 @@ -{ - "folders": [ - { - "path": "." - } - ], - "settings": { - "C_Cpp.autoAddFileAssociations": false, - "doxdocgen.generic.paramTemplate": "@param[in, out] {param} ", - "doxdocgen.file.fileOrder": [ - "custom", - "file", - "brief", - "author", - "date" - ], - "doxdocgen.file.customTag": [ - "This file is part of INSTINCT, the INS Toolkit for Integrated", - "Navigation Concepts and Training by the Institute of Navigation of", - "the University of Stuttgart, Germany.", - "", - "This Source Code Form is subject to the terms of the Mozilla Public", - "License, v. 2.0. If a copy of the MPL was not distributed with this", - "file, You can obtain one at https://mozilla.org/MPL/2.0/.", - "", - "<-- KEEP ONE EMPTY LINE HERE AND MAKE LICENSE COMMENTS only 2 slashes '//'", - "" - ], - "doxdocgen.generic.order": [ - "brief", - "tparam", - "param", - "return" - ], - "doxdocgen.c.commentPrefix": "/// ", - "doxdocgen.c.firstLine": "", - "doxdocgen.c.lastLine": "", - "doxdocgen.c.triggerSequence": "///", - "doxdocgen.generic.boolReturnsTrueFalse": false, - "doxdocgen.generic.returnTemplate": "@return ", - "doxdocgen.cpp.ctorText": "Constructor", - "doxdocgen.cpp.dtorText": "Destructor", - "files.associations": { - "**/C++/**": "cpp", - "**/tbb/**": "cpp", - "**/boost/**": "cpp", - "*.tpp": "cpp", - "*.log.*": "log", - "*.flow": "json", - "*.dox": "latex" - }, - "mathover.matchRegex": "\\/\\/\\sMath:\\s(.+)", - "mathover.forwardSkip": 9, - "todo-tree.filtering.excludeGlobs": [ - "**/lib" - ], - "rainbow_csv.autodetect_separators": [ - "," - ], - "rainbow_csv.comment_prefix": "#" - } -} \ No newline at end of file From 75400c05a35234e37f03791dc02e56507755f1c4 Mon Sep 17 00:00:00 2001 From: rolandlintz <130669222+rolandlintz@users.noreply.github.com> Date: Tue, 18 Jun 2024 12:21:05 +0200 Subject: [PATCH 21/27] Delete src/util/Time/TimeBase.cpp --- src/util/Time/TimeBase.cpp | 100 ------------------------------------- 1 file changed, 100 deletions(-) delete mode 100644 src/util/Time/TimeBase.cpp diff --git a/src/util/Time/TimeBase.cpp b/src/util/Time/TimeBase.cpp deleted file mode 100644 index 35617ba4c..000000000 --- a/src/util/Time/TimeBase.cpp +++ /dev/null @@ -1,100 +0,0 @@ -// This file is part of INSTINCT, the INS Toolkit for Integrated -// Navigation Concepts and Training by the Institute of Navigation of -// the University of Stuttgart, Germany. -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License, v. 2.0. If a copy of the MPL was not distributed with this -// file, You can obtain one at https://mozilla.org/MPL/2.0/. - -#include "TimeBase.hpp" - -#include -#include - -#include "util/Logger.hpp" - -/* -------------------------------------------------------------------------------------------------------- */ -/* Private Members */ -/* -------------------------------------------------------------------------------------------------------- */ - -NAV::util::time::Mode timeMode = NAV::util::time::Mode::REAL_TIME; -NAV::InsTime currentTime; - -std::chrono::steady_clock::time_point currentTimeComputer; - -/* -------------------------------------------------------------------------------------------------------- */ -/* Private Function Declarations */ -/* -------------------------------------------------------------------------------------------------------- */ - -namespace NAV::util::time -{ -} // namespace NAV::util::time - -/* -------------------------------------------------------------------------------------------------------- */ -/* Public Members */ -/* -------------------------------------------------------------------------------------------------------- */ - -namespace NAV::util::time -{ -} // namespace NAV::util::time - -/* -------------------------------------------------------------------------------------------------------- */ -/* Function Definitions */ -/* -------------------------------------------------------------------------------------------------------- */ - -NAV::util::time::Mode NAV::util::time::GetMode() -{ - return timeMode; -} - -void NAV::util::time::SetMode(NAV::util::time::Mode mode) -{ - timeMode = mode; -} - -NAV::InsTime NAV::util::time::GetCurrentInsTime() -{ - if (timeMode == Mode::POST_PROCESSING || currentTime.empty()) - { - return currentTime; - } - // (timeMode == Mode::REAL_TIME) - auto elapsed = std::chrono::steady_clock::now() - currentTimeComputer; - return currentTime + elapsed; -} - -void NAV::util::time::SetCurrentTime(const NAV::InsTime& insTime) -{ - if (auto currentExactTime = GetCurrentInsTime(); - insTime < currentExactTime) - { - LOG_DATA("Not updating current Time [{} {:.6f}] to [{} {:.6f}], because the new time is earlier.", - currentExactTime.toGPSweekTow().gpsWeek, currentExactTime.toGPSweekTow().tow, - insTime.toGPSweekTow().gpsWeek, insTime.toGPSweekTow().tow); - } - else if (insTime >= currentExactTime) - { - if (timeMode == Mode::REAL_TIME) - { - LOG_DATA("Updating current Time [{}] to [{} ]", currentExactTime, insTime); - } - currentTimeComputer = std::chrono::steady_clock::now(); - currentTime = insTime; - LOG_DATA("Updating current Time [{}] to [{} ]", currentExactTime, insTime); - } -} - -void NAV::util::time::SetCurrentTimeToComputerTime() -{ - std::time_t t = std::time(nullptr); - std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe) - - currentTimeComputer = std::chrono::steady_clock::now(); - currentTime = InsTime{ static_cast(now->tm_year + 1900), static_cast(now->tm_mon), static_cast(now->tm_mday), - static_cast(now->tm_hour), static_cast(now->tm_min), static_cast(now->tm_sec) }; -} - -void NAV::util::time::ClearCurrentTime() -{ - currentTime.reset(); -} \ No newline at end of file From 9870a47c7f16952fd36e2cb20ac06e75a5272f8b Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Mon, 15 Jul 2024 13:10:51 +0200 Subject: [PATCH 22/27] Add launch.json again --- .vscode/launch.json | 166 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 166 insertions(+) create mode 100644 .vscode/launch.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 000000000..a92c2474c --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,166 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "Mac Debug", + "type": "cppdbg", + "request": "launch", + "preLaunchTask": "MAIN: Build project", + "program": "${workspaceFolder}/build/bin/Debug/instinct", + "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' + "--config=config.ini", + "--load=\"flow/Default.flow\"", + // "--load=\"test/flow/Nodes/DataProcessor/GNSS/SinglePointPositioning.flow\"", + // "--nogui", + // "--noinit", + "--console-log-level=trace", // trace/debug/info/warning/error/critical/off + "--file-log-level=trace", // trace/debug/info/warning/error/critical/off + // "--log-filter=SinglePointPositioning.cpp", + // ================ To debug flow tests ================ + // "--flow-path=test/flow", + // "--input-path=test/data", + // "--output-path=test/logs", + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "lldb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "Linux Debug Tests", + "type": "cppdbg", + "request": "launch", + "preLaunchTask": "TEST: Build project", + "program": "${workspaceFolder}/build/bin/Debug/tests", + "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' + "'[Debug]'", + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "LLDB Debug", + "type": "lldb", + "request": "launch", + "preLaunchTask": "MAIN: Build project", + "program": "${workspaceFolder}/build/bin/Debug/instinct", + "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' + "--config=config.ini", + "--load=\"flow/Default.flow\"", + // "--load=\"test/flow/Nodes/DataProcessor/GNSS/SinglePointPositioning.flow\"", + // "--nogui", + // "--noinit", + "--console-log-level=trace", // trace/debug/info/warning/error/critical/off + "--file-log-level=trace", // trace/debug/info/warning/error/critical/off + // "--log-filter=SinglePointPositioning.cpp", + // ================ To debug flow tests ================ + // "--flow-path=test/flow", + // "--input-path=test/data", + // "--output-path=test/logs", + ], + "cwd": "${workspaceFolder}" + }, + { + "name": "LLDB Debug Tests", + "type": "lldb", + "request": "launch", + "preLaunchTask": "TEST: Build project", + "program": "${workspaceFolder}/build/bin/Debug/tests", + "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' + "[Debug]", + ], + "cwd": "${workspaceFolder}", + }, + { + "name": "Linux Debug Attach", + "type": "cppdbg", + "request": "attach", + "program": "${workspaceFolder}/build/bin/Debug/instinct", + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + }, + { + "description": "Set Disassembly Flavor to Intel", + "text": "-gdb-set disassembly-flavor intel", + "ignoreFailures": true + } + ] + }, + { + "name": "Linux Debug Attach Tests", + "type": "cppdbg", + "request": "attach", + "preLaunchTask": "TEST: Build project", + "program": "${workspaceFolder}/build/bin/Debug/tests", + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + }, + { + "description": "Set Disassembly Flavor to Intel", + "text": "-gdb-set disassembly-flavor intel", + "ignoreFailures": true + } + ] + }, + { + "name": "Windows Debug", + "type": "cppvsdbg", + "request": "launch", + "preLaunchTask": "MAIN: Build project", + "program": "${workspaceFolder}/build/bin/Debug/instinct", + "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' + "--config=config.ini", + "--load=\"flow/Default.flow\"", + // "--nogui", + // "--noinit", + "--console-log-level=trace", // trace/debug/info/warning/error/critical/off + "--file-log-level=trace", // trace/debug/info/warning/error/critical/off + // "--log-filter=SinglePointPositioning.cpp", + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "console": "integratedTerminal", + }, + { + "name": "Windows Debug Tests", + "type": "cppvsdbg", + "request": "launch", + "preLaunchTask": "TEST: Build project", + "program": "${workspaceFolder}/build/bin/Debug/tests", + "args": [ // If there is a space in the option, VSCode automatically encloses the option in '...' + "[Debug]", + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "console": "integratedTerminal", + }, + ] +} \ No newline at end of file From 1f817186fd118763b99c1a34f34f6937e9f94cdf Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Mon, 15 Jul 2024 13:11:11 +0200 Subject: [PATCH 23/27] Revert "Delete .vscode/settings.json" This reverts commit 69b02d4df71c3fda8a784ca649a3b3acd1c51f13. --- .vscode/settings.json | 192 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 192 insertions(+) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 000000000..166077973 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,192 @@ +{ + "[cpp]": { + "editor.defaultFormatter": "ms-vscode.cpptools" + }, + "[c]": { + "editor.defaultFormatter": "ms-vscode.cpptools" + }, + "[cmake]": { + "editor.formatOnSave": false + }, + "[latex]": { + "editor.formatOnSave": false, + "editor.tabSize": 2, + }, + "C_Cpp.autoAddFileAssociations": false, // Otherwise the "files.associations" list will be filled everytime opening a file without extension (e.g. ) + "C_Cpp.default.cppStandard": "c++20", + "cSpell.words": [ + "cmake", + "coeffs", + "docopt", + "doppler", + "Doxygen", + "ECEF", + "firable", + "GNSS", + "gtest", + "hobiger", + "Kalman", + "Klobuchar", + "kutta", + "MappingFunction", + "mathbf", + "nodiscard", + "Orolia", + "pseudorange", + "Pseudorange", + "quaterniond", + "runge", + "Saastamoinen", + "Sagnac", + "Skydel", + "Spirent", + "troposphereModel", + "ublox" + ], + "editor.formatOnSave": true, + "editor.rulers": [ + 150 + ], + "doxdocgen.generic.paramTemplate": "@param[in, out] {param} ", + "doxdocgen.file.fileOrder": [ + "custom", + "file", + "brief", + "author", + "date", + ], + "doxdocgen.file.customTag": [ + "This file is part of INSTINCT, the INS Toolkit for Integrated", + "Navigation Concepts and Training by the Institute of Navigation of", + "the University of Stuttgart, Germany.", + "", + "This Source Code Form is subject to the terms of the Mozilla Public", + "License, v. 2.0. If a copy of the MPL was not distributed with this", + "file, You can obtain one at https://mozilla.org/MPL/2.0/.", + "", + "<-- KEEP ONE EMPTY LINE HERE AND MAKE LICENSE COMMENTS only 2 slashes '//'", + "", + ], + "doxdocgen.generic.order": [ + "brief", + "tparam", + "param", + "return" + ], + "doxdocgen.c.commentPrefix": "/// ", + "doxdocgen.c.firstLine": "", + "doxdocgen.c.lastLine": "", + "doxdocgen.c.triggerSequence": "///", + "doxdocgen.generic.boolReturnsTrueFalse": false, + "doxdocgen.generic.returnTemplate": "@return ", + "doxdocgen.cpp.ctorText": "Constructor", + "doxdocgen.cpp.dtorText": "Destructor", + "files.associations": { + "**/C++/**": "cpp", // <-- stl libraries + "**/tbb/**": "cpp", // <-- intel libraries + "**/boost/**": "cpp", // <-- boost libraries + "*.tpp": "cpp", // <-- Template files + "*.log.*": "log", // <-- log files + "*.flow": "json", // <-- nodeeditor save files + "*.dox": "latex", // <-- Doxygen files + }, + "logFileHighlighter.customPatterns": [ + { + "pattern": "(\\[T\\]|\\[TRACE\\]|\\[Trace\\]|\\[trace\\])", + "foreground": "white", + }, + { + "pattern": "(\\[D\\]|\\[DEBUG\\]|\\[Debug\\]|\\[debug\\])", + "foreground": "cyan", + }, + { + "pattern": "(\\[I\\]|\\[INFO\\]|\\[Info\\]|\\[info\\])", + "foreground": "green", + }, + { + "pattern": "(\\[W\\]|\\[WARN\\w*\\]|\\[Warn\\w*\\]|\\[warn\\w*\\])", + "foreground": "yellow", + }, + { + "pattern": "(\\[E\\]|\\[ERROR\\]|\\[Error\\]|\\[error\\])", + "foreground": "red", + }, + { + "pattern": "(\\[C\\]|\\[CRIT\\w*\\]|\\[Crit\\w*\\]|\\[crit\\w*\\])", + "foreground": "white", + "background": "red" + }, + ], + "vsicons.associations.folders": [ + { + "icon": "library", + "extensions": [ + "lib", + ".lib", + "library", + "libraries" + ], + "format": "FileFormat.svg" + }, + { + "icon": "blueprint", + "extensions": [ + "gui" + ], + "format": "FileFormat.svg" + } + ], + "clang-tidy.compilerArgs": [ + "-Wno-unknown-warning-option" + ], + "comment-divider.length": 110, + "comment-divider.lineFiller": "#", + "comment-divider.mainHeaderFiller": "#", + "comment-divider.subheaderFiller": "-", + "comment-divider.shouldLengthIncludeIndent": true, + "mathover.matchRegex": "\\/\\/\\sMath:\\s(.+)", + "mathover.forwardSkip": 9, + "search.exclude": { + "**/.git": true, + "**/.DS_Store": true, + ".vscode": true, + "bin": true, + "build": true, + "cmake": true, + "data": true, + "flow": true, + "logs": true, + "resources": true, + "tools": true, + }, + "files.watcherExclude": { + "**/.git/objects/**": true, + "**/.git/subtree-cache/**": true, + }, + "files.exclude": { + "**/.git": true, + "**/.DS_Store": true, + "**/tools": true, + ".cache": true, + // ".gitattributes": true, + ".github": true, + ".gitlab": true, + ".clang-format": true, + ".clang-tidy": true, + ".cmake-format.yaml": true, + ".cppcheck-suppressions-list": true, + ".gitlab-ci.yml": true, + ".gitmodules": true, + "CMakeUserPresets.json": true, + "INSTINCT.ini": true, + "compile_commands.json": true, + }, + "todo-tree.filtering.excludeGlobs": [ + "**/lib" + ], + "matched-line-dimmer.enabled": true, + "matched-line-dimmer.patterns": [ + "LOG_(DATA)", + ], + "matched-line-dimmer.opacity": 40, +} \ No newline at end of file From eb8b7d2a932d22ce5ea8e36b43d64327291752db Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Mon, 15 Jul 2024 13:11:51 +0200 Subject: [PATCH 24/27] Revert "Delete src/util/Time/TimeBase.cpp" This reverts commit 75400c05a35234e37f03791dc02e56507755f1c4. --- src/util/Time/TimeBase.cpp | 100 +++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 src/util/Time/TimeBase.cpp diff --git a/src/util/Time/TimeBase.cpp b/src/util/Time/TimeBase.cpp new file mode 100644 index 000000000..35617ba4c --- /dev/null +++ b/src/util/Time/TimeBase.cpp @@ -0,0 +1,100 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "TimeBase.hpp" + +#include +#include + +#include "util/Logger.hpp" + +/* -------------------------------------------------------------------------------------------------------- */ +/* Private Members */ +/* -------------------------------------------------------------------------------------------------------- */ + +NAV::util::time::Mode timeMode = NAV::util::time::Mode::REAL_TIME; +NAV::InsTime currentTime; + +std::chrono::steady_clock::time_point currentTimeComputer; + +/* -------------------------------------------------------------------------------------------------------- */ +/* Private Function Declarations */ +/* -------------------------------------------------------------------------------------------------------- */ + +namespace NAV::util::time +{ +} // namespace NAV::util::time + +/* -------------------------------------------------------------------------------------------------------- */ +/* Public Members */ +/* -------------------------------------------------------------------------------------------------------- */ + +namespace NAV::util::time +{ +} // namespace NAV::util::time + +/* -------------------------------------------------------------------------------------------------------- */ +/* Function Definitions */ +/* -------------------------------------------------------------------------------------------------------- */ + +NAV::util::time::Mode NAV::util::time::GetMode() +{ + return timeMode; +} + +void NAV::util::time::SetMode(NAV::util::time::Mode mode) +{ + timeMode = mode; +} + +NAV::InsTime NAV::util::time::GetCurrentInsTime() +{ + if (timeMode == Mode::POST_PROCESSING || currentTime.empty()) + { + return currentTime; + } + // (timeMode == Mode::REAL_TIME) + auto elapsed = std::chrono::steady_clock::now() - currentTimeComputer; + return currentTime + elapsed; +} + +void NAV::util::time::SetCurrentTime(const NAV::InsTime& insTime) +{ + if (auto currentExactTime = GetCurrentInsTime(); + insTime < currentExactTime) + { + LOG_DATA("Not updating current Time [{} {:.6f}] to [{} {:.6f}], because the new time is earlier.", + currentExactTime.toGPSweekTow().gpsWeek, currentExactTime.toGPSweekTow().tow, + insTime.toGPSweekTow().gpsWeek, insTime.toGPSweekTow().tow); + } + else if (insTime >= currentExactTime) + { + if (timeMode == Mode::REAL_TIME) + { + LOG_DATA("Updating current Time [{}] to [{} ]", currentExactTime, insTime); + } + currentTimeComputer = std::chrono::steady_clock::now(); + currentTime = insTime; + LOG_DATA("Updating current Time [{}] to [{} ]", currentExactTime, insTime); + } +} + +void NAV::util::time::SetCurrentTimeToComputerTime() +{ + std::time_t t = std::time(nullptr); + std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe) + + currentTimeComputer = std::chrono::steady_clock::now(); + currentTime = InsTime{ static_cast(now->tm_year + 1900), static_cast(now->tm_mon), static_cast(now->tm_mday), + static_cast(now->tm_hour), static_cast(now->tm_min), static_cast(now->tm_sec) }; +} + +void NAV::util::time::ClearCurrentTime() +{ + currentTime.reset(); +} \ No newline at end of file From 0b3811fe70135e405a35443e49c2633de4f58656 Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Sat, 20 Jul 2024 23:52:21 +0200 Subject: [PATCH 25/27] replace wifiposlogger with csvlogger --- src/NodeData/WiFi/WiFiPositioningSolution.hpp | 145 ++++--- src/NodeRegistry.cpp | 2 - .../Converter/GNSS/UartPacketConverter.cpp | 16 +- src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp | 3 +- .../WiFi/WiFiPositioningSolutionLogger.cpp | 356 ------------------ .../WiFi/WiFiPositioningSolutionLogger.hpp | 81 ---- .../DataProcessor/WiFi/WiFiPositioning.cpp | 10 +- src/Nodes/Plotting/Plot.cpp | 3 +- src/Nodes/Plotting/Plot.hpp | 1 + 9 files changed, 90 insertions(+), 527 deletions(-) delete mode 100644 src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp delete mode 100644 src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp diff --git a/src/NodeData/WiFi/WiFiPositioningSolution.hpp b/src/NodeData/WiFi/WiFiPositioningSolution.hpp index 850844c14..b32e0c2cf 100644 --- a/src/NodeData/WiFi/WiFiPositioningSolution.hpp +++ b/src/NodeData/WiFi/WiFiPositioningSolution.hpp @@ -38,96 +38,95 @@ class WiFiPositioningSolution : public PosVel return parent; } - // ------------------------------------------------------------- Getter ---------------------------------------------------------------- - - /// Returns the standard deviation of the position in ECEF frame coordinates in [m] - [[nodiscard]] const Eigen::Vector3d& e_positionStdev() const { return _e_positionStdev; } - - /// Returns the standard deviation of the position in local navigation frame coordinates in [m] - [[nodiscard]] const Eigen::Vector3d& n_positionStdev() const { return _n_positionStdev; } - - /// Returns the Bias in [m] - [[nodiscard]] double bias() const { return _bias; } - - /// Returns the standard deviation of the Bias in [m] - [[nodiscard]] double biasStdev() const { return _biasStdev; } - - /// Returns the standard deviation of the velocity in [m/s], in earth coordinates - [[nodiscard]] const Eigen::Vector3d& e_velocityStdev() const { return _e_velocityStdev; } - - /// Returns the standard deviation of the velocity in [m/s], in navigation coordinates - [[nodiscard]] const Eigen::Vector3d& n_velocityStdev() const { return _n_velocityStdev; } - - /// Returns the Covariance matrix in ECEF frame - [[nodiscard]] const Eigen::MatrixXd& e_CovarianceMatrix() const { return _e_covarianceMatrix; } - - /// Returns the Covariance matrix in local navigation frame - [[nodiscard]] const Eigen::MatrixXd& n_CovarianceMatrix() const { return _n_covarianceMatrix; } - - // ------------------------------------------------------------- Setter ---------------------------------------------------------------- - - /// @brief Set the Position in ECEF coordinates and its standard deviation - /// @param[in] e_position New Position in ECEF coordinates [m] - /// @param[in] e_PositionCovarianceMatrix Standard deviation of Position in ECEF coordinates [m] - void setPositionAndStdDev_e(const Eigen::Vector3d& e_position, const Eigen::Matrix3d& e_PositionCovarianceMatrix) + /// @brief Returns a vector of data descriptors + [[nodiscard]] static std::vector GetStaticDataDescriptors() { - setPosition_e(e_position); - _e_positionStdev = e_PositionCovarianceMatrix.diagonal().cwiseSqrt(); - _n_positionStdev = (n_Quat_e().toRotationMatrix() * e_PositionCovarianceMatrix * n_Quat_e().conjugate().toRotationMatrix()).diagonal().cwiseSqrt(); + auto desc = PosVel::GetStaticDataDescriptors(); + desc.reserve(GetStaticDescriptorCount()); + desc.emplace_back("Bias [m]"); + desc.emplace_back("Bias StDev [m]"); + return desc; } - /// @brief Set the Bias and its standard deviation - /// @param[in] bias New Bias [m] - /// @param[in] biasStdev Standard deviation of Bias [m] - void setBiasAndStdDev(double bias, double biasStdev) - { - _bias = bias; - _biasStdev = biasStdev; - } + /// @brief Get the amount of descriptors + [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 41; } - /// @brief Set the Velocity in ECEF coordinates and its standard deviation - /// @param[in] e_velocity New Velocity in ECEF coordinates [m/s] - /// @param[in] e_velocityCovarianceMatrix Covariance matrix of Velocity in earth coordinates [m/s] - void setVelocityAndStdDev_e(const Eigen::Vector3d& e_velocity, const Eigen::Matrix3d& e_velocityCovarianceMatrix) - { - setVelocity_e(e_velocity); - _e_velocityStdev = e_velocityCovarianceMatrix.diagonal().cwiseSqrt(); - _n_velocityStdev = (n_Quat_e().toRotationMatrix() * e_velocityCovarianceMatrix * n_Quat_e().conjugate().toRotationMatrix()).diagonal().cwiseSqrt(); - } + /// @brief Returns a vector of data descriptors + [[nodiscard]] std::vector staticDataDescriptors() const override { return GetStaticDataDescriptors(); } - /// @brief Set the Covariance matrix - /// @param[in] P Covariance matrix - void setCovarianceMatrix(const Eigen::MatrixXd& P) - { - _e_covarianceMatrix = P; - n_CovarianceMatrix_e(); - } + /// @brief Get the amount of descriptors + [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); } - /// @brief Transforms the covariance matrix from ECEF frame to local navigation frame - void n_CovarianceMatrix_e() + /// @brief Get the value at the index + /// @param idx Index corresponding to data descriptor order + /// @return Value if in the observation + [[nodiscard]] std::optional getValueAt(size_t idx) const override { - _n_covarianceMatrix = _e_covarianceMatrix; - - Eigen::Vector3d lla_pos = lla_position(); - Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_pos(0), lla_pos(1)); - _n_covarianceMatrix.block<3, 3>(0, 0) = n_Quat_e.toRotationMatrix() * _e_covarianceMatrix.block<3, 3>(0, 0) * n_Quat_e.conjugate().toRotationMatrix(); // variance of position - if (_e_covarianceMatrix.rows() >= 4 && _e_covarianceMatrix.cols() >= 4) // velocity is also available + INS_ASSERT(idx < GetStaticDescriptorCount()); + switch (idx) { - _n_covarianceMatrix.block<3, 3>(3, 3) = n_Quat_e.toRotationMatrix() * _e_covarianceMatrix.block<3, 3>(3, 3) * n_Quat_e.toRotationMatrix(); // variance of velocity + case 0: // Latitude [deg] + case 1: // Longitude [deg] + case 2: // Altitude [m] + case 3: // North/South [m] + case 4: // East/West [m] + case 5: // X-ECEF [m] + case 6: // Y-ECEF [m] + case 7: // Z-ECEF [m] + case 8: // X-ECEF StDev [m] + case 9: // Y-ECEF StDev [m] + case 10: // Z-ECEF StDev [m] + case 11: // XY-ECEF StDev [m] + case 12: // XZ-ECEF StDev [m] + case 13: // YZ-ECEF StDev [m] + case 14: // North StDev [m] + case 15: // East StDev [m] + case 16: // Down StDev [m] + case 17: // NE StDev [m] + case 18: // ND StDev [m] + case 19: // ED StDev [m] + case 20: // Velocity norm [m/s] + case 21: // X velocity ECEF [m/s] + case 22: // Y velocity ECEF [m/s] + case 23: // Z velocity ECEF [m/s] + case 24: // North velocity [m/s] + case 25: // East velocity [m/s] + case 26: // Down velocity [m/s] + case 27: // X velocity ECEF StDev [m/s] + case 28: // Y velocity ECEF StDev [m/s] + case 29: // Z velocity ECEF StDev [m/s] + case 30: // XY velocity StDev [m] + case 31: // XZ velocity StDev [m] + case 32: // YZ velocity StDev [m] + case 33: // North velocity StDev [m/s] + case 34: // East velocity StDev [m/s] + case 35: // Down velocity StDev [m/s] + case 36: // NE velocity StDev [m] + case 37: // ND velocity StDev [m] + case 38: // ED velocity StDev [m] + return PosVel::getValueAt(idx); + case 39: // Bias [m] + return bias; + case 40: // Bias StDev [m] + return biasStdev; + default: + return std::nullopt; } + return std::nullopt; } + // --------------------------------------------------------- Public Members ------------------------------------------------------------ + /// Bias [m] + double bias = std::nan(""); + /// Standard deviation of Bias [m] + double biasStdev = std::nan(""); + private: /// Standard deviation of Position in ECEF coordinates [m] Eigen::Vector3d _e_positionStdev = Eigen::Vector3d::Zero() * std::nan(""); /// Standard deviation of Position in local navigation frame coordinates [m] Eigen::Vector3d _n_positionStdev = Eigen::Vector3d::Zero() * std::nan(""); - /// Bias [m] - double _bias = std::nan(""); - /// Standard deviation of Bias [m] - double _biasStdev = std::nan(""); - /// Standard deviation of Velocity in earth coordinates [m/s] Eigen::Vector3d _e_velocityStdev = Eigen::Vector3d::Zero() * std::nan(""); /// Standard deviation of Velocity in navigation coordinates [m/s] diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index 5ddbdc3d0..b91d624f8 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -191,7 +191,6 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataLogger/GNSS/UartDataLogger.hpp" #include "Nodes/DataLogger/IMU/VectorNavDataLogger.hpp" #include "Nodes/DataLogger/WiFi/WiFiObsLogger.hpp" -#include "Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp" // Data Processor #include "Nodes/DataProcessor/ErrorModel/ErrorModel.hpp" #include "Nodes/DataProcessor/GNSS/GnssAnalyzer.hpp" @@ -264,7 +263,6 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); - registerNodeType(); // Data Processor registerNodeType(); registerNodeType(); diff --git a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp index af1abd103..445d90311 100644 --- a/src/Nodes/Converter/GNSS/UartPacketConverter.cpp +++ b/src/Nodes/Converter/GNSS/UartPacketConverter.cpp @@ -198,29 +198,29 @@ void NAV::UartPacketConverter::receiveObs(NAV::InputPin::NodeDataQueue& queue, [ vendor::espressif::decryptWiFiObs(obs, packet, nameId()); if (_syncInPin && inputPins.at(1).isPinLinked()) { - auto timeSyncMaster = getInputValue(1); + auto timeSyncMaster = getInputValue(0); if (_lastSyncInCnt > obs->timeOutputs.syncInCnt) // reset of the slave { _syncOutCntCorr = 0; - _syncInCntCorr = timeSyncMaster->syncOutCnt; + _syncInCntCorr = timeSyncMaster->v->syncOutCnt; } - else if (_lastSyncOutCnt > timeSyncMaster->syncOutCnt) // reset of the master + else if (_lastSyncOutCnt > timeSyncMaster->v->syncOutCnt) // reset of the master { _syncInCntCorr = 0; _syncOutCntCorr = obs->timeOutputs.syncInCnt; } - else if (_lastSyncOutCnt == 0 && timeSyncMaster->syncOutCnt > 1) // slave counter started later + else if (_lastSyncOutCnt == 0 && timeSyncMaster->v->syncOutCnt > 1) // slave counter started later { - _syncInCntCorr = timeSyncMaster->syncOutCnt; + _syncInCntCorr = timeSyncMaster->v->syncOutCnt; } else if (_lastSyncOutCnt == 0 && obs->timeOutputs.syncInCnt > 1) // master counter started later { _syncOutCntCorr = obs->timeOutputs.syncInCnt; } - _lastSyncOutCnt = timeSyncMaster->syncOutCnt; + _lastSyncOutCnt = timeSyncMaster->v->syncOutCnt; _lastSyncInCnt = obs->timeOutputs.syncInCnt; - int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->syncOutCnt - _syncOutCntCorr; - obs->insTime = timeSyncMaster->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn) + int64_t syncCntDiff = obs->timeOutputs.syncInCnt + _syncInCntCorr - timeSyncMaster->v->syncOutCnt - _syncOutCntCorr; + obs->insTime = timeSyncMaster->v->ppsTime + std::chrono::microseconds(obs->timeOutputs.timeSyncIn) + std::chrono::seconds(syncCntDiff); // LOG_DATA("{}: Syncing time {}, pps {}, syncOutCnt {}, syncInCnt {}, syncCntDiff {}, syncInCntCorr {}, syncOutCntCorr {}", // nameId(), obs->insTime.toGPSweekTow(), timeSyncMaster->ppsTime.toGPSweekTow(), diff --git a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp index ac3c27c96..50a0245b8 100644 --- a/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp +++ b/src/Nodes/DataLogger/WiFi/WiFiObsLogger.hpp @@ -13,9 +13,10 @@ #pragma once +#include "util/Logger/CommonLog.hpp" + #include "internal/Node/Node.hpp" #include "Nodes/DataLogger/Protocol/FileWriter.hpp" -#include "Nodes/DataLogger/Protocol/CommonLog.hpp" namespace NAV { diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp deleted file mode 100644 index fa412d742..000000000 --- a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.cpp +++ /dev/null @@ -1,356 +0,0 @@ -// This file is part of INSTINCT, the INS Toolkit for Integrated -// Navigation Concepts and Training by the Institute of Navigation of -// the University of Stuttgart, Germany. -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License, v. 2.0. If a copy of the MPL was not distributed with this -// file, You can obtain one at https://mozilla.org/MPL/2.0/. - -#include "WiFiPositioningSolutionLogger.hpp" - -#include "Navigation/Transformations/Units.hpp" - -#include "util/Logger.hpp" - -#include // std::setprecision - -#include "internal/NodeManager.hpp" -namespace nm = NAV::NodeManager; -#include "internal/FlowManager.hpp" - -#include "NodeData/WiFi/WiFiPositioningSolution.hpp" - -NAV::WiFiPositioningSolutionLogger::WiFiPositioningSolutionLogger() - : Node(typeStatic()) -{ - LOG_TRACE("{}: called", name); - - _fileType = FileType::ASCII; - - _hasConfig = true; - _guiConfigDefaultWindowSize = { 380, 70 }; - - nm::CreateInputPin(this, "writeObservation", Pin::Type::Flow, { WiFiPositioningSolution::type() }, &WiFiPositioningSolutionLogger::writeObservation); -} - -NAV::WiFiPositioningSolutionLogger::~WiFiPositioningSolutionLogger() -{ - LOG_TRACE("{}: called", nameId()); -} - -std::string NAV::WiFiPositioningSolutionLogger::typeStatic() -{ - return "WiFiPositioningSolutionLogger"; -} - -std::string NAV::WiFiPositioningSolutionLogger::type() const -{ - return typeStatic(); -} - -std::string NAV::WiFiPositioningSolutionLogger::category() -{ - return "Data Logger"; -} - -void NAV::WiFiPositioningSolutionLogger::guiConfig() -{ - if (FileWriter::guiConfig(".csv", { ".csv" }, size_t(id), nameId())) - { - flow::ApplyChanges(); - doDeinitialize(); - } - - if (ImGui::Checkbox(fmt::format("Log NaN values##{}", nameId()).c_str(), &_logNanValues)) - { - flow::ApplyChanges(); - } -} - -[[nodiscard]] json NAV::WiFiPositioningSolutionLogger::save() const -{ - LOG_TRACE("{}: called", nameId()); - - json j; - - j["FileWriter"] = FileWriter::save(); - j["logNanValues"] = _logNanValues; - - return j; -} - -void NAV::WiFiPositioningSolutionLogger::restore(json const& j) -{ - LOG_TRACE("{}: called", nameId()); - - if (j.contains("FileWriter")) - { - FileWriter::restore(j.at("FileWriter")); - } - if (j.contains("logNanValues")) - { - _logNanValues = j.at("logNanValues"); - } -} - -void NAV::WiFiPositioningSolutionLogger::flush() -{ - _filestream.flush(); -} - -bool NAV::WiFiPositioningSolutionLogger::initialize() -{ - LOG_TRACE("{}: called", nameId()); - - if (!FileWriter::initialize()) - { - return false; - } - - CommonLog::initialize(); - - _filestream << "Time [s],GpsCycle,GpsWeek,GpsTow [s]," - << "Pos ECEF X [m],Pos ECEF Y [m],Pos ECEF Z [m],Latitude [deg],Longitude [deg],Altitude [m]," - << "North/South [m],East/West [m]," - << "Bias [m],Bias StDev [m]," - << "Vel ECEF X [m/s],Vel ECEF Y [m/s],Vel ECEF Z [m/s],Vel N [m/s],Vel E [m/s],Vel D [m/s]," - << "X-ECEF StDev [m],Y-ECEF StDev [m],Z-ECEF StDev [m]," - // << "XY-ECEF StDev [m],XZ-ECEF StDev [m],YZ-ECEF StDev [m]," - << "North StDev [m],East StDev [m],Down StDev [m]," - // << "NE-ECEF StDev [m],ND-ECEF StDev [m],ED-ECEF StDev [m]," - << "X velocity ECEF StDev [m/s],Y velocity ECEF StDev [m/s],Z velocity ECEF StDev [m/s]," - // << "XY velocity StDev [m],XZ velocity StDev [m],YZ velocity StDev [m]," - << "North velocity StDev [m/s],East velocity StDev [m/s],Down velocity StDev [m/s]," - // << "NE velocity StDev [m],ND velocity StDev [m],ED velocity StDev [m]," - << std::endl; - - return true; -} - -void NAV::WiFiPositioningSolutionLogger::deinitialize() -{ - LOG_TRACE("{}: called", nameId()); - - FileWriter::deinitialize(); -} - -void NAV::WiFiPositioningSolutionLogger::writeObservation(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) -{ - auto obs = std::static_pointer_cast(queue.extract_front()); - - constexpr int gpsCyclePrecision = 3; - constexpr int gpsTimePrecision = 12; - constexpr int valuePrecision = 12; - - if (!obs->insTime.empty()) - { - _filestream << std::setprecision(valuePrecision) << std::round(calcTimeIntoRun(obs->insTime) * 1e9) / 1e9; - } - _filestream << ","; - if (!obs->insTime.empty()) - { - _filestream << std::fixed << std::setprecision(gpsCyclePrecision) << obs->insTime.toGPSweekTow().gpsCycle; - } - _filestream << ","; - if (!obs->insTime.empty()) - { - _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs->insTime.toGPSweekTow().gpsWeek; - } - _filestream << ","; - if (!obs->insTime.empty()) - { - _filestream << std::defaultfloat << std::setprecision(gpsTimePrecision) << obs->insTime.toGPSweekTow().tow; - } - _filestream << "," << std::setprecision(valuePrecision); - - if (_logNanValues) - { - // -------------------------------------------------------- Position ----------------------------------------------------------- - - _filestream << obs->e_position().x(); // Pos ECEF X [m] - _filestream << ","; - _filestream << obs->e_position().y(); // Pos ECEF Y [m] - _filestream << ","; - _filestream << obs->e_position().z(); // Pos ECEF Z [m] - _filestream << ","; - _filestream << rad2deg(obs->lla_position().x()); // Latitude [deg] - _filestream << ","; - _filestream << rad2deg(obs->lla_position().y()); // Longitude [deg] - _filestream << ","; - _filestream << obs->lla_position().z(); // Altitude [m] - _filestream << ","; - auto localPosition = calcLocalPosition(obs->lla_position()); - _filestream << localPosition.northSouth << ","; // North/South [m] - _filestream << localPosition.eastWest << ","; // East/West [m] - - // -------------------------------------------------------- Bias ----------------------------------------------------------- - _filestream << obs->bias(); // Bias [m] - _filestream << ","; - _filestream << obs->biasStdev(); // Bias StDev [m] - _filestream << ","; - // -------------------------------------------------------- Velocity ----------------------------------------------------------- - - _filestream << obs->e_velocity().x(); // Vel ECEF X [m/s] - _filestream << ","; - _filestream << obs->e_velocity().y(); // Vel ECEF Y [m/s] - _filestream << ","; - _filestream << obs->e_velocity().z(); // Vel ECEF Z [m/s] - _filestream << ","; - _filestream << obs->n_velocity().x(); // Vel N [m/s] - _filestream << ","; - _filestream << obs->n_velocity().y(); // Vel E [m/s] - _filestream << ","; - _filestream << obs->n_velocity().z(); // Vel D [m/s] - _filestream << ","; - // -------------------------------------------------------- Standard Deviation ----------------------------------------------------------- - _filestream << obs->e_positionStdev()(0); // X-ECEF StDev [m] - _filestream << ","; - _filestream << obs->e_positionStdev()(1); // Y-ECEF StDev [m] - _filestream << ","; - _filestream << obs->e_positionStdev()(2); // Z-ECEF StDev [m] - _filestream << ","; - // _filestream << obs->e_positionStdev()(0, 1); // XY-ECEF StDev [m] - // _filestream << ","; - // _filestream << obs->e_positionStdev()(0, 2); // XZ-ECEF StDev [m] - // _filestream << ","; - // _filestream << obs->e_positionStdev()(1, 2); // YZ-ECEF StDev [m] - // _filestream << ","; - _filestream << obs->n_positionStdev()(0); // North StDev [m] - _filestream << ","; - _filestream << obs->n_positionStdev()(1); // East StDev [m] - _filestream << ","; - _filestream << obs->n_positionStdev()(2); // Down StDev [m] - _filestream << ","; - // _filestream << obs->n_positionStdev()(0, 1); // NE-ECEF StDev [m] - // _filestream << ","; - // _filestream << obs->n_positionStdev()(0, 2); // ND-ECEF StDev [m] - // _filestream << ","; - // _filestream << obs->n_positionStdev()(1, 2); // ED-ECEF StDev [m] - // _filestream << ","; - _filestream << obs->e_velocityStdev()(0); // X velocity ECEF StDev [m/s] - _filestream << ","; - _filestream << obs->e_velocityStdev()(1); // Y velocity ECEF StDev [m/s] - _filestream << ","; - _filestream << obs->e_velocityStdev()(2); // Z velocity ECEF StDev [m/s] - _filestream << ","; - // _filestream << obs->e_velocityStdev()(0, 1); // XY velocity StDev [m] - // _filestream << ","; - // _filestream << obs->e_velocityStdev()(0, 2); // XZ velocity StDev [m] - // _filestream << ","; - // _filestream << obs->e_velocityStdev()(1, 2); // YZ velocity StDev [m] - // _filestream << ","; - _filestream << obs->n_velocityStdev()(0); // North velocity StDev [m/s] - _filestream << ","; - _filestream << obs->n_velocityStdev()(1); // East velocity StDev [m/s] - _filestream << ","; - _filestream << obs->n_velocityStdev()(2); // Down velocity StDev [m/s] - _filestream << ","; - // _filestream << obs->n_velocityStdev()(0, 1); // NE velocity StDev [m] - // _filestream << ","; - // _filestream << obs->n_velocityStdev()(0, 2); // ND velocity StDev [m] - // _filestream << ","; - // _filestream << obs->n_velocityStdev()(1, 2); // ED velocity StDev [m] - // _filestream << ","; - _filestream << '\n'; - } - else - { - // -------------------------------------------------------- Position ----------------------------------------------------------- - - if (!std::isnan(obs->e_position().x())) { _filestream << obs->e_position().x(); } // Pos ECEF X [m] - _filestream << ","; - if (!std::isnan(obs->e_position().y())) { _filestream << obs->e_position().y(); } // Pos ECEF Y [m] - _filestream << ","; - if (!std::isnan(obs->e_position().z())) { _filestream << obs->e_position().z(); } // Pos ECEF Z [m] - _filestream << ","; - if (!std::isnan(obs->lla_position().x())) { _filestream << rad2deg(obs->lla_position().x()); } // Latitude [deg] - _filestream << ","; - if (!std::isnan(obs->lla_position().y())) { _filestream << rad2deg(obs->lla_position().y()); } // Longitude [deg] - _filestream << ","; - if (!std::isnan(obs->lla_position().z())) { _filestream << obs->lla_position().z(); } // Altitude [m] - _filestream << ","; - if (!std::isnan(obs->lla_position().x()) && !std::isnan(obs->lla_position().y())) - { - auto localPosition = calcLocalPosition(obs->lla_position()); - _filestream << localPosition.northSouth << ","; // North/South [m] - _filestream << localPosition.eastWest << ","; // East/West [m] - } - else - { - _filestream << ",,"; - } - - // -------------------------------------------------------- Bias ----------------------------------------------------------- - if (!std::isnan(obs->bias())) { _filestream << obs->bias(); } // Bias [m] - _filestream << ","; - if (!std::isnan(obs->biasStdev())) { _filestream << obs->biasStdev(); } // Bias StDev [m] - _filestream << ","; - - // -------------------------------------------------------- Velocity ----------------------------------------------------------- - - if (!std::isnan(obs->e_velocity().x())) { _filestream << obs->e_velocity().x(); } // Vel ECEF X [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocity().y())) { _filestream << obs->e_velocity().y(); } // Vel ECEF Y [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocity().z())) { _filestream << obs->e_velocity().z(); } // Vel ECEF Z [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocity().x())) { _filestream << obs->n_velocity().x(); } // Vel N [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocity().y())) { _filestream << obs->n_velocity().y(); } // Vel E [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocity().z())) { _filestream << obs->n_velocity().z(); } // Vel D [m/s] - _filestream << ","; - - // -------------------------------------------------------- Standard Deviation ----------------------------------------------------------- - if (!std::isnan(obs->e_positionStdev()(0))) { _filestream << obs->e_positionStdev()(0); }; // X-ECEF StDev [m] - _filestream << ","; - if (!std::isnan(obs->e_positionStdev()(1))) { _filestream << obs->e_positionStdev()(1); }; // Y-ECEF StDev [m] - _filestream << ","; - if (!std::isnan(obs->e_positionStdev()(2))) { _filestream << obs->e_positionStdev()(2); }; // Z-ECEF StDev [m] - _filestream << ","; - // if (!std::isnan(obs->e_positionStdev()(0, 1))) { _filestream << obs->e_positionStdev()(0, 1); }; // XY-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_positionStdev()(0, 2))) { _filestream << obs->e_positionStdev()(0, 2); }; // XZ-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_positionStdev()(1, 2))) { _filestream << obs->e_positionStdev()(1, 2); }; // YZ-ECEF StDev [m] - // _filestream << ","; - if (!std::isnan(obs->n_positionStdev()(0))) { _filestream << obs->n_positionStdev()(0); }; // North StDev [m] - _filestream << ","; - if (!std::isnan(obs->n_positionStdev()(1))) { _filestream << obs->n_positionStdev()(1); }; // East StDev [m] - _filestream << ","; - if (!std::isnan(obs->n_positionStdev()(2))) { _filestream << obs->n_positionStdev()(2); }; // Down StDev [m] - _filestream << ","; - // if (!std::isnan(obs->n_positionStdev()(0, 1))) { _filestream << obs->n_positionStdev()(0, 1); }; // NE-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_positionStdev()(0, 2))) { _filestream << obs->n_positionStdev()(0, 2); }; // ND-ECEF StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_positionStdev()(1, 2))) { _filestream << obs->n_positionStdev()(1, 2); }; // ED-ECEF StDev [m] - // _filestream << ","; - if (!std::isnan(obs->e_velocityStdev()(0))) { _filestream << obs->e_velocityStdev()(0); }; // X velocity ECEF StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocityStdev()(1))) { _filestream << obs->e_velocityStdev()(1); }; // Y velocity ECEF StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->e_velocityStdev()(2))) { _filestream << obs->e_velocityStdev()(2); }; // Z velocity ECEF StDev [m/s] - _filestream << ","; - // if (!std::isnan(obs->e_velocityStdev()(0, 1))) { _filestream << obs->e_velocityStdev()(0, 1); }; // XY velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_velocityStdev()(0, 2))) { _filestream << obs->e_velocityStdev()(0, 2); }; // XZ velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->e_velocityStdev()(1, 2))) { _filestream << obs->e_velocityStdev()(1, 2); }; // YZ velocity StDev [m] - // _filestream << ","; - if (!std::isnan(obs->n_velocityStdev()(0))) { _filestream << obs->n_velocityStdev()(0); }; // North velocity StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocityStdev()(1))) { _filestream << obs->n_velocityStdev()(1); }; // East velocity StDev [m/s] - _filestream << ","; - if (!std::isnan(obs->n_velocityStdev()(2))) { _filestream << obs->n_velocityStdev()(2); }; // Down velocity StDev [m/s] - _filestream << ","; - // if (!std::isnan(obs->n_velocityStdev()(0, 1))) { _filestream << obs->n_velocityStdev()(0, 1); }; // NE velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_velocityStdev()(0, 2))) { _filestream << obs->n_velocityStdev()(0, 2); }; // ND velocity StDev [m] - // _filestream << ","; - // if (!std::isnan(obs->n_velocityStdev()(1, 2))) { _filestream << obs->n_velocityStdev()(1, 2); }; // ED velocity StDev [m] - // _filestream << ","; - - _filestream << '\n'; - } -} \ No newline at end of file diff --git a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp b/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp deleted file mode 100644 index b7c813d22..000000000 --- a/src/Nodes/DataLogger/WiFi/WiFiPositioningSolutionLogger.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// This file is part of INSTINCT, the INS Toolkit for Integrated -// Navigation Concepts and Training by the Institute of Navigation of -// the University of Stuttgart, Germany. -// -// This Source Code Form is subject to the terms of the Mozilla Public -// License, v. 2.0. If a copy of the MPL was not distributed with this -// file, You can obtain one at https://mozilla.org/MPL/2.0/. - -/// @file WiFiPositioningSolutionLogger.hpp -/// @brief Data Logger for SppSolution observations -/// @author R. Lintz (r-lintz@gmx.de) (master thesis) -/// @author T. Topp (topp@ins.uni-stuttgart.de) -/// @date 2024-03-20 - -#pragma once - -#include "internal/Node/Node.hpp" -#include "Nodes/DataLogger/Protocol/FileWriter.hpp" -#include "Nodes/DataLogger/Protocol/CommonLog.hpp" - -namespace NAV -{ -class NodeData; - -/// Data Logger for SppSolution observations -class WiFiPositioningSolutionLogger : public Node, public FileWriter, public CommonLog -{ - public: - /// @brief Default constructor - WiFiPositioningSolutionLogger(); - /// @brief Destructor - ~WiFiPositioningSolutionLogger() override; - /// @brief Copy constructor - WiFiPositioningSolutionLogger(const WiFiPositioningSolutionLogger&) = delete; - /// @brief Move constructor - WiFiPositioningSolutionLogger(WiFiPositioningSolutionLogger&&) = delete; - /// @brief Copy assignment operator - WiFiPositioningSolutionLogger& operator=(const WiFiPositioningSolutionLogger&) = delete; - /// @brief Move assignment operator - WiFiPositioningSolutionLogger& operator=(WiFiPositioningSolutionLogger&&) = delete; - - /// @brief String representation of the Class Type - [[nodiscard]] static std::string typeStatic(); - - /// @brief String representation of the Class Type - [[nodiscard]] std::string type() const override; - - /// @brief String representation of the Class Category - [[nodiscard]] static std::string category(); - - /// @brief ImGui config window which is shown on double click - /// @attention Don't forget to set _hasConfig to true in the constructor of the node - void guiConfig() override; - - /// @brief Saves the node into a json object - [[nodiscard]] json save() const override; - - /// @brief Restores the node from a json object - /// @param[in] j Json object with the node state - void restore(const json& j) override; - - /// @brief Function called by the flow executer after finishing to flush out remaining data - void flush() override; - - private: - /// @brief Initialize the node - bool initialize() override; - - /// @brief Deinitialize the node - void deinitialize() override; - - /// @brief Selection weather to log nan values or not - bool _logNanValues = false; - - /// @brief Write Observation to the file - /// @param[in] queue Queue with all the received data messages - /// @param[in] pinIdx Index of the pin the data is received on - void writeObservation(InputPin::NodeDataQueue& queue, size_t pinIdx); -}; - -} // namespace NAV diff --git a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp index cf16278fe..5048a8964 100644 --- a/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp +++ b/src/Nodes/DataProcessor/WiFi/WiFiPositioning.cpp @@ -840,10 +840,11 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size { LeastSquaresResult lsqSolution = WiFiPositioning::lsqSolution(); wifiPositioningSolution->setPositionAndStdDev_e(lsqSolution.solution.block<3, 1>(0, 0), lsqSolution.variance.block<3, 3>(0, 0).cwiseSqrt()); - wifiPositioningSolution->setCovarianceMatrix(lsqSolution.variance.block<3, 3>(0, 0)); + wifiPositioningSolution->setPosCovarianceMatrix_e(lsqSolution.variance.block<3, 3>(0, 0)); if (_estimateBias) { - wifiPositioningSolution->setBiasAndStdDev(_state.bias, lsqSolution.variance(3, 3)); + wifiPositioningSolution->bias = _state.bias; + wifiPositioningSolution->biasStdev = lsqSolution.variance(3, 3); } invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); } @@ -855,10 +856,11 @@ void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size wifiPositioningSolution->setPositionAndStdDev_e(_kalmanFilter.x.block<3, 1>(0, 0), _kalmanFilter.P.block<3, 3>(0, 0).cwiseSqrt()); if (_estimateBias) { - wifiPositioningSolution->setBiasAndStdDev(_kalmanFilter.x(6), _kalmanFilter.P(6, 6)); + wifiPositioningSolution->bias = _kalmanFilter.x(6); + wifiPositioningSolution->biasStdev = _kalmanFilter.P(6, 6); } wifiPositioningSolution->setVelocityAndStdDev_e(_kalmanFilter.x.block<3, 1>(3, 0), _kalmanFilter.P.block<3, 3>(3, 3).cwiseSqrt()); - wifiPositioningSolution->setCovarianceMatrix(_kalmanFilter.P); + wifiPositioningSolution->setPosVelCovarianceMatrix_e(_kalmanFilter.P); invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution); } diff --git a/src/Nodes/Plotting/Plot.cpp b/src/Nodes/Plotting/Plot.cpp index c6a40284b..64c16f937 100644 --- a/src/Nodes/Plotting/Plot.cpp +++ b/src/Nodes/Plotting/Plot.cpp @@ -1854,8 +1854,7 @@ void NAV::Plot::afterCreateLink(OutputPin& startPin, InputPin& endPin) } else if (startPin.dataIdentifier.front() == WiFiPositioningSolution::type()) { - for (const auto& desc : SppSolution::GetStaticDataDescriptors()) { _pinData.at(pinIndex).addPlotDataItem(i++, desc); } - _pinData.at(pinIndex).dynamicDataStartIndex = static_cast(i); + for (const auto& desc : WiFiPositioningSolution::GetStaticDataDescriptors()) { _pinData.at(pinIndex).addPlotDataItem(i++, desc); } } else if (startPin.dataIdentifier.front() == RtklibPosObs::type()) { diff --git a/src/Nodes/Plotting/Plot.hpp b/src/Nodes/Plotting/Plot.hpp index 2b3aba46e..fba6615c1 100644 --- a/src/Nodes/Plotting/Plot.hpp +++ b/src/Nodes/Plotting/Plot.hpp @@ -42,6 +42,7 @@ #include "NodeData/State/InsGnssLCKFSolution.hpp" #include "NodeData/State/PosVelAtt.hpp" #include "NodeData/State/InsGnssTCKFSolution.hpp" +#include "NodeData/WiFi/WiFiPositioningSolution.hpp" namespace NAV { From 7ee27ec5368ad55f04024b136c8b40356834daea Mon Sep 17 00:00:00 2001 From: Roland Lintz Date: Sun, 21 Jul 2024 16:36:15 +0200 Subject: [PATCH 26/27] replace json files --- .vscode/launch.json | 4 +- .vscode/settings.json | 2 +- .vscode/tasks.json | 269 +++++++++++++++++++++++------------------- 3 files changed, 151 insertions(+), 124 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index d70a9e618..56d0b17c1 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -2,7 +2,7 @@ "version": "0.2.0", "configurations": [ { - "name": "Mac Debug", + "name": "Linux Debug", "type": "cppdbg", "request": "launch", "preLaunchTask": "MAIN: Build project", @@ -26,7 +26,7 @@ "cwd": "${workspaceFolder}", "environment": [], "externalConsole": false, - "MIMode": "lldb", + "MIMode": "gdb", "setupCommands": [ { "description": "Enable pretty-printing for gdb", diff --git a/.vscode/settings.json b/.vscode/settings.json index 7b3e19f14..e757025ec 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,6 @@ { "[cpp]": { - "editor.defaultFormatter": "ms-vscode.cpptools" + "editor.defaultFormatter": "xaver.clang-format" }, "[c]": { "editor.defaultFormatter": "ms-vscode.cpptools" diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 68014b513..2d6197236 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -1,14 +1,17 @@ { "version": "2.0.0", "tasks": [ - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ CLEAN Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // CLEAN: Remove Build Files "label": "CLEAN: Remove Build Files", "type": "shell", "group": "build", "command": "rm", "args": [ "-rf", - "build" + "build", ], "windows": { "command": "Remove-Item", @@ -23,16 +26,16 @@ "clear": true, "showReuseMessage": false, }, - "problemMatcher": [] + "problemMatcher": [], }, - { + { // CLEAN: Tests "label": "CLEAN: Tests", "type": "shell", "group": "build", "command": "rm", "args": [ "-rf", - "test/logs/[^\\.]*" + "test/logs/[^\\.]*", ], "windows": { "command": "Get-ChildItem", @@ -60,9 +63,12 @@ "clear": true, "showReuseMessage": false, }, - "problemMatcher": [] + "problemMatcher": [], }, - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ Conan Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // Conan: Detect "label": "Conan: Detect", "type": "shell", "group": "build", @@ -86,11 +92,11 @@ }, }, "presentation": { - "clear": true + "clear": true, }, - "problemMatcher": [] + "problemMatcher": [], }, - { + { // Conan "label": "Conan", "type": "shell", "group": "build", @@ -118,11 +124,11 @@ }, }, "dependsOn": [ - "Conan: Detect" + "Conan: Detect", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // Conan: Debug "label": "Conan: Debug", "type": "shell", "group": "build", @@ -150,11 +156,11 @@ }, }, "dependsOn": [ - "Conan: Detect" + "Conan: Detect", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // Conan: Release "label": "Conan: Release", "type": "shell", "group": "build", @@ -182,11 +188,14 @@ }, }, "dependsOn": [ - "Conan: Detect" + "Conan: Detect", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ CMAKE Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // CMAKE: Main "label": "CMAKE: Main", "type": "shell", "group": "build", @@ -214,16 +223,16 @@ "options": { "env": { "VSLANG": "1033", - "chcp": "1252" + "chcp": "1252", } - } + }, }, "dependsOn": [ - "Conan" + "Conan", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // CMAKE: Testing "label": "CMAKE: Testing", "type": "shell", "group": "build", @@ -246,21 +255,21 @@ "-DENABLE_CLANG_TIDY=OFF", "-DENABLE_CPPCHECK=OFF", "-DENABLE_INCLUDE_WHAT_YOU_USE=OFF", - "-DWARNINGS_AS_ERRORS=OFF" + "-DWARNINGS_AS_ERRORS=OFF", ], "options": { "env": { "VSLANG": "1033", - "chcp": "1252" + "chcp": "1252", } - } + }, }, "dependsOn": [ - "Conan" + "Conan", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // CMAKE: Coverage "label": "CMAKE: Coverage", "type": "shell", "group": "build", @@ -287,11 +296,11 @@ } }, "dependsOn": [ - "Conan: Debug" + "Conan: Debug", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // CMAKE: Documentation "label": "CMAKE: Documentation", "type": "shell", "group": "build", @@ -301,11 +310,11 @@ "export CC=${input:Compiler} && export CXX=$([[ \"${input:Compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/Release-doc-${input:Compiler} -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=ON -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=OFF -DENABLE_INCLUDE_WHAT_YOU_USE=OFF -DDOC_CHECK_CODE_DOCUMENTATION=NO -DUSE_ALTERNATE_LINKER=${input:Linker}", ], "dependsOn": [ - "Conan: Release" + "Conan: Release", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // CMAKE: Documentation Check "label": "CMAKE: Documentation Check", "type": "shell", "group": "build", @@ -315,11 +324,14 @@ "export CC=${input:Compiler} && export CXX=$([[ \"${input:Compiler}\" == \"gcc\" ]] && echo g++ || echo clang++) && cmake -Bbuild/Release-doc-${input:Compiler} -S. -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=build/Release/generators/conan_toolchain.cmake -DENABLE_MAIN=OFF -DENABLE_TESTING=OFF -DENABLE_DOXYGEN=ON -DENABLE_CLANG_TIDY=OFF -DENABLE_CPPCHECK=OFF -DLOG_LEVEL=OFF -DENABLE_INCLUDE_WHAT_YOU_USE=OFF -DDOC_CHECK_CODE_DOCUMENTATION=YES -DUSE_ALTERNATE_LINKER=${input:Linker}", ], "dependsOn": [ - "Conan: Release" + "Conan: Release", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ CMAKE Configure Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // CMAKE: Configure "label": "CMAKE: Configure", "type": "shell", "group": "build", @@ -339,11 +351,14 @@ "focus": true }, "dependsOn": [ - "CMAKE: Main" + "CMAKE: Main", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ Build Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // MAIN: Build project "label": "MAIN: Build project", "type": "shell", "group": "build", @@ -373,16 +388,16 @@ "options": { "env": { "VSLANG": "1033", - "chcp": "1252" + "chcp": "1252", } - } + }, }, "dependsOn": [ - "CMAKE: Main" + "CMAKE: Main", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // TEST: Build project "label": "TEST: Build project", "type": "shell", "group": "build", @@ -412,16 +427,16 @@ "options": { "env": { "VSLANG": "1033", - "chcp": "1252" + "chcp": "1252", } - } + }, }, "dependsOn": [ - "CMAKE: Testing" + "CMAKE: Testing", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Build Coverage Report "label": "COVERAGE: Build Coverage Report", "type": "shell", "group": "build", @@ -440,11 +455,11 @@ ], }, "dependsOn": [ - "CMAKE: Coverage" + "CMAKE: Coverage", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // DOXYGEN: Build Documentation "label": "DOXYGEN: Build Documentation", "type": "shell", "group": "build", @@ -453,14 +468,14 @@ "--build", "build/Release-doc-${input:Compiler}", "--target", - "doc" + "doc", ], "dependsOn": [ - "CMAKE: Documentation" + "CMAKE: Documentation", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // DOXYGEN: Check for errors "label": "DOXYGEN: Check for errors", "type": "shell", "group": "test", @@ -469,14 +484,17 @@ "--build", "build/Release-doc-${input:Compiler}", "--target", - "doc" + "doc", ], "dependsOn": [ - "CMAKE: Documentation Check" + "CMAKE: Documentation Check", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ Run Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // MAIN: Build & run project "label": "MAIN: Build & run project", "type": "shell", "group": { @@ -506,14 +524,14 @@ // "--output-path=test/logs", ], "options": { - "cwd": "${workspaceFolder}" + "cwd": "${workspaceFolder}", }, "dependsOn": [ - "MAIN: Build project" + "MAIN: Build project", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // TEST: Build & run "label": "TEST: Build & run", "type": "shell", "group": { @@ -522,7 +540,11 @@ }, "command": "ctest", "args": [ - "--output-on-failure" + // "-j4", + "--output-on-failure", + // "--tests-regex", + // "'PVAError|PosVelAtt'", + // "--verbose", ], "options": { "cwd": "${workspaceFolder}/build/${input:Build-Type}-${input:Compiler}", @@ -533,11 +555,11 @@ }, }, "dependsOn": [ - "TEST: Build project" + "TEST: Build project", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // TEST: Build & run w/o flow tests "label": "TEST: Build & run w/o flow tests", "type": "shell", "group": "test", @@ -555,28 +577,29 @@ "cwd": "${workspaceFolder}/build/${input:Build-Type}-${input:Compiler}", }, "dependsOn": [ - "TEST: Build project" + "TEST: Build project", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Run Coverage Tests "label": "COVERAGE: Run Coverage Tests", "type": "shell", "group": "test", "command": "ctest", "args": [ "-j4", - "--output-on-failure" + "--output-on-failure", + // "--verbose", ], "options": { - "cwd": "${workspaceFolder}/build/Debug-cov" + "cwd": "${workspaceFolder}/build/Debug-cov", }, "dependsOn": [ - "COVERAGE: Build Coverage Report" + "COVERAGE: Build Coverage Report", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Run Coverage Tests w/o flow tests "label": "COVERAGE: Run Coverage Tests w/o flow tests", "type": "shell", "group": "test", @@ -586,83 +609,87 @@ "--output-on-failure", "--exclude-regex", "'\\[flow\\]'" + // "--verbose", ], "options": { - "cwd": "${workspaceFolder}/build/Debug-cov" + "cwd": "${workspaceFolder}/build/Debug-cov", }, "dependsOn": [ - "COVERAGE: Build Coverage Report" + "COVERAGE: Build Coverage Report", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Create "label": "COVERAGE: Create", "type": "shell", "group": "test", "command": "bash", "args": [ "-c", - "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg" + "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg", ], "dependsOn": [ - "COVERAGE: Run Coverage Tests" + "COVERAGE: Run Coverage Tests", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Create w/o flow tests "label": "COVERAGE: Create w/o flow tests", "type": "shell", "group": "test", "command": "bash", "args": [ "-c", - "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg" + "mkdir -p build/coverage && gcovr --config doc/coverage/gcovr.cfg", ], "dependsOn": [ - "COVERAGE: Run Coverage Tests w/o flow tests" + "COVERAGE: Run Coverage Tests w/o flow tests", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Create & Show "label": "COVERAGE: Create & Show", "type": "shell", "group": "build", "command": "xdg-open", "args": [ - "build/coverage/index.html" + "build/coverage/index.html", ], "dependsOn": [ - "COVERAGE: Create" + "COVERAGE: Create", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // COVERAGE: Create & Show w/o flow tests "label": "COVERAGE: Create & Show w/o flow tests", "type": "shell", "group": "build", "command": "xdg-open", "args": [ - "build/coverage/index.html" + "build/coverage/index.html", ], "dependsOn": [ - "COVERAGE: Create w/o flow tests" + "COVERAGE: Create w/o flow tests", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // DOXYGEN: Create & Show "label": "DOXYGEN: Create & Show", "type": "shell", "group": "build", "command": "xdg-open", "args": [ - "build/doc/html/index.html" + "build/doc/html/index.html", ], "dependsOn": [ - "DOXYGEN: Build Documentation" + "DOXYGEN: Build Documentation", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ + // ║ Debugging Tasks ║ + // ╚══════════════════════════════════════════════════════════════════════════════════════════╝ + { // Gperftools: Run profiler "label": "Gperftools: Run profiler", "type": "shell", "group": "test", @@ -689,7 +716,7 @@ // "--output-path=test/logs", ], "options": { - "cwd": "${workspaceFolder}" + "cwd": "${workspaceFolder}", }, "linux": { "options": { @@ -700,11 +727,11 @@ } }, "dependsOn": [ - "MAIN: Build project" + "MAIN: Build project", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // Gperftools: Show profiled output "label": "Gperftools: Show profiled output", "type": "shell", "group": "test", @@ -718,11 +745,11 @@ // "build/bin/${input:Build-Type}/callgrind.out", // For --callgrind option ], "options": { - "cwd": "${workspaceFolder}" + "cwd": "${workspaceFolder}", }, - "problemMatcher": [] + "problemMatcher": [], }, - { + { // VALGRIND: Run profiler "label": "VALGRIND: Run profiler", "type": "shell", "group": "test", @@ -732,14 +759,14 @@ "valgrind --callgrind-out-file=build/bin/${input:Build-Type}/callgrind.out --tool=callgrind build/bin/${input:Build-Type}/instinct -f config.ini --nogui -l flow/Default.flow && kcachegrind build/bin/${input:Build-Type}/callgrind.out", ], "options": { - "cwd": "${workspaceFolder}" + "cwd": "${workspaceFolder}", }, "dependsOn": [ - "MAIN: Build project" + "MAIN: Build project", ], - "problemMatcher": [] + "problemMatcher": [], }, - { + { // VALGRIND: Show profiled output "label": "VALGRIND: Show profiled output", "type": "shell", "group": "test", @@ -748,11 +775,11 @@ "build/bin/${input:Build-Type}/callgrind.out", ], "options": { - "cwd": "${workspaceFolder}" + "cwd": "${workspaceFolder}", }, - "problemMatcher": [] + "problemMatcher": [], }, - { + { // VALGRIND: Memory Leak Check "label": "VALGRIND: Memory Leak Check", "type": "shell", "group": "test", @@ -764,15 +791,15 @@ "config.ini", "--nogui", "-l", - "flow/Default.flow" + "flow/Default.flow", ], "options": { - "cwd": "${workspaceFolder}" + "cwd": "${workspaceFolder}", }, "dependsOn": [ - "MAIN: Build project" + "MAIN: Build project", ], - "problemMatcher": [] + "problemMatcher": [], } ], // ╔══════════════════════════════════════════════════════════════════════════════════════════╗ From c82705287b5cd086a9a203c1cda8e681bd927981 Mon Sep 17 00:00:00 2001 From: rolandlintz <130669222+rolandlintz@users.noreply.github.com> Date: Tue, 6 Aug 2024 13:43:07 +0200 Subject: [PATCH 27/27] Update src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp Co-authored-by: ToppDev --- src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp index e9dd34602..152204244 100644 --- a/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp +++ b/src/Nodes/DataProvider/WiFi/Sensors/ArubaSensor.cpp @@ -301,7 +301,7 @@ void NAV::ArubaSensor::readSensorDataThread(void* userData) std::istringstream iss(receivedData); std::string line; - while (std::getline(iss, line) && line.find("Peer-bssid") == std::string::npos); // Skip lines until the header "Peer-bssid" is found + while (std::getline(iss, line) && line.find("Peer-bssid") == std::string::npos) {} // Skip lines until the header "Peer-bssid" is found // Skip the header lines std::getline(iss, line);