Swarm Control & Broadcasting

[mikendu]

This discussion is carried over from this forum post

Most of the context for my project is in the forum post above, but I'll summarize here as well:
I've forked this repository (as well as crazyflie-lib-python) with the goal of adding support for broadcasting to multiple drones, similar to the way the crazyswarm project works, and using that to build a swarm controller application. I'm currently running into issues where the drones don't seem to be responding to the broadcasted commands I'm attempting to send.

As mentioned in the forum post, I'm testing with the following setup:

• Two drones, one at uri radio://0/55/2M/E7E7E7E7E7 and one at radio://0/55/2M/E7E7E7E7E8, both flashed with the latest firmware (including the latest nrf firmware)
• A Crazyradio PA dongle, flashed with firmware I built from the latest commit on crazyradio-firmware

I'm trying to broadcast to the drones by opening a link at "broadcast address" radio://0/55/2M/FFE7E7E7E7, and sending out high level commander packets for takeoff/land commands, but the drones do not respond to these commands. As I was learning in the forum post, there's a command to enable "noAck" mode for the radio, which I'm likely missing, and opening a link to the broadcast address manually might not be the right way to go about it, so I'm looking for some pointers on what I should be doing to implement this functionality.

[whoenig]

This would be a great add-on and is also the missing feature before the Crazyswarm can switch to this library.

• There should be no need to use send2PacketsNoAck. This is just a performance improvement that has to be used carefully (the two packets need to have exactly the same length). It is better to just use the regular sendPacketNoAck for now.
• In this library, it should be indeed sufficient to open a link using radio://0/55/2M/FFE7E7E7E7?safelink=0&autoping=0. In the future, it might be nicer to add a special broadcast mode, which could also use the send2PacketsNoAck on demand.

[mikendu]

Thanks for the guidance! I actually had some time to play around with the code over the weekend and I figured out what I was doing wrong—I was repurposing the existing Connection class, which uses safelink by default, and so even though I had commented out most of the acknowledgement & auto-pinging code, all the packets I was attempting to broadcast had the safelink bits enabled (side note: I actually didn't realize the URI supported query parameters like that, but I see the regex match happening in the constructor now, good to know!).

As soon as I set safelink to 0, I was able to successfully broadcast to the two crazyflies I have to do a synchronized takeoff & land. Since it sounds like this would be something good to contribute, I'm going to clean up this code a bit, and probably pull this feature out into a separate class (similar to the CrazyflieBroadcaster class in crazyflie_cpp), and I can open up a pull request to review, and possibly merge in at some point.

[whoenig]

Awesome - thanks for the update! One word of warning though: For crazyflie-link-cpp, the goal is to always go through the Connection class to have a unified interface on the Python side as well as into Crazyflie-lib-python (where this library can be used as backend). As such, we don't want a ConnectionBroadcast, but instead use the URI accordingly. I can think of three options:

1. Add a new broadcast flag which is by default off, i.e., one could create a broadcast interface using radio://0/55/2M/FFE7E7E7E7?broadcast=1. In the first version, this could just set safelink=0 and autoping=0, but in later versions we can add support for send2PacketsNoAck for performance improvements.
2. Assume that broadcast is always done on address FFE7E7E7E7, i.e., radio://0/55/2M/FFE7E7E7E7 would create a broadcast connection. I believe this is true for current NRF firmware versions. This is shorter than 1), but less explicit so perhaps harder to understand for newcomers.
3. Mix 1) and 2): radio://0/55/2M?broadcast=1. This is explicit as 1), compact as 2), and also hides unnecessary information (since the address part cannot really be changed without a firmware hack).

I prefer option 3. It would be good to get some input from @ataffanel, who co-designed the interface of this library.

[mikendu]

That's good to know, because I was planning on doing exactly that actually, adding a BroadcastConnection class, exposing it through the Python bindings, and then making subsequent changes to crazyflie-lib-python accordingly. Also, I assume we also want to re-use Connection so that the broadcast connection's packet can go out on the same queue/thread as the normal unicast packets.

Btw, can you explain the concern around send2PacketsNoAck? You mentioned that the two packets must have the same length, but if we have a send function that takes a single packet, I don't see how the two packets could differ.

[whoenig]

The send2PacketsNoAck function (see https://github.com/whoenig/crazyflie_cpp/blob/master/src/Crazyradio.cpp#L233-L235) does not take 1 packet, but a raw data pointer. It assumes that this array consists of two packets with the same length, see https://github.com/whoenig/crazyflie_cpp/blob/master/src/Crazyflie.cpp#L1670-L1674 for an example. I think on the radio side this also only works if the combined length is greater than 32, i.e., one packet's size is strictly greater than 16. The whole purpose of this approach was to reduce the number of USB requests and this is only used to transfer motion capture information. Since you are using LH, this mode is of limited interest to you, since most (consecutive) packets won't have the exact same length.

[mikendu]

Interesting, I didn't realize that's how it was intended to be used—it does take a raw data pointer and a buffer length like you said, but so do sendPacket and sendPacketNoAck, so I figured I'd be able to pass it the buffer & size from the Packet same as the other calls (this actually seemed to work fine when I was testing).

Either way though, I sounds like it would be at best a minor optimization for me, and testing with sendPacketNoAck worked just as fine. I've gone ahead and opened a pull request for the modifications I made: #9

I ended up using radio://0/55/2M/broadcast for the URI format, since the other flags get overridden when broadcasting anyway, and we can short-circuit some of the address & flag logic this way.

[whoenig]

Thanks - I left a review. As stated earlier, we'll need @ataffanel input for the URI design. I think radio://0/55/2M/broadcast is perhaps not within the original spec of the URI design. Another option that I saw in the code was to use radio://0/55/2M/* for broadcasts (this was what I originally planned, but this was never implemented nor discussed).

[ataffanel]

Hi, Thanks for the great work, it will be awesome to finally get broadcast in the python cflib and to unify Crazyswarm and the cflib link!

Between the two URI, radio://0/55/2M/* is the one that looks the least 'funy' to me. Though, thinking about it, I keep having a problem with hacking URIs that way: the difference between broadcast and unicast is not only about the address, it is also another protocol that does not guarantee delivery and is unidirectional. In that sense, having another connection object might have made sense, though I do see the appeal of keeping things simple with a single object that does everything.

I can think of two uri scheme that would work for me:

1. radiobc://0/55/2M: Creating a new 'protocol' that is a broadcast radio (the name is just a proposal), this lets us change the broadcast address in the future if needed
2. radio://0/55/2M/*: We just add it to the current URI

[whoenig]

I think we had more than two options in total:

1. radio://0/55/2M/FFE7E7E7E7?broadcast=1
2. radio://0/55/2M/FFE7E7E7E7
3. radio://0/55/2M?broadcast=1
4. radio://0/55/2M/*
5. radiobc://0/55/2M

I actually also like using a new protocol for the reasons Arnaud stated. Perhaps radiobroadcast://0/55/2M or broadcast://0/55/2M, since bc itself is not very descriptive?

[ataffanel]

The "bc" was an unnecessary shortening. I like both radiobroadcast://0/55/2M and broadcast://0/55/2M. I might lean more towards radiobroadcast://0/55/2M since I have started working on new low level radio protocol that will also need to have both a unicast and a broadcast mode, so this was we can keep space for another broadcast mode.

So my vote would go for radiobroadcast://0/55/2M.

[mikendu]

Just an note that I thought I should leave here for future reference, in case anyone's looking at this thread later: I've been doing some testing with the broadcast feature, and while it works great, it seems it's also important to maintain a unicast connection, with autoping enabled, for each of the copters in the swarm, in order to allow them to keep sending packets back and flush their send queues.

I had to do a bit of debugging with the firmware to figure this out, but it seems that because the firmware is designed to not drop packets, and most operations send back an ack packet after processing the incoming packet, if no sends are happening, the copters can can get "backed up", and they will eventually stop processing incoming packets.

[whoenig]

@mikendu Do you have an example that shows the issue with the congested queues? I am afraid that this might be a bug (and even a regression) and we should file github issue in the official firmware to investigate.

[mikendu]

Yep, here's the example code I was testing with (slightly hacky):

import struct
import time
from enum import Enum

from cflib.crtp.cflinkcppdriver import CfLinkCppDriver
from cflib.crtp.radiodriver import Crazyradio
from cflib.crtp.crtpstack import CRTPPacket
from cflib.crtp.crtpstack import CRTPPort
from cflib.crazyflie import HighLevelCommander
from cflib.crazyflie.param import WRITE_CHANNEL

class RingEffect(Enum):
    OFF = 0
    FADE_EFFECT = 14
    TIMING_EFFECT = 17

# Hard coding TOC param ids for now, based on the current firmware
class ParameterID(Enum):
    RING_EFFECT = 181       # uint8_t, <B
    FADE_COLOR = 190        # uint32_t, <L
    FADE_TIME = 191         # float, <f

class LightController:
    def __init__(self, cf):
        self.cf = cf

    def set_effect(self, effect):
        if not isinstance(effect, RingEffect):
            raise ValueError("Invalid effect given: " + str(effect))

        packet = self._effect_change(effect.value)
        self.cf.send_packet(packet)

    def set_color(self, r, g, b, time = 0.0):
        color = (int(r) << 16) | (int(g) << 8) | int(b)
        color_packet = self._fade_color(color)
        time_packet = self._fade_time(time)
        self.cf.send_packet(color_packet)
        self.cf.send_packet(time_packet)

    def _fade_time(self, duration):
        packet = CRTPPacket()
        packet.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
        packet.data = struct.pack('<H', int(ParameterID.FADE_TIME.value))
        packet.data += struct.pack('<f', float(duration))
        return packet

    def _fade_color(self, color):
        packet = CRTPPacket()
        packet.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
        packet.data = struct.pack('<H', int(ParameterID.FADE_COLOR.value))
        packet.data += struct.pack('<L', int(color))
        return packet

    def _effect_change(self, effect):
        packet = CRTPPacket()
        packet.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
        packet.data = struct.pack('<H', int(ParameterID.RING_EFFECT.value))
        packet.data += struct.pack('<B', int(effect))
        return packet

class Broadcaster():

    def __init__(self, channel, datarate = Crazyradio.DR_2MPS):
        self._validate_channel(channel)
        self._validate_datarate(datarate)

        self._uri = self._construct_uri(channel, datarate)
        self._link = CfLinkCppDriver()
        self._is_link_open = False

        self.high_level_commander = HighLevelCommander(self)
        self.light_controller = LightController(self)

    def open_link(self):
        if (self.is_link_open()):
            raise Exception('Link already open')

        print('Connecting to %s' % self._uri)
        self._link.connect(self._uri, None, None)
        self._is_link_open = True

    def close_link(self):
        if (self.is_link_open()):
            self._link.close()
            self._is_link_open = False

    def __enter__(self):
        self.open_link()
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.close_link()

    def is_link_open(self):
        return self._is_link_open

    def send_packet(self, packet):
        if not self.is_link_open():
            raise Exception('Link is not open')

        self._link.send_packet(packet)

    def __str__(self):
        return "BroadcastLink <" + str(self._uri)  + ">"

    def _construct_uri(self, channel, datarate):
        return "radiobroadcast://*/" + str(channel) + "/" + self._get_data_rate_string(datarate)

    def _validate_channel(self, channel):
        if channel and (isinstance(channel, int) or channel.is_integer()):
            if channel >= 0 and channel <= 127:
                return
        raise ValueError("Invalid channel: " + str(channel))

    def _validate_datarate(self, datarate):
        if not(datarate == Crazyradio.DR_250KPS or \
            datarate == Crazyradio.DR_1MPS or \
            datarate == Crazyradio.DR_2MPS):
            raise ValueError("Invalid data rate: " + str(datarate))

    def _get_data_rate_string(self, datarate):
        if datarate == Crazyradio.DR_250KPS:
            return '250K'
        elif datarate == Crazyradio.DR_1MPS:
            return '1M'
        elif datarate == Crazyradio.DR_2MPS:
            return '2M'


link = Broadcaster(55)
link.open_link()

link.light_controller.set_color(255, 0, 0, 0.5)
time.sleep(0.5)
link.light_controller.set_color(0, 0, 0, 0.5)
time.sleep(0.5)

link.light_controller.set_color(0, 255, 0, 0.5)
time.sleep(0.5)
link.light_controller.set_color(0, 0, 0, 0.5)
time.sleep(0.5)

link.light_controller.set_color(0, 0, 255, 0.5)
time.sleep(0.5)
link.light_controller.set_color(0, 0, 0, 0.5)
time.sleep(1.0)

print("Taking off...")
link.high_level_commander.takeoff(0.5, 3.0)
time.sleep(3.2)

print("Landing...")
link.high_level_commander.land(0.05, 3.0)
time.sleep(3.2)

link.high_level_commander.stop()
time.sleep(1.0)

link.close_link()

Basically I was doing some synchronized takeoffs, landings, and LED ring color changes, and I was broadcasting HighLevelCommander packets, and parameter writes for ring.fadeTime and ring.fadeColor. I noticed that if I ran the example a couple of times, without restarting the copters, then they would eventually stop responding to the broadcast packets, and in fact, when I would then try to open a unicast connection to any of the "stuck" crazyflies, they'd reset themselves.

I checked the console output when that happened, and found that the reset was caused by this particular ASSERT in the firmware failing. I thought this was odd, as I was only sending broadcast packets, and didn't see any reason why the queue should be backed up. I realized that the crtpPacketDelivery queue was shared for all incoming packets, meaning the incoming broadcast packets weren't being cleared from the receive queue, so when I then tried to open a unicast connection, the queue was still full, and the assert failed, causing the crazyflie to reset itself.

So then that raised the question of why those broadcast packets weren't being processed, and after a fair bit of code reading and debugging, I found what I think is the full story:

• This ASSERT I mentioned before fails, because crtpPacketDelivery is full
• crtpPacketDelivery is full because radiolinkReceiveCRTPPacket isn't being called
• radiolinkReceiveCRTPPacket isn't being called because the crtpRxTask is blocked on sending the packet to the queue for the port (in this case HighLevelCommander/Param ports), because the port's queue is full
• The port's queue is full because its task is stuck trying to send back an ack packet after processing an incoming packet, so it's not grabbing new packets off of the incoming queue (ex: crtpCommanderHighLevelTask. A similar issue exists with the param port/task)
• The port task is blocked on trying to send out an ack packet because the crtp txQueue is full
• The crtp txQueue is full because the radiolink txQueue is full
• The radiolink txQueue is full because items only get popped off of it when a unicast packet comes in, and we send back an outgoing packet

I was able to get my example working consistently by just opening a unicast link to each copter in the swarm, and that seemed to mitigate the issue for now. I was thinking about making some modifications to the firmware, either:

• Sending out a packet when we receive a broadcast packet, same as what happens for unicast packets. This seems concerning though, since we don't know whether that packet was related to a broadcast transmission or not, so seems like that might open up potential dropped packets.
• Adding a flag, or creating a wrapper for CRTPPacket that contains data on whether the packet is related to a broadcast transmission. This way, we could ignore acks for broadcast packets. Would be complicated though, as that data needs to be propagated through the whole roundtrip of the packet.

Decided against doing either of these, since neither idea seemed ideal, and I had a workaround I could work with, but I hope this helps.