add support to measure latency

include/crazyflie_cpp/Crazyflie.h

@@ -370,6 +370,14 @@ class Crazyflie
  void readUSDLogFile(
  std::vector<uint8_t>& data);
 #endif
+
+ // latency measurements
+ void setLatencyCallback(
+ std::function<void(uint64_t)> cb) {
+ m_latencyCallback = cb;
+ }
+ void triggerLatencyMeasurement();
+
 private:
  bitcraze::crazyflieLinkCpp::Packet waitForResponse(
  std::function<bool(const bitcraze::crazyflieLinkCpp::Packet&)> condition);
@@ -460,6 +468,12 @@ class Crazyflie
  Logger& m_logger;
 
  bitcraze::crazyflieLinkCpp::Connection m_connection;
+
+ // latency measurements
+ std::chrono::time_point<std::chrono::steady_clock> m_clock_start;
+ std::function<void(uint64_t)> m_latencyCallback;
+ uint32_t m_latencyCounter;
+
 };
 
 template<class T>

include/crazyflie_cpp/crtp.h

@@ -1003,6 +1003,29 @@ struct crtpGetDeviceTypeNameResponse
  static std::string name(const bitcraze::crazyflieLinkCpp::Packet &p);
 };
 
+// Port 15 (Link)
+class crtpLatencyMeasurementRequest
+ : public bitcraze::crazyflieLinkCpp::Packet
+{
+public:
+ crtpLatencyMeasurementRequest(
+ uint32_t id,
+ uint64_t timestamp)
+ : Packet(15, 0, 12)
+ {
+ setPayloadAt<uint32_t>(0, id);
+ setPayloadAt<uint64_t>(4, timestamp);
+ }
+};
+
+struct crtpLatencyMeasurementResponse
+{
+ static bool valid(const bitcraze::crazyflieLinkCpp::Packet &p);
+
+ static uint32_t id(const bitcraze::crazyflieLinkCpp::Packet &p);
+ static uint64_t timestamp(const bitcraze::crazyflieLinkCpp::Packet &p);
+};
+
 #if 0
 // The crazyflie-nrf firmware sends empty packets with the signal strength, if nothing else is in the queue
 struct crtpPlatformRSSIAck

src/Crazyflie.cpp

@@ -35,6 +35,8 @@ Crazyflie::Crazyflie(
  , m_protocolVersion(-1)
  , m_logger(logger)
  , m_connection(link_uri)
+ , m_clock_start(std::chrono::steady_clock::now())
+ , m_latencyCounter(0)
 {
 }
 
@@ -993,6 +995,21 @@ void Crazyflie::processPacket(const bitcraze::crazyflieLinkCpp::Packet& p)
  m_logger.warning("Received unrequested data for block: " + std::to_string((int)blockId));
  }
  }
+ else if (crtpLatencyMeasurementResponse::valid(p))
+ {
+ if (m_latencyCallback)
+ {
+ if (crtpLatencyMeasurementResponse::id(p) != m_latencyCounter-1) {
+ m_logger.warning("Received wrong latency id: " + std::to_string(crtpLatencyMeasurementResponse::id(p)));
+ } else {
+ auto now = std::chrono::steady_clock::now();
+ uint64_t recv_timestamp = std::chrono::duration_cast<std::chrono::microseconds>(now - m_clock_start).count();
+ uint64_t send_timestamp = crtpLatencyMeasurementResponse::timestamp(p);
+ uint64_t latency_in_us = recv_timestamp - send_timestamp;
+ m_latencyCallback(latency_in_us);
+ }
+ }
+ }
 }
 
 const Crazyflie::LogTocEntry* Crazyflie::getLogTocEntry(
@@ -1137,6 +1154,16 @@ void Crazyflie::readUSDLogFile(
  throw std::runtime_error("Could not find MemoryTypeUSD!");
 }
 #endif
+
+ void Crazyflie::triggerLatencyMeasurement()
+ {
+ auto now = std::chrono::steady_clock::now();
+ uint64_t timestamp = std::chrono::duration_cast<std::chrono::microseconds>(now - m_clock_start).count();
+ crtpLatencyMeasurementRequest req(m_latencyCounter, timestamp);
+ m_connection.send(req);
+
+ ++m_latencyCounter;
+ }
 ////////////////////////////////////////////////////////////////
 
 CrazyflieBroadcaster::CrazyflieBroadcaster(

src/crtp.cpp

@@ -445,4 +445,21 @@ crtpParamSetByNameRequest<float>::crtpParamSetByNameRequest(
  : crtpParamSetByNameRequest(group, name, ParamTypeFloat, &value, sizeof(float))
 {
 }
-#endif
+#endif
+
+bool crtpLatencyMeasurementResponse::valid(const bitcraze::crazyflieLinkCpp::Packet &p)
+{
+ return p.port() == 15 &&
+ p.channel() == 0 &&
+ p.payloadSize() == 12;
+}
+
+uint32_t crtpLatencyMeasurementResponse::id(const bitcraze::crazyflieLinkCpp::Packet &p)
+{
+ return p.payloadAt<uint32_t>(0);
+}
+
+uint64_t crtpLatencyMeasurementResponse::timestamp(const bitcraze::crazyflieLinkCpp::Packet &p)
+{
+ return p.payloadAt<uint64_t>(4);
+}