Introduce Link Statistics and Latency Measurement

cflib/crazyflie/__init__.py

@@ -44,6 +44,7 @@
 from .commander import Commander
 from .console import Console
 from .extpos import Extpos
+from .link_statistics import LinkStatistics
 from .localization import Localization
 from .log import Log
 from .mem import Memory
@@ -121,6 +122,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None):
         self.mem = Memory(self)
         self.platform = PlatformService(self)
         self.appchannel = Appchannel(self)
+        self.link_statistics = LinkStatistics(self)
 
         self.link_uri = ''
 
@@ -153,6 +155,11 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None):
         self.fully_connected.add_callback(
             lambda uri: logger.info('Callback->Connection completed [%s]', uri))
 
+        self.connected.add_callback(
+            lambda uri: self.link_statistics.start())
+        self.disconnected.add_callback(
+            lambda uri: self.link_statistics.stop())
+
     def _disconnected(self, link_uri):
         """ Callback when disconnected."""
         self.connected_ts = None
@@ -284,6 +291,14 @@ def remove_port_callback(self, port, cb):
         """Remove the callback cb on port"""
         self.incoming.remove_port_callback(port, cb)
 
+    def add_header_callback(self, cb, port, channel, port_mask=0xFF, channel_mask=0xFF):
+        """Add a callback to cb on port and channel"""
+        self.incoming.add_header_callback(cb, port, channel, port_mask, channel_mask)
+
+    def remove_header_callback(self, cb, port, channel, port_mask=0xFF, channel_mask=0xFF):
+        """Remove the callback cb on port and channel"""
+        self.incoming.remove_header_callback(cb, port, channel, port_mask, channel_mask)
+
     def _no_answer_do_retry(self, pk, pattern):
         """Resend packets that we have not gotten answers to"""
         logger.info('Resending for pattern %s', pattern)
@@ -380,9 +395,7 @@ def add_port_callback(self, port, cb):
     def remove_port_callback(self, port, cb):
         """Remove a callback for data that comes on a specific port"""
         logger.debug('Removing callback on port [%d] to [%s]', port, cb)
-        for port_callback in self.cb:
-            if port_callback.port == port and port_callback.callback == cb:
-                self.cb.remove(port_callback)
+        self.remove_header_callback(cb, port, 0, 0xff, 0x0)
 
     def add_header_callback(self, cb, port, channel, port_mask=0xFF,
                             channel_mask=0xFF):
@@ -394,6 +407,19 @@ def add_header_callback(self, cb, port, channel, port_mask=0xFF,
         self.cb.append(_CallbackContainer(port, port_mask,
                                           channel, channel_mask, cb))
 
+    def remove_header_callback(self, cb, port, channel, port_mask=0xFF,
+                               channel_mask=0xFF):
+        """
+        Remove a callback for a specific port/header callback with the
+        possibility to add a mask for channel and port for multiple
+        hits for same callback.
+        """
+        for port_callback in self.cb:
+            if port_callback.port == port and port_callback.port_mask == port_mask and \
+                    port_callback.channel == channel and port_callback.channel_mask == channel_mask and \
+                    port_callback.callback == cb:
+                self.cb.remove(port_callback)
+
     def run(self):
         while True:
             if self.cf.link is None:

cflib/crazyflie/link_statistics.py

@@ -0,0 +1,188 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2024 Bitcraze AB
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, in version 3.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+This module provides tools for tracking statistics related to the communication
+link between the Crazyflie and the lib. Currently, it focuses on tracking latency
+but is designed to be extended with additional link statistics in the future.
+"""
+import struct
+import time
+from threading import Event
+from threading import Thread
+
+import numpy as np
+
+from cflib.crtp.crtpstack import CRTPPacket
+from cflib.crtp.crtpstack import CRTPPort
+
+__author__ = 'Bitcraze AB'
+__all__ = ['LinkStatistics']
+
+PING_HEADER = 0x0
+ECHO_CHANNEL = 0
+
+
+class LinkStatistics:
+    """
+    LinkStatistics class manages the collection of various statistics related to the
+    communication link between the Crazyflie and the lib.
+
+    This class serves as a high-level manager, initializing and coordinating multiple
+    statistics trackers, such as Latency. It allows starting and stopping all
+    statistics trackers simultaneously. Future statistics can be added to extend
+    the class's functionality.
+
+    Attributes:
+        _cf (Crazyflie): A reference to the Crazyflie instance.
+        latency (Latency): An instance of the Latency class that tracks latency statistics.
+    """
+
+    def __init__(self, crazyflie):
+        self._cf = crazyflie
+
+        self.latency = Latency(self._cf)
+
+    def start(self):
+        """
+        Start collecting all statistics.
+        """
+        self.latency.start()
+
+    def stop(self):
+        """
+        Stop collecting all statistics.
+        """
+        self.latency.stop()
+
+
+class Latency:
+    """
+    The Latency class measures and tracks the latency of the communication link
+    between the Crazyflie and the lib.
+
+    This class periodically sends ping requests to the Crazyflie and tracks
+    the round-trip time (latency). It calculates and stores the 95th percentile
+    latency over a rolling window of recent latency measurements.
+
+    Attributes:
+        _cf (Crazyflie): A reference to the Crazyflie instance.
+        latency (float): The current calculated 95th percentile latency in milliseconds.
+        _stop_event (Event): An event object to control the stopping of the ping thread.
+        _ping_thread_instance (Thread): Thread instance for sending ping requests at intervals.
+    """
+
+    def __init__(self, crazyflie):
+        self._cf = crazyflie
+        self._cf.add_header_callback(self._ping_response, CRTPPort.LINKCTRL, 0)
+        self._stop_event = Event()
+        self._ping_thread_instance = Thread(target=self._ping_thread)
+        self.latency = 0
+
+    def start(self):
+        """
+        Start the latency tracking process.
+
+        This method initiates a background thread that sends ping requests
+        at regular intervals to measure and track latency statistics.
+        """
+        self._ping_thread_instance.start()
+
+    def stop(self):
+        """
+        Stop the latency tracking process.
+
+        This method stops the background thread and ceases sending further
+        ping requests, halting latency measurement.
+        """
+        self._stop_event.set()
+        self._ping_thread_instance.join()
+
+    def _ping_thread(self, interval: float = 1.0) -> None:
+        """
+        Background thread method that sends a ping to the Crazyflie at regular intervals.
+
+        This method runs in a separate thread and continues to send ping requests
+        until the stop event is set.
+
+        Args:
+            interval (float): The time (in seconds) to wait between ping requests. Default is 1 second.
+        """
+        while not self._stop_event.is_set():
+            self.ping()
+            time.sleep(interval)
+
+    def ping(self) -> None:
+        """
+        Send a ping request to the Crazyflie to measure latency.
+
+        A ping packet is sent to the Crazyflie with the current timestamp and a
+        header identifier to differentiate it from other echo responses. The latency
+        is calculated upon receiving the response.
+        """
+        ping_packet = CRTPPacket()
+        ping_packet.set_header(CRTPPort.LINKCTRL, ECHO_CHANNEL)
+
+        # Pack the current time as the ping timestamp
+        current_time = time.time()
+        ping_packet.data = struct.pack('<Bd', PING_HEADER, current_time)
+        self._cf.send_packet(ping_packet)
+
+    def _ping_response(self, packet):
+        """
+        Callback method for processing the echo response received from the Crazyflie.
+
+        This method is called when a ping response is received. It checks the header
+        to verify that it matches the sent ping header before calculating the latency
+        based on the timestamp included in the ping request.
+
+        Args:
+            packet (CRTPPacket): The packet received from the Crazyflie containing
+            the echo response data.
+        """
+        received_header, received_timestamp = struct.unpack('<Bd', packet.data)
+        if received_header != PING_HEADER:
+            return
+        self.latency = self._calculate_p95_latency(received_timestamp)
+
+    def _calculate_p95_latency(self, timestamp):
+        """
+        Calculate the 95th percentile latency based on recent ping measurements.
+
+        This method records the round-trip time for a ping response and maintains
+        a rolling window of latency values to compute the 95th percentile.
+
+        Args:
+            timestamp (float): The timestamp from the sent ping packet to calculate
+            the round-trip time.
+
+        Returns:
+            float: The updated 95th percentile latency in milliseconds.
+        """
+        if not hasattr(self, '_latencies'):
+            self._latencies = []
+
+        instantaneous_latency = (time.time() - timestamp) * 1000
+        self._latencies.append(instantaneous_latency)
+        if len(self._latencies) > 100:
+            self._latencies.pop(0)
+        p95_latency = np.percentile(self._latencies, 95)
+        return p95_latency