Calculate latency, receive uplink RSSI, calculate bandwidth congestion and packet rate

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
@@ -101,6 +102,11 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None):
         self.packet_sent = Caller()
         # Called when the link driver updates the link quality measurement
         self.link_quality_updated = Caller()
+        self.uplink_rssi_updated = Caller()
+        self.uplink_rate_updated = Caller()
+        self.downlink_rate_updated = Caller()
+        self.uplink_congestion_updated = Caller()
+        self.downlink_congestion_updated = Caller()
 
         self.state = State.DISCONNECTED
 
@@ -123,6 +129,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 = ''
 
@@ -155,6 +162,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
@@ -208,9 +220,20 @@ def _link_error_cb(self, errmsg):
             self.disconnected_link_error.call(self.link_uri, errmsg)
         self.state = State.DISCONNECTED
 
-    def _link_quality_cb(self, percentage):
-        """Called from link driver to report link quality"""
-        self.link_quality_updated.call(percentage)
+    def _radio_link_statistics_cb(self, radio_link_statistics):
+        """Called from link driver to report radio link statistics"""
+        if 'link_quality' in radio_link_statistics:
+            self.link_quality_updated.call(radio_link_statistics['link_quality'])
+        if 'uplink_rssi' in radio_link_statistics:
+            self.uplink_rssi_updated.call(radio_link_statistics['uplink_rssi'])
+        if 'uplink_rate' in radio_link_statistics:
+            self.uplink_rate_updated.call(radio_link_statistics['uplink_rate'])
+        if 'downlink_rate' in radio_link_statistics:
+            self.downlink_rate_updated.call(radio_link_statistics['downlink_rate'])
+        if 'uplink_congestion' in radio_link_statistics:
+            self.uplink_congestion_updated.call(radio_link_statistics['uplink_congestion'])
+        if 'downlink_congestion' in radio_link_statistics:
+            self.downlink_congestion_updated.call(radio_link_statistics['downlink_congestion'])
 
     def _check_for_initial_packet_cb(self, data):
         """
@@ -233,7 +256,7 @@ def open_link(self, link_uri):
         self.link_uri = link_uri
         try:
             self.link = cflib.crtp.get_link_driver(
-                link_uri, self._link_quality_cb, self._link_error_cb)
+                link_uri, self._radio_link_statistics_cb, self._link_error_cb)
 
             if not self.link:
                 message = 'No driver found or malformed URI: {}' \
@@ -288,6 +311,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)
@@ -384,9 +415,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):
@@ -398,6 +427,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,196 @@
+# -*- 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
+from cflib.utils.callbacks import Caller
+
+__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 = None
+        self.latency = 0
+        self.latency_updated = Caller()
+
+    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.
+        """
+        if self._ping_thread_instance is None or not self._ping_thread_instance.is_alive():
+            self._stop_event.clear()
+            self._ping_thread_instance = Thread(target=self._ping_thread)
+            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()
+        if self._ping_thread_instance is not None:
+            self._ping_thread_instance.join()
+            self._ping_thread_instance = None
+
+    def _ping_thread(self, interval: float = 0.1) -> 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 0.1 seconds.
+        """
+        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)
+        self.latency_updated.call(self.latency)
+
+    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

cflib/crtp/__init__.py

@@ -89,14 +89,14 @@ def get_interfaces_status():
     return status
 
 
-def get_link_driver(uri, link_quality_callback=None, link_error_callback=None):
+def get_link_driver(uri, radio_link_statistics_callback=None, link_error_callback=None):
     """Return the link driver for the given URI. Returns None if no driver
     was found for the URI or the URI was not well formatted for the matching
     driver."""
     for driverClass in CLASSES:
         try:
             instance = driverClass()
-            instance.connect(uri, link_quality_callback, link_error_callback)
+            instance.connect(uri, radio_link_statistics_callback, link_error_callback)
             return instance
         except WrongUriType:
             continue

cflib/crtp/cflinkcppdriver.py

@@ -34,6 +34,7 @@
 
 from .crtpstack import CRTPPacket
 from cflib.crtp.crtpdriver import CRTPDriver
+from cflib.crtp.radio_link_statistics import RadioLinkStatistics
 
 __author__ = 'Bitcraze AB'
 __all__ = ['CfLinkCppDriver']
@@ -54,22 +55,23 @@ def __init__(self):
         self.needs_resending = False
 
         self._connection = None
+        self._radio_link_statistics = RadioLinkStatistics()
 
-    def connect(self, uri, link_quality_callback, link_error_callback):
+    def connect(self, uri, radio_link_statistics_callback, link_error_callback):
         """Connect the driver to a specified URI
 
         @param uri Uri of the link to open
-        @param link_quality_callback Callback to report link quality in percent
+        @param radio_link_statistics_callback Callback to report radio link statistics
         @param link_error_callback Callback to report errors (will result in
                disconnection)
         """
 
         self._connection = cflinkcpp.Connection(uri)
         self.uri = uri
-        self._link_quality_callback = link_quality_callback
+        self._radio_link_statistics_callback = radio_link_statistics_callback
         self._link_error_callback = link_error_callback
 
-        if uri.startswith('radio://') and link_quality_callback is not None:
+        if uri.startswith('radio://') and radio_link_statistics_callback is not None:
             self._last_connection_stats = self._connection.statistics
             self._recompute_link_quality_timer()
 
@@ -181,13 +183,13 @@ def _recompute_link_quality_timer(self):
         sent_count = stats.sent_count - self._last_connection_stats.sent_count
         ack_count = stats.ack_count - self._last_connection_stats.ack_count
         if sent_count > 0:
-            link_quality = min(ack_count, sent_count) / sent_count * 100.0
+            self._radio_link_statistics.link_quality = min(ack_count, sent_count) / sent_count * 100.0
         else:
-            link_quality = 1
+            self._radio_link_statistics.link_quality = 1
         self._last_connection_stats = stats
 
-        if self._link_quality_callback is not None:
-            self._link_quality_callback(link_quality)
+        if self._radio_link_statistics_callback is not None:
+            self._radio_link_statistics_callback(self._radio_link_statistics)
 
         if sent_count > 10 and ack_count == 0 and self._link_error_callback is not None:
             self._link_error_callback('Too many packets lost')