Merge pull request #104 from microsoft/handleRanging

Handle ranging data notification
microsoft · Mar 1, 2023 · 4819235 · 4819235
2 parents 6616c95 + 7264080
commit 4819235
Show file tree

Hide file tree

Showing 5 changed files with 83 additions and 1 deletion.
diff --git a/lib/uwb/UwbDevice.cxx b/lib/uwb/UwbDevice.cxx
@@ -111,7 +111,15 @@ UwbDevice::OnSessionRangingData(UwbRangingData rangingData)
         return;
     }
 
-    // TODO: implement this
+    PLOG_VERBOSE << "Session with id " << rangingData.SessionId << " processing new ranging data";
+    std::vector<UwbPeer> peersData;
+    peersData.reserve(rangingData.RangingMeasurements.size());
+    for (const auto& peerData : rangingData.RangingMeasurements) {
+        UwbPeer data{ peerData };
+        PLOG_VERBOSE << "Peer data: " << data.ToString();
+        peersData.push_back(std::move(data));
+    }
+    session->ProcessRangingData(peersData);
 }
 
 void

diff --git a/lib/uwb/UwbPeer.cxx b/lib/uwb/UwbPeer.cxx
@@ -1,4 +1,5 @@
 
+#include <cmath>
 #include <tuple>
 
 #include <notstd/tostring.hxx>
@@ -32,6 +33,49 @@ UwbPeer::UwbPeer(UwbMacAddress address) :
     m_address(std::move(address))
 {}
 
+/**
+ * @brief Converts a Q9.7-formatted value to an IEEE 754 double precision floating point formatted value.
+ *
+ * Assuming the Arm definition of Qm.n formatting, the most significant bit is the sign, the next
+ * (m-1) bits are an integer, and the next n bits is the number to be multiplied by pow(2,n)
+ * The double equivalent will be the sum of those two results
+ * TODO double check this conversion, write tests for it
+ *
+ * @param q97 a number in Q9.7 format
+ * @return double
+ */
+double
+ConvertQ97FormatToIEEE(uint16_t q97)
+{
+    static const double pow2 = std::pow(2, -7);
+    static const uint16_t signMask =            0b1000'0000'0000'0000U;
+    static const uint16_t unsignedIntegerMask = 0b0111'1111'1000'0000U;
+    static const uint16_t fractionMask = ~(signMask | unsignedIntegerMask);
+
+    bool sign = q97 & signMask;
+    int unsignedIntegerPart = (q97 & unsignedIntegerMask) >> 7U;
+    int fractionPart = q97 & fractionMask;
+
+    double unsignedNumber = ((double)unsignedIntegerPart) + (((double)fractionPart) * pow2);
+
+    return (sign ? -1 : 1) * unsignedNumber;
+}
+
+UwbPeer::UwbPeer(const uwb::protocol::fira::UwbRangingMeasurement& data) :
+    m_address{ data.PeerMacAddress },
+    m_spatialProperties{
+        .Distance{ data.Distance }, // TODO is this also q97
+        .AngleAzimuth{ ConvertQ97FormatToIEEE(data.AoAAzimuth.Result) },
+        .AngleElevation{ ConvertQ97FormatToIEEE(data.AoAElevation.Result) },
+        .Elevation{ ConvertQ97FormatToIEEE(data.AoaDestinationElevation.Result) }, // TODO is this right?
+
+        .AngleAzimuthFom{ data.AoAAzimuth.FigureOfMerit },
+        .AngleElevationFom{ data.AoAElevation.FigureOfMerit },
+        .ElevationFom{ data.AoaDestinationElevation.FigureOfMerit }
+    }
+{
+}
+
 UwbPeer::UwbPeer(const UwbPeer& other) :
     m_address(other.m_address),
     m_spatialProperties(other.m_spatialProperties)

diff --git a/lib/uwb/UwbSession.cxx b/lib/uwb/UwbSession.cxx
@@ -78,4 +78,14 @@ UwbSession::InsertPeerImpl(const uwb::UwbMacAddress& peerAddress)
 {
     m_peers.insert(peerAddress);
     PLOG_VERBOSE << "Session with id " << m_sessionId << " added peer via DDI with mac address " << peerAddress.ToString();
+}
+
+void
+UwbSession::ProcessRangingData(const std::vector<uwb::UwbPeer>& peerRangingData)
+{
+    auto callbacks = m_callbacks.lock();
+    if (callbacks) {
+        PLOG_VERBOSE << "Session with id " << m_sessionId << " processing peer ranging data";
+        callbacks->OnPeerPropertiesChanged(this, peerRangingData);
+    }
 }
diff --git a/lib/uwb/include/uwb/UwbPeer.hxx b/lib/uwb/include/uwb/UwbPeer.hxx
@@ -6,6 +6,7 @@
 #include <optional>
 
 #include <uwb/UwbMacAddress.hxx>
+#include <uwb/protocols/fira/FiraDevice.hxx>
 
 namespace uwb
 {
@@ -19,6 +20,10 @@ struct UwbPeerSpatialProperties
     std::optional<double> AngleElevation;
     std::optional<double> Elevation;
 
+    std::optional<uint8_t> AngleAzimuthFom;
+    std::optional<uint8_t> AngleElevationFom;
+    std::optional<uint8_t> ElevationFom;
+
     std::string
     ToString() const;
 
@@ -39,6 +44,13 @@ public:
      */
     explicit UwbPeer(UwbMacAddress address);
 
+    /**
+     * @brief Construct a new Uwb Peer object from UwbRangingMeasurement data
+     * 
+     * @param data 
+     */
+    explicit UwbPeer(const uwb::protocol::fira::UwbRangingMeasurement& data);
+
     /**
      * @brief Construct a new UwbPeer object from another.
      *

diff --git a/lib/uwb/include/uwb/UwbSession.hxx b/lib/uwb/include/uwb/UwbSession.hxx
@@ -118,6 +118,14 @@ public:
         }
     }
 
+    /**
+     * @brief Temporarily public function to call the UwbSessionEventCallbacks callback for new ranging data 
+     * 
+     * @param peerRangingData 
+     */
+    void
+    ProcessRangingData(const std::vector<uwb::UwbPeer>& peerRangingData);
+
 private:
     /**
      * @brief Internal function to insert a peer address to this session