Feat/swarinfo 619 angle parameters (#63)

* DTO * Integrate with other messages * Update max pdoa slopes * bump * Remove TDOA * Add board orientation offset * Update common.cmake
SwarmUS · Nov 14, 2021 · 026accd · 026accd
1 parent 0d23dd8
commit 026accd
Show file tree

Hide file tree

Showing 11 changed files with 305 additions and 4 deletions.
diff --git a/cmake/pheromones/common.cmake b/cmake/pheromones/common.cmake
@@ -13,7 +13,7 @@ function(pheromones_fetch_populate)
         ${PROJECT_NAME}_pheromones
 
         GIT_REPOSITORY  https://github.com/SwarmUS/Pheromones
-        GIT_TAG         0d1b296
+        GIT_TAG         1846e35
         GIT_PROGRESS    TRUE
     )
 

diff --git a/src/pheromones/CMakeLists.txt b/src/pheromones/CMakeLists.txt
@@ -72,6 +72,7 @@ set(LIB_SOURCES
     src/interloc/InterlocRxFrameRawAngleDataDTO.cpp
     src/interloc/ConfigureInterlocDumpsDTO.cpp
     src/interloc/InterlocDumpDTO.cpp
+    src/interloc/ConfigureAngleParametersDTO.cpp
 
     src/HiveConnectNetworkAccessDTO.cpp
     src/HiveConnectRootNode.cpp
@@ -144,6 +145,7 @@ set(LIB_HEADERS
     include/pheromones/interloc/InterlocOutputMessageDTO.h
     include/pheromones/interloc/ConfigureInterlocDumpsDTO.h
     include/pheromones/interloc/InterlocDumpDTO.h
+    include/pheromones/interloc/ConfigureAngleParametersDTO.h
 
     include/pheromones/HiveConnectNetworkAccessDTO.h
     include/pheromones/HiveConnectRootNodeDTO.h

diff --git a/src/pheromones/DefaultPheromonesOptions.cmake b/src/pheromones/DefaultPheromonesOptions.cmake
@@ -17,3 +17,6 @@ set(NETWORK_PASSWORD_MAX_LENGTH 63) # WPA2 password max characters
 set_default(INTERLOC_BEEBOARDS_SIZE 3)
 set_default(INTERLOC_RAW_ANGLE_FRAMES_SIZE 3)
 set_default(INTERLOC_DUMP_MAX_UPDATES_SIZE 20)
+set_default(INTERLOC_ANTENNAS_PER_PAIR 2)
+set_default(INTERLOC_MAX_ANTENNA_PAIRS 3)
+set_default(INTERLOC_MAX_PDOA_SLOPES 2)
diff --git a/src/pheromones/include/pheromones/PheromonesSettings.in.h b/src/pheromones/include/pheromones/PheromonesSettings.in.h
@@ -21,6 +21,10 @@
 #define INTERLOC_BEEBOARDS_SIZE (@INTERLOC_BEEBOARDS_SIZE@)
 #define INTERLOC_RAW_ANGLE_FRAMES_SIZE (@INTERLOC_RAW_ANGLE_FRAMES_SIZE@)
 #define INTERLOC_DUMP_MAX_UPDATES_SIZE (@INTERLOC_DUMP_MAX_UPDATES_SIZE@)
+#define  INTERLOC_ANTENNAS_PER_PAIR (@INTERLOC_ANTENNAS_PER_PAIR@)
+#define INTERLOC_MAX_ANTENNA_PAIRS (@INTERLOC_MAX_ANTENNA_PAIRS@)
+#define INTERLOC_MAX_TDOA_SLOPES (@INTERLOC_MAX_TDOA_SLOPES@)
+#define INTERLOC_MAX_PDOA_SLOPES (@INTERLOC_MAX_PDOA_SLOPES@)
 
 // clang-format on
 #endif // __PHEROMONESSETTINGS_IN_H_
diff --git a/src/pheromones/include/pheromones/interloc/ConfigureAngleParametersDTO.h b/src/pheromones/include/pheromones/interloc/ConfigureAngleParametersDTO.h
@@ -0,0 +1,55 @@
+#ifndef __CONFIGUREANGLEPARAMETERSDTO_H__
+#define __CONFIGUREANGLEPARAMETERSDTO_H__
+
+#include <array>
+#include <cstdint>
+#include <message.pb.h>
+#include <pheromones/PheromonesSettings.h>
+
+class ConfigureAngleParametersDTO {
+  public:
+    ConfigureAngleParametersDTO(const ConfigureAngleParameters& message);
+
+    uint8_t getPairId() const;
+
+    const std::array<uint8_t, INTERLOC_ANTENNAS_PER_PAIR>& getAntennas() const;
+    uint8_t getAntennasLength() const;
+
+    const std::array<uint8_t, INTERLOC_MAX_ANTENNA_PAIRS>& getSlopeDecision() const;
+    uint8_t getSlopeDecisionLength() const;
+
+    const std::array<float, INTERLOC_MAX_PDOA_SLOPES>& getPdoaSlopes() const;
+    uint8_t getPdoaSlopesLength() const;
+
+    const std::array<float, INTERLOC_MAX_PDOA_SLOPES>& getPdoaIntercepts() const;
+    uint8_t getPdoaInterceptsLength() const;
+
+    float getPdoaNormalizationFactor() const;
+
+    float getBoardOrientationOffset() const;
+
+    /**
+     *@brief serialize a Message for nanopb, sets the fields properly before using
+     *pb_encode
+     *@param [out] message the message to serialize
+     *@return a boolean, true if successfull (fields were recognized) false if not */
+    bool serialize(ConfigureAngleParameters& message) const;
+
+  private:
+    uint8_t m_pairId;
+
+    std::array<uint8_t, INTERLOC_ANTENNAS_PER_PAIR> m_antennas;
+    uint8_t m_antennasLength;
+    std::array<uint8_t, INTERLOC_MAX_ANTENNA_PAIRS> m_slopeDecision;
+    uint8_t m_slopeDecisionLength;
+
+    std::array<float, INTERLOC_MAX_PDOA_SLOPES> m_pdoaSlopes;
+    uint8_t m_pdoaSlopesLength;
+    std::array<float, INTERLOC_MAX_PDOA_SLOPES> m_pdoaIntercepts;
+    uint8_t m_pdoaInterceptsLength;
+    float m_pdoaNormalizationFactor;
+
+    float m_boardOientationOffset;
+};
+
+#endif //__CONFIGUREANGLEPARAMETERSDTO_H__
diff --git a/src/pheromones/include/pheromones/interloc/InterlocConfigurationDTO.h b/src/pheromones/include/pheromones/interloc/InterlocConfigurationDTO.h
@@ -2,6 +2,7 @@
 #define __INTERLOCCONFIGURATIONDTO_H__
 
 #include "ConfigureAngleCalibrationDTO.h"
+#include "ConfigureAngleParametersDTO.h"
 #include "ConfigureInterlocDumpsDTO.h"
 #include "ConfigureTWRCalibrationDTO.h"
 #include <message.pb.h>
@@ -17,6 +18,7 @@ class InterlocConfigurationDTO {
     InterlocConfigurationDTO(const ConfigureAngleCalibrationDTO& configureDTO);
     InterlocConfigurationDTO(const ConfigureTWRCalibrationDTO& configureDTO);
     InterlocConfigurationDTO(const ConfigureInterlocDumpsDTO& configureDTO);
+    InterlocConfigurationDTO(const ConfigureAngleParametersDTO& configureDTO);
 
     /**
      * @brief Returns the inner configuration message
@@ -25,7 +27,8 @@ class InterlocConfigurationDTO {
     const std::variant<std::monostate,
                        ConfigureAngleCalibrationDTO,
                        ConfigureTWRCalibrationDTO,
-                       ConfigureInterlocDumpsDTO>&
+                       ConfigureInterlocDumpsDTO,
+                       ConfigureAngleParametersDTO>&
     getConfigurationMessage() const;
 
     /**
@@ -39,7 +42,8 @@ class InterlocConfigurationDTO {
     std::variant<std::monostate,
                  ConfigureAngleCalibrationDTO,
                  ConfigureTWRCalibrationDTO,
-                 ConfigureInterlocDumpsDTO>
+                 ConfigureInterlocDumpsDTO,
+                 ConfigureAngleParametersDTO>
         m_configMessage;
 };
 

diff --git a/src/pheromones/src/interloc/ConfigureAngleParametersDTO.cpp b/src/pheromones/src/interloc/ConfigureAngleParametersDTO.cpp
@@ -0,0 +1,101 @@
+#include "interloc/ConfigureAngleParametersDTO.h"
+
+ConfigureAngleParametersDTO::ConfigureAngleParametersDTO(const ConfigureAngleParameters& message) {
+    m_pairId = message.anglePairId;
+
+    m_antennasLength =
+        message.antennas_count <= INTERLOC_ANTENNAS_PER_PAIR ? message.antennas_count : 0;
+    for (unsigned int i = 0; i < m_antennasLength; i++) {
+        m_antennas[i] = message.antennas[i];
+    }
+
+    m_slopeDecisionLength =
+        message.slopeDecision_count <= INTERLOC_MAX_ANTENNA_PAIRS ? message.slopeDecision_count : 0;
+    for (unsigned int i = 0; i < m_slopeDecisionLength; i++) {
+        m_slopeDecision[i] = message.slopeDecision[i];
+    }
+
+    m_pdoaSlopesLength =
+        message.pdoaSlopes_count <= INTERLOC_MAX_PDOA_SLOPES ? message.pdoaSlopes_count : 0;
+    for (unsigned int i = 0; i < m_pdoaSlopesLength; i++) {
+        m_pdoaSlopes[i] = message.pdoaSlopes[i];
+    }
+    m_pdoaInterceptsLength =
+        message.pdoaIntercepts_count <= INTERLOC_MAX_PDOA_SLOPES ? message.pdoaIntercepts_count : 0;
+    for (unsigned int i = 0; i < m_pdoaInterceptsLength; i++) {
+        m_pdoaIntercepts[i] = message.pdoaIntercepts[i];
+    }
+
+    m_pdoaNormalizationFactor = message.pdoaNormalizationFactor;
+    m_boardOientationOffset = message.boardOrientationOffset;
+}
+
+uint8_t ConfigureAngleParametersDTO::getPairId() const { return m_pairId; }
+
+const std::array<uint8_t, INTERLOC_ANTENNAS_PER_PAIR>& ConfigureAngleParametersDTO::getAntennas()
+    const {
+    return m_antennas;
+}
+
+uint8_t ConfigureAngleParametersDTO::getAntennasLength() const { return m_antennasLength; }
+
+const std::array<uint8_t, INTERLOC_MAX_ANTENNA_PAIRS>& ConfigureAngleParametersDTO::
+    getSlopeDecision() const {
+    return m_slopeDecision;
+}
+
+uint8_t ConfigureAngleParametersDTO::getSlopeDecisionLength() const {
+    return m_slopeDecisionLength;
+}
+
+const std::array<float, INTERLOC_MAX_PDOA_SLOPES>& ConfigureAngleParametersDTO::getPdoaSlopes()
+    const {
+    return m_pdoaSlopes;
+}
+
+uint8_t ConfigureAngleParametersDTO::getPdoaSlopesLength() const { return m_pdoaSlopesLength; }
+
+float ConfigureAngleParametersDTO::getPdoaNormalizationFactor() const {
+    return m_pdoaNormalizationFactor;
+}
+
+uint8_t ConfigureAngleParametersDTO::getPdoaInterceptsLength() const {
+    return m_pdoaInterceptsLength;
+}
+
+const std::array<float, INTERLOC_MAX_PDOA_SLOPES>& ConfigureAngleParametersDTO::getPdoaIntercepts()
+    const {
+    return m_pdoaIntercepts;
+}
+
+float ConfigureAngleParametersDTO::getBoardOrientationOffset() const {
+    return m_boardOientationOffset;
+}
+
+bool ConfigureAngleParametersDTO::serialize(ConfigureAngleParameters& message) const {
+    message.anglePairId = m_pairId;
+
+    message.antennas_count = m_antennasLength;
+    for (unsigned int i = 0; i < m_antennasLength; i++) {
+        message.antennas[i] = m_antennas[i];
+    }
+
+    message.slopeDecision_count = m_slopeDecisionLength;
+    for (unsigned int i = 0; i < m_slopeDecisionLength; i++) {
+        message.slopeDecision[i] = m_slopeDecision[i];
+    }
+
+    message.pdoaSlopes_count = m_pdoaSlopesLength;
+    for (unsigned int i = 0; i < m_pdoaSlopesLength; i++) {
+        message.pdoaSlopes[i] = m_pdoaSlopes[i];
+    }
+    message.pdoaNormalizationFactor = m_pdoaNormalizationFactor;
+    message.pdoaIntercepts_count = m_pdoaInterceptsLength;
+    for (unsigned int i = 0; i < m_pdoaInterceptsLength; i++) {
+        message.pdoaIntercepts[i] = m_pdoaIntercepts[i];
+    }
+
+    message.boardOrientationOffset = m_boardOientationOffset;
+
+    return true;
+}
diff --git a/src/pheromones/src/interloc/InterlocConfigurationDTO.cpp b/src/pheromones/src/interloc/InterlocConfigurationDTO.cpp
@@ -19,6 +19,11 @@ InterlocConfigurationDTO::InterlocConfigurationDTO(const InterlocConfiguration&
             ConfigureInterlocDumpsDTO(message.configurationMessage.configureInterlocDumps);
         break;
 
+    case InterlocConfiguration_configureAngleParameters_tag:
+        m_configMessage =
+            ConfigureAngleParametersDTO(message.configurationMessage.configureAngleParameters);
+        break;
+
     default:
         m_configMessage = std::monostate();
     }
@@ -37,10 +42,16 @@ InterlocConfigurationDTO::InterlocConfigurationDTO(const ConfigureInterlocDumpsD
     m_configMessage = configureDTO;
 }
 
+InterlocConfigurationDTO::InterlocConfigurationDTO(
+    const ConfigureAngleParametersDTO& configureDTO) {
+    m_configMessage = configureDTO;
+}
+
 const std::variant<std::monostate,
                    ConfigureAngleCalibrationDTO,
                    ConfigureTWRCalibrationDTO,
-                   ConfigureInterlocDumpsDTO>&
+                   ConfigureInterlocDumpsDTO,
+                   ConfigureAngleParametersDTO>&
 InterlocConfigurationDTO::getConfigurationMessage() const {
     return m_configMessage;
 }
@@ -61,5 +72,10 @@ bool InterlocConfigurationDTO::serialize(InterlocConfiguration& message) const {
         return configMessage->serialize(message.configurationMessage.configureInterlocDumps);
     }
 
+    if (const auto* configMessage = std::get_if<ConfigureAngleParametersDTO>(&m_configMessage)) {
+        message.which_configurationMessage = InterlocConfiguration_configureAngleParameters_tag;
+        return configMessage->serialize(message.configurationMessage.configureAngleParameters);
+    }
+
     return false;
 }
diff --git a/src/pheromones/tests/CMakeLists.txt b/src/pheromones/tests/CMakeLists.txt
@@ -63,6 +63,7 @@ set(TEST_SOURCES
     interloc/InterlocOutputMessageDTOUnitTests.cpp
     interloc/ConfigureInterlocDumpsDTOUnitTests.cpp
     interloc/InterlocDumpDTOUnitTest.cpp
+    interloc/ConfigureAngleParametersDTOUnitTests.cpp
 
     HiveConnectNetworkAccessDTOTests.cpp
     HiveConnectRootNodeDTOTests.cpp

diff --git a/src/pheromones/tests/interloc/ConfigureAngleParametersDTOUnitTests.cpp b/src/pheromones/tests/interloc/ConfigureAngleParametersDTOUnitTests.cpp
@@ -0,0 +1,86 @@
+#include <gtest/gtest.h>
+#include <pheromones/interloc/ConfigureAngleParametersDTO.h>
+
+class ConfigureAngleParametersFixture : public testing::Test {
+  public:
+    void SetUp() override {}
+
+    void TearDown() override {}
+};
+
+TEST_F(ConfigureAngleParametersFixture, ConfigureInterlocDumps_deserialize_pairId) {
+    ConfigureAngleParameters msg;
+    memset(&msg, 0, sizeof(msg));
+    msg.anglePairId = 42;
+
+    auto dto = ConfigureAngleParametersDTO(msg);
+
+    EXPECT_EQ(dto.getPairId(), msg.anglePairId);
+}
+
+TEST_F(ConfigureAngleParametersFixture, ConfigureInterlocDumps_deserialize_slopeDecision) {
+    ConfigureAngleParameters msg;
+    memset(&msg, 0, sizeof(msg));
+
+    msg.slopeDecision[0] = 1;
+    msg.slopeDecision[1] = 2;
+    msg.slopeDecision_count = 2;
+
+    auto dto = ConfigureAngleParametersDTO(msg);
+
+    EXPECT_EQ(dto.getSlopeDecision()[0], msg.slopeDecision[0]);
+    EXPECT_EQ(dto.getSlopeDecision()[1], msg.slopeDecision[1]);
+    EXPECT_EQ(dto.getSlopeDecisionLength(), msg.slopeDecision_count);
+}
+
+TEST_F(ConfigureAngleParametersFixture, ConfigureInterlocDumps_deserialize_pdoaSlopes) {
+    ConfigureAngleParameters msg;
+    memset(&msg, 0, sizeof(msg));
+
+    msg.pdoaSlopes[0] = 1;
+    msg.pdoaSlopes[1] = 2;
+    msg.pdoaSlopes_count = 2;
+
+    auto dto = ConfigureAngleParametersDTO(msg);
+
+    EXPECT_EQ(dto.getPdoaSlopes()[0], msg.pdoaSlopes[0]);
+    EXPECT_EQ(dto.getPdoaSlopes()[1], msg.pdoaSlopes[1]);
+    EXPECT_EQ(dto.getPdoaSlopesLength(), msg.pdoaSlopes_count);
+}
+
+TEST_F(ConfigureAngleParametersFixture, ConfigureInterlocDumps_deserialize_pdoaNormalization) {
+    ConfigureAngleParameters msg;
+    memset(&msg, 0, sizeof(msg));
+
+    msg.pdoaNormalizationFactor = 1;
+
+    auto dto = ConfigureAngleParametersDTO(msg);
+
+    EXPECT_EQ(dto.getPdoaNormalizationFactor(), msg.pdoaNormalizationFactor);
+}
+
+TEST_F(ConfigureAngleParametersFixture, ConfigureInterlocDumps_deserialize_boardOffset) {
+    ConfigureAngleParameters msg;
+    memset(&msg, 0, sizeof(msg));
+
+    msg.boardOrientationOffset = 42.42;
+
+    auto dto = ConfigureAngleParametersDTO(msg);
+
+    EXPECT_EQ(dto.getBoardOrientationOffset(), msg.boardOrientationOffset);
+}
+
+TEST_F(ConfigureAngleParametersFixture, ConfigureInterlocDumps_deserialize_pdoaIntercepts) {
+    ConfigureAngleParameters msg;
+    memset(&msg, 0, sizeof(msg));
+
+    msg.pdoaIntercepts[0] = 1;
+    msg.pdoaIntercepts[1] = 2;
+    msg.pdoaIntercepts_count = 2;
+
+    auto dto = ConfigureAngleParametersDTO(msg);
+
+    EXPECT_EQ(dto.getPdoaIntercepts()[0], msg.pdoaIntercepts[0]);
+    EXPECT_EQ(dto.getPdoaIntercepts()[1], msg.pdoaIntercepts[1]);
+    EXPECT_EQ(dto.getPdoaInterceptsLength(), msg.pdoaIntercepts_count);
+}