diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py
index cb9a0eed4..d8c4fa919 100644
--- a/cflib/crazyflie/__init__.py
+++ b/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:
diff --git a/cflib/crazyflie/link_statistics.py b/cflib/crazyflie/link_statistics.py
new file mode 100644
index 000000000..af23f3b56
--- /dev/null
+++ b/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 .
+"""
+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(' 100:
+ self._latencies.pop(0)
+ p95_latency = np.percentile(self._latencies, 95)
+ return p95_latency
diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py
index 756e9a66c..3d410554b 100644
--- a/cflib/crtp/__init__.py
+++ b/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
diff --git a/cflib/crtp/cflinkcppdriver.py b/cflib/crtp/cflinkcppdriver.py
index ab69f0773..9d760a7a7 100644
--- a/cflib/crtp/cflinkcppdriver.py
+++ b/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')
diff --git a/cflib/crtp/crtpdriver.py b/cflib/crtp/crtpdriver.py
index 2f620fd30..598b29e3d 100644
--- a/cflib/crtp/crtpdriver.py
+++ b/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, 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)
"""
diff --git a/cflib/crtp/radio_link_statistics.py b/cflib/crtp/radio_link_statistics.py
new file mode 100644
index 000000000..d2a68e0f6
--- /dev/null
+++ b/cflib/crtp/radio_link_statistics.py
@@ -0,0 +1,124 @@
+import collections
+import time
+
+import numpy as np
+
+
+class RadioLinkStatistics:
+ """
+ Tracks the health of the signal by monitoring link quality, uplink RSSI,
+ packet rates, and congestion.
+ """
+
+ def __init__(self, radio_link_statistics_callback, alpha=0.1):
+ """
+ Initialize the RadioLinkStatistics class.
+
+ :param alpha: Weight for the exponential moving average (default 0.1)
+ """
+ self._alpha = alpha
+ self._radio_link_statistics_callback = radio_link_statistics_callback
+
+ self._retries = collections.deque()
+ self._retry_sum = 0
+
+ def update(self, ack, packet_out):
+ """
+ Update the radio link statistics based on the acknowledgment data.
+
+ :param ack: Acknowledgment object containing retry and RSSI data.
+ """
+ self.radio_link_statistics = {}
+
+ self._update_link_quality(ack)
+ self._update_rssi(ack)
+ self._update_rate_and_congestion(ack, packet_out)
+
+ if self.radio_link_statistics and self._radio_link_statistics_callback:
+ self._radio_link_statistics_callback(self.radio_link_statistics)
+
+ 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.radio_link_statistics['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.radio_link_statistics['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.radio_link_statistics['uplink_rate'] = self._amount_packets_up / \
+ (time.time() - self._previous_time_stamp)
+ self.radio_link_statistics['downlink_rate'] = self._amount_packets_down / \
+ (time.time() - self._previous_time_stamp)
+ self.radio_link_statistics['uplink_congestion'] = 1.0 - \
+ self._amount_null_packets_up / self._amount_packets_up
+ self.radio_link_statistics['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()
diff --git a/cflib/crtp/radiodriver.py b/cflib/crtp/radiodriver.py
index 7d22b6c52..a4d82ee19 100644
--- a/cflib/crtp/radiodriver.py
+++ b/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.radio_link_statistics import RadioLinkStatistics
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.radio_link_statistics_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, radio_link_statistics_callback, link_error_callback):
"""
Connect the link driver to a specified URI of the format:
radio:////[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 radio link statistics can be called at any moment from the
+ driver to report back the radio link statistics. 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,
+ radio_link_statistics_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.radio_link_statistics_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.radio_link_statistics_callback = None
def _scan_radio_channels(self, radio: _SharedRadioInstance,
start=0, stop=125):
@@ -520,7 +520,7 @@ class _RadioDriverThread(threading.Thread):
Crazyradio USB driver. """
def __init__(self, radio, inQueue, outQueue,
- link_quality_callback, link_error_callback, link, rate_limit: Optional[int]):
+ radio_link_statistics_callback, link_error_callback, link, rate_limit: Optional[int]):
""" Create the object """
threading.Thread.__init__(self)
self._radio = radio
@@ -528,10 +528,8 @@ def __init__(self, radio, inQueue, outQueue,
self._out_queue = outQueue
self._sp = False
self._link_error_callback = link_error_callback
- self._link_quality_callback = link_quality_callback
+ self._radio_link_statistics = RadioLinkStatistics(radio_link_statistics_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._radio_link_statistics.update(ackStatus, outPacket)
+
def set_retries_before_disconnect(nr_of_retries):
global _nr_of_retries
diff --git a/cflib/crtp/usbdriver.py b/cflib/crtp/usbdriver.py
index 859244438..349e58d5c 100644
--- a/cflib/crtp/usbdriver.py
+++ b/cflib/crtp/usbdriver.py
@@ -52,21 +52,19 @@ def __init__(self):
self.cfusb = None
self.uri = ''
self.link_error_callback = None
- self.link_quality_callback = None
+ self.radio_link_statistics_callback = None
self.in_queue = None
self.out_queue = None
self._thread = None
self.needs_resending = False
- def connect(self, uri, link_quality_callback, link_error_callback):
+ def connect(self, uri, radio_link_statistics_callback, link_error_callback):
"""
Connect the link driver to a specified URI of the format:
radio:////[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
- an error message.
+ The callback for radio link statistics should not be called from the usb driver
+ The callback from linkError will be called when a error occurs with an error message.
"""
# check if the URI is a radio URI
@@ -100,7 +98,7 @@ def connect(self, uri, link_quality_callback, link_error_callback):
# Launch the comm thread
self._thread = _UsbReceiveThread(self.cfusb, self.in_queue,
- link_quality_callback,
+ radio_link_statistics_callback,
link_error_callback)
self._thread.start()
@@ -152,7 +150,7 @@ def restart(self):
return
self._thread = _UsbReceiveThread(self.cfusb, self.in_queue,
- self.link_quality_callback,
+ self.radio_link_statistics_callback,
self.link_error_callback)
self._thread.start()
@@ -208,7 +206,7 @@ class _UsbReceiveThread(threading.Thread):
Radio link receiver thread used to read data from the
Crazyradio USB driver. """
- def __init__(self, cfusb, inQueue, link_quality_callback,
+ def __init__(self, cfusb, inQueue, radio_link_statistics_callback,
link_error_callback):
""" Create the object """
threading.Thread.__init__(self)
@@ -216,7 +214,7 @@ def __init__(self, cfusb, inQueue, link_quality_callback,
self.in_queue = inQueue
self.sp = False
self.link_error_callback = link_error_callback
- self.link_quality_callback = link_quality_callback
+ self.radio_link_statistics_callback = radio_link_statistics_callback
def stop(self):
""" Stop the thread """
diff --git a/examples/link_quality/latency.py b/examples/link_quality/latency.py
new file mode 100644
index 000000000..21faf8aa0
--- /dev/null
+++ b/examples/link_quality/latency.py
@@ -0,0 +1,55 @@
+# -*- 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 .
+import time
+
+import cflib.crtp # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.utils import uri_helper
+
+# Reads the CFLIB_URI environment variable for URI or uses default
+uri = uri_helper.uri_from_env(default='radio://0/90/2M/E7E7E7E7E7')
+
+
+def latency_callback(latency: float):
+ """A callback to run when we get an updated latency estimate"""
+ print(f'Latency: {latency:.3f} ms')
+
+
+if __name__ == '__main__':
+ # Initialize the low-level drivers
+ cflib.crtp.init_drivers()
+
+ # Create Crazyflie object, with cache to avoid re-reading param and log TOC
+ cf = Crazyflie(rw_cache='./cache')
+
+ # Add a callback to whenever we receive an updated latency estimate
+ #
+ # This could also be a Python lambda, something like:
+ cf.link_statistics.latency.latency_updated.add_callback(latency_callback)
+
+ # This will connect the Crazyflie with the URI specified above.
+ with SyncCrazyflie(uri, cf=cf) as scf:
+ print('[host] Connected, use ctrl-c to quit.')
+
+ while True:
+ time.sleep(1)
diff --git a/sys_test/swarm_test_rig/test_response_time.py b/sys_test/swarm_test_rig/test_response_time.py
index da9222fce..40158239c 100644
--- a/sys_test/swarm_test_rig/test_response_time.py
+++ b/sys_test/swarm_test_rig/test_response_time.py
@@ -128,14 +128,14 @@ def _is_response_correct_seq_nr(self, response, seq_nr):
return False
def connect_link(self, uri):
- link = cflib.crtp.get_link_driver(uri, self._link_quality_cb,
+ link = cflib.crtp.get_link_driver(uri, self._radio_link_statistics_cb,
self._link_error_cb)
self.assertIsNotNone(link)
self.links.append(link)
return link
- def _link_quality_cb(self, percentage):
+ def _radio_link_statistics_cb(self, radoi_link_statistics):
pass
def _link_error_cb(self, errmsg):