Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Bandwidth congestion and rate from radio driver #479

Closed
wants to merge 3 commits into from
Closed

Bandwidth congestion and rate from radio driver #479

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d63af7c
quick stats in printouts
knmcguire Sep 25, 2024
aa1fa57
add congestion statistics
knmcguire Sep 26, 2024
942fab9
add low level quality call back with renaming
knmcguire Sep 26, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 8 additions & 1 deletion cflib/crazyflie/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ 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.link_quality_low_level_updated = Caller()

self.state = State.DISCONNECTED

Expand Down Expand Up @@ -208,6 +209,12 @@ def _link_quality_cb(self, percentage):
"""Called from link driver to report link quality"""
self.link_quality_updated.call(percentage)

def _link_quality_low_level_cb(self, rate_up, rate_down, congestion_up, congestion_down):
"""Called from link driver to report link congestion"""
#logger.info('Rate congestion: up=%d pk/sec down=%d pk/sec', rate_up, rate_down)
#logger.info('Link congestion: up=%f perc down=%f perc', congestion_up, congestion_down)
self.link_quality_low_level_updated.call(int(rate_up), int(rate_down), congestion_up, congestion_down)

def _check_for_initial_packet_cb(self, data):
"""
Called when first packet arrives from Crazyflie.
Expand All @@ -229,7 +236,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._link_quality_cb, self._link_error_cb, self._link_quality_low_level_cb)

if not self.link:
message = 'No driver found or malformed URI: {}' \
Expand Down
7 changes: 5 additions & 2 deletions cflib/crtp/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,14 +89,17 @@ def get_interfaces_status():
return status


def get_link_driver(uri, link_quality_callback=None, link_error_callback=None):
def get_link_driver(uri, link_quality_callback=None, link_error_callback=None, link_quality_low_level_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)
if isinstance(instance,RadioDriver):
instance.connect(uri, link_quality_callback, link_error_callback, link_quality_low_level_callback)
else:
instance.connect(uri, link_quality_callback, link_error_callback)
return instance
except WrongUriType:
continue
Expand Down
47 changes: 45 additions & 2 deletions cflib/crtp/radiodriver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -242,12 +242,13 @@ def __init__(self):
self.uri = ''
self.link_error_callback = None
self.link_quality_callback = None
self.link_quality_low_level_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, link_quality_callback, link_error_callback, link_quality_low_level_callback):
"""
Connect the link driver to a specified URI of the format:
radio://<dongle nbr>/<radio channel>/[250K,1M,2M]
Expand Down Expand Up @@ -285,6 +286,7 @@ def connect(self, uri, link_quality_callback, link_error_callback):
self.out_queue,
link_quality_callback,
link_error_callback,
link_quality_low_level_callback,
self,
rate_limit)
self._thread.start()
Expand Down Expand Up @@ -383,6 +385,7 @@ def restart(self):
self.out_queue,
self.link_quality_callback,
self.link_error_callback,
self.link_quality_low_level_callback,
self)
self._thread.start()

Expand All @@ -402,6 +405,7 @@ def close(self):
# Clear callbacks
self.link_error_callback = None
self.link_quality_callback = None
self.link_quality_low_level_callback = None

def _scan_radio_channels(self, radio: _SharedRadioInstance,
start=0, stop=125):
Expand Down Expand Up @@ -520,7 +524,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]):
link_quality_callback, link_error_callback, link_quality_low_level_callback, link, rate_limit: Optional[int]):
""" Create the object """
threading.Thread.__init__(self)
self._radio = radio
Expand All @@ -529,6 +533,7 @@ def __init__(self, radio, inQueue, outQueue,
self._sp = False
self._link_error_callback = link_error_callback
self._link_quality_callback = link_quality_callback
self._link_quality_low_level_callback = link_quality_low_level_callback
self._retry_before_disconnect = _nr_of_retries
self._retries = collections.deque()
self._retry_sum = 0
Expand Down Expand Up @@ -585,6 +590,13 @@ def run(self):
break
self._link.needs_resending = not self._has_safelink

# Low level stats initialization
previous_time_stamp = time.time()
amount_null_packets_up = 0
amount_packets_up = 0
amount_null_packets_down = 0
amount_packets_down = 0

while (True):
if (self._sp):
break
Expand All @@ -594,6 +606,8 @@ def run(self):
ackStatus = self._send_packet_safe(self._radio, dataOut)
else:
ackStatus = self._radio.send_packet(dataOut)
print(dataOut)

except Exception as e:
import traceback

Expand Down Expand Up @@ -627,10 +641,18 @@ def run(self):
continue
self._retry_before_disconnect = _nr_of_retries

## Find null packets in the downlink and count them
data = ackStatus.data
mask = 0b11110011
empty_ack_packet = int(data[0]) & mask

if empty_ack_packet == 0xF3:
amount_null_packets_down += 1
amount_packets_down += 1

# If there is a copter in range, the packet is analysed and the
# next packet to send is prepared
# TODO: THis seems not 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)
Expand All @@ -645,6 +667,7 @@ def run(self):
else:
waitTime = 0


# If there is a rate limit setup, sleep here to force the rate limit
if self.rate_limit:
time.sleep(1.0/self.rate_limit)
Expand All @@ -667,7 +690,27 @@ def run(self):
else:
dataOut.append(ord(X))
else:
# If no packet to send, send a null packet
dataOut.append(0xFF)
amount_null_packets_up += 1
amount_packets_up += 1

# Low level stats every second
if time.time() - previous_time_stamp > 1:
rate_up = amount_packets_up / (time.time() - previous_time_stamp)
rate_down = amount_packets_down / (time.time() - previous_time_stamp)
congestion_up = 1.0 - amount_null_packets_up / amount_packets_up
congestion_down = 1.0 - amount_null_packets_down / amount_packets_down

amount_packets_up = 0
amount_null_packets_up = 0
amount_packets_down = 0
amount_null_packets_down = 0
previous_time_stamp = time.time()

self._link_quality_low_level_callback(rate_up, rate_down, congestion_up, congestion_down)




def set_retries_before_disconnect(nr_of_retries):
Expand Down
Loading