Receive uplink RSSI, calculate bandwidth congestion and packet rate

cflib/crazyflie/__init__.py

@@ -101,6 +101,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
 
@@ -208,9 +213,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 _signal_health_cb(self, signal_health):
+        """Called from link driver to report signal health"""
+        if 'link_quality' in signal_health:
+            self.link_quality_updated.call(signal_health['link_quality'])
+        if 'uplink_rssi' in signal_health:
+            self.uplink_rssi_updated.call(signal_health['uplink_rssi'])
+        if 'uplink_rate' in signal_health:
+            self.uplink_rate_updated.call(signal_health['uplink_rate'])
+        if 'downlink_rate' in signal_health:
+            self.downlink_rate_updated.call(signal_health['downlink_rate'])
+        if 'uplink_congestion' in signal_health:
+            self.uplink_congestion_updated.call(signal_health['uplink_congestion'])
+        if 'downlink_congestion' in signal_health:
+            self.downlink_congestion_updated.call(signal_health['downlink_congestion'])
 
     def _check_for_initial_packet_cb(self, data):
         """
@@ -233,7 +249,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._signal_health_cb, self._link_error_cb)
 
             if not self.link:
                 message = 'No driver found or malformed URI: {}' \

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, signal_health_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, signal_health_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.signal_health import SignalHealth
 
 __author__ = 'Bitcraze AB'
 __all__ = ['CfLinkCppDriver']
@@ -54,22 +55,23 @@ def __init__(self):
         self.needs_resending = False
 
         self._connection = None
+        self._signal_health = SignalHealth()
 
-    def connect(self, uri, link_quality_callback, link_error_callback):
+    def connect(self, uri, signal_health_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 signal_health_callback Callback to report signal health
         @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._signal_health_callback = signal_health_callback
         self._link_error_callback = link_error_callback
 
-        if uri.startswith('radio://') and link_quality_callback is not None:
+        if uri.startswith('radio://') and signal_health_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._signal_health.link_quality = min(ack_count, sent_count) / sent_count * 100.0
         else:
-            link_quality = 1
+            self._signal_health.link_quality = 1
         self._last_connection_stats = stats
 
-        if self._link_quality_callback is not None:
-            self._link_quality_callback(link_quality)
+        if self._signal_health_callback is not None:
+            self._signal_health_callback(self._signal_health)
 
         if sent_count > 10 and ack_count == 0 and self._link_error_callback is not None:
             self._link_error_callback('Too many packets lost')

cflib/crtp/crtpdriver.py

@@ -42,11 +42,11 @@ def __init__(self):
         """
         self.needs_resending = True
 
-    def connect(self, uri, link_quality_callback, link_error_callback):
+    def connect(self, uri, signal_health_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 signal_health_callback Callback to report signal health
         @param link_error_callback Callback to report errors (will result in
                disconnection)
         """

cflib/crtp/radiodriver.py

@@ -30,7 +30,6 @@
 """
 import array
 import binascii
-import collections
 import logging
 import queue
 import re
@@ -53,6 +52,7 @@
 from .crtpstack import CRTPPacket
 from .exceptions import WrongUriType
 from cflib.crtp.crtpdriver import CRTPDriver
+from cflib.crtp.signal_health import SignalHealth
 from cflib.drivers.crazyradio import Crazyradio
 
 
@@ -241,20 +241,20 @@ def __init__(self):
         self._radio = None
         self.uri = ''
         self.link_error_callback = None
-        self.link_quality_callback = None
+        self.signal_health_callback = None
         self.in_queue = None
         self.out_queue = None
         self._thread = None
         self.needs_resending = True
 
-    def connect(self, uri, link_quality_callback, link_error_callback):
+    def connect(self, uri, signal_health_callback, link_error_callback):
         """
         Connect the link driver to a specified URI of the format:
         radio://<dongle nbr>/<radio channel>/[250K,1M,2M]
 
-        The callback for linkQuality can be called at any moment from the
-        driver to report back the link quality in percentage. The
-        callback from linkError will be called when a error occurs with
+        The callback for signal health can be called at any moment from the
+        driver to report back the signal health. The callback from linkError
+        will be called when a error occurs with
         an error message.
         """
 
@@ -283,7 +283,7 @@ def connect(self, uri, link_quality_callback, link_error_callback):
         self._thread = _RadioDriverThread(self._radio,
                                           self.in_queue,
                                           self.out_queue,
-                                          link_quality_callback,
+                                          signal_health_callback,
                                           link_error_callback,
                                           self,
                                           rate_limit)
@@ -381,7 +381,7 @@ def restart(self):
 
         self._thread = _RadioDriverThread(self._radio, self.in_queue,
                                           self.out_queue,
-                                          self.link_quality_callback,
+                                          self.signal_health_callback,
                                           self.link_error_callback,
                                           self)
         self._thread.start()
@@ -401,7 +401,7 @@ def close(self):
 
         # Clear callbacks
         self.link_error_callback = None
-        self.link_quality_callback = None
+        self.signal_health_callback = None
 
     def _scan_radio_channels(self, radio: _SharedRadioInstance,
                              start=0, stop=125):
@@ -520,18 +520,16 @@ class _RadioDriverThread(threading.Thread):
     Crazyradio USB driver. """
 
     def __init__(self, radio, inQueue, outQueue,
-                 link_quality_callback, link_error_callback, link, rate_limit: Optional[int]):
+                 signal_health_callback, link_error_callback, link, rate_limit: Optional[int]):
         """ Create the object """
         threading.Thread.__init__(self)
         self._radio = radio
         self._in_queue = inQueue
         self._out_queue = outQueue
         self._sp = False
         self._link_error_callback = link_error_callback
-        self._link_quality_callback = link_quality_callback
+        self._signal_health = SignalHealth(signal_health_callback)
         self._retry_before_disconnect = _nr_of_retries
-        self._retries = collections.deque()
-        self._retry_sum = 0
         self.rate_limit = rate_limit
 
         self._curr_up = 0
@@ -607,16 +605,6 @@ def run(self):
                 logger.info('Dongle reported ACK status == None')
                 continue
 
-            if (self._link_quality_callback is not None):
-                # track the mean of a sliding window of the last N packets
-                retry = 10 - ackStatus.retry
-                self._retries.append(retry)
-                self._retry_sum += retry
-                if len(self._retries) > 100:
-                    self._retry_sum -= self._retries.popleft()
-                link_quality = float(self._retry_sum) / len(self._retries) * 10
-                self._link_quality_callback(link_quality)
-
             # If no copter, retry
             if ackStatus.ack is False:
                 self._retry_before_disconnect = \
@@ -631,6 +619,7 @@ def run(self):
 
             # If there is a copter in range, the packet is analysed and the
             # next packet to send is prepared
+            # TODO: This does not seem to work since there is always a byte filled in the data even with null packets
             if (len(data) > 0):
                 inPacket = CRTPPacket(data[0], list(data[1:]))
                 self._in_queue.put(inPacket)
@@ -667,8 +656,11 @@ def run(self):
                     else:
                         dataOut.append(ord(X))
             else:
+                # If no packet to send, send a null packet
                 dataOut.append(0xFF)
 
+            self._signal_health.update(ackStatus, outPacket)
+
 
 def set_retries_before_disconnect(nr_of_retries):
     global _nr_of_retries

cflib/crtp/signal_health.py

@@ -0,0 +1,122 @@
+import collections
+import time
+
+import numpy as np
+
+
+class SignalHealth:
+    """
+    Tracks the health of the signal by monitoring link quality and uplink RSSI
+    using exponential moving averages.
+    """
+
+    def __init__(self, signal_health_callback, alpha=0.1):
+        """
+        Initialize the SignalHealth class.
+
+        :param alpha: Weight for the exponential moving average (default 0.1)
+        """
+        self._alpha = alpha
+        self._signal_health_callback = signal_health_callback
+
+        self._retries = collections.deque()
+        self._retry_sum = 0
+
+    def update(self, ack, packet_out):
+        """
+        Update the signal health based on the acknowledgment data.
+
+        :param ack: Acknowledgment object containing retry and RSSI data.
+        """
+        self.signal_health = {}
+
+        self._update_link_quality(ack)
+        self._update_rssi(ack)
+        self._update_rate_and_congestion(ack, packet_out)
+
+        if self.signal_health and self._signal_health_callback:
+            self._signal_health_callback(self.signal_health)
+
+    def _update_link_quality(self, ack):
+        """
+        Updates the link quality based on the number of retries.
+
+        :param ack: Acknowledgment object with retry data.
+        """
+        if ack:
+            retry = 10 - ack.retry
+            self._retries.append(retry)
+            self._retry_sum += retry
+            if len(self._retries) > 100:
+                self._retry_sum -= self._retries.popleft()
+            self.signal_health['link_quality'] = float(self._retry_sum) / len(self._retries) * 10
+
+    def _update_rssi(self, ack):
+        """
+        Updates the uplink RSSI based on the acknowledgment signal.
+
+        :param ack: Acknowledgment object with RSSI data.
+        """
+        if not hasattr(self, '_rssi_timestamps'):
+            self._rssi_timestamps = collections.deque(maxlen=100)
+        if not hasattr(self, '_rssi_values'):
+            self._rssi_values = collections.deque(maxlen=100)
+
+        # update RSSI if the acknowledgment contains RSSI data
+        if ack.ack and len(ack.data) > 2 and ack.data[0] & 0xf3 == 0xf3 and ack.data[1] == 0x01:
+            instantaneous_rssi = ack.data[2]
+            self._rssi_values.append(instantaneous_rssi)
+            self._rssi_timestamps.append(time.time())
+
+            # Calculate time-weighted average RSSI
+            if len(self._rssi_timestamps) >= 2:  # At least 2 points are needed to calculate differences
+                time_diffs = np.diff(self._rssi_timestamps, prepend=time.time())
+                weights = np.exp(-time_diffs)
+                weighted_average = np.sum(weights * self._rssi_values) / np.sum(weights)
+                self.signal_health['uplink_rssi'] = weighted_average
+
+    def _update_rate_and_congestion(self, ack, packet_out):
+        """
+        Updates the packet rate and bandwidth congestion based on the acknowledgment data.
+
+        :param ack: Acknowledgment object with congestion data.
+        """
+        if not hasattr(self, '_previous_time_stamp'):
+            self._previous_time_stamp = time.time()
+        if not hasattr(self, '_amount_null_packets_up'):
+            self._amount_null_packets_up = 0
+        if not hasattr(self, '_amount_packets_up'):
+            self._amount_packets_up = 0
+        if not hasattr(self, '_amount_null_packets_down'):
+            self._amount_null_packets_down = 0
+        if not hasattr(self, '_amount_packets_down'):
+            self._amount_packets_down = 0
+
+        self._amount_packets_up += 1  # everytime this function is called, a packet is sent
+        if not packet_out:  # if the packet is empty, we send a null packet
+            self._amount_null_packets_up += 1
+
+        # Find null packets in the downlink and count them
+        mask = 0b11110011
+        if ack.data:
+            empty_ack_packet = int(ack.data[0]) & mask
+
+            if empty_ack_packet == 0xF3:
+                self._amount_null_packets_down += 1
+            self._amount_packets_down += 1
+
+            # rate and congestion stats every N seconds
+            if time.time() - self._previous_time_stamp > 0.1:
+                # self._uplink_rate = self._amount_packets_up / (time.time() - self._previous_time_stamp)
+                self.signal_health['uplink_rate'] = self._amount_packets_up / (time.time() - self._previous_time_stamp)
+                self.signal_health['downlink_rate'] = self._amount_packets_down / \
+                    (time.time() - self._previous_time_stamp)
+                self.signal_health['uplink_congestion'] = 1.0 - self._amount_null_packets_up / self._amount_packets_up
+                self.signal_health['downlink_congestion'] = 1.0 - \
+                    self._amount_null_packets_down / self._amount_packets_down
+
+                self._amount_packets_up = 0
+                self._amount_null_packets_up = 0
+                self._amount_packets_down = 0
+                self._amount_null_packets_down = 0
+                self._previous_time_stamp = time.time()