Add functionality to allow the STM32 to send/receive P2P radio packets

[ataffanel]

One common request for the Crazyflie lately has been to be able to have Crazyflies communicating directly with each-other (P2P). This functionality has not been implemeted yet for two main reason: the Crazyflie communication protocol is optimized to have as low latency as possible with one host and to do so it is organized as a star network around the host, also there is no generic use-case for P2P communication, each use case requires very different communication scheme.

So to get started we could implement a simple low level way for the STM32 to send and receive P2P radio packets that could run in independently from the existing CRTP link.

The proposed solution is to create functionalities and Syslink packets for:

• Sending P2P packet to a specific address or broadcast
• Receiving P2P packet, either unicast or broadcast
• There is 16 P2P 'port'. Eatch packet, unitcast or broadcast, is sent on one port.
• Crazyflie <-> Crazyflie payload size up to 63 Bytes, Crazyradio PA <-> Crazyflie payload size up to 30 Bytes.

In this mode the ack packet payload would always be empty (unlike CRTP that uses the ack packet payload as downlink). The TX/RX channel and device address is common with CRTP.

On the radio, this is implemented by prepending the packet with [0x3f, 0x8x] with x being the P2P port number. In P2P this decodes as a NULL packet, the nRF firmware will recognize the pattern and send the packet to the STM32 as a P2P packet. By keeping the current radio packet unchanged, the payload size can be increased to 63 bytes. This meas that Crazyflie to Crazyflie P2P communication packets can have a payload of up to 61 bytes (63 - 2). Crazyradio to Crazyflie will have a payload up to 30 bytes.

A ticket needs to be created in the bitcraze/crazyflie-firmware repos to implement the STM32 API side.

I think this would allow to implement commonly requested schemes like exchanging sensor values between Crazyflie or event some relaying of information or simple mesh network. Though the implementation of a specific use-case is out of scope, it will require custom code on the STM32 side.

[ataffanel]

Edited description to remove the use of specific radio addresses for P2P communication and add a CRTP header instead to send and receive P2P packets using the existing CRTP unicast and broadcast address.

This has the drawback of reducing the useful payload by 2 bytes. However it makes implementation and documentation much simpler: if we where to use new addresses for P2P there would be rules about what address can be used and there would anyway be a link between P2P and CRTP addresses, this is due to the way the address matching is implemented in the nRF51 radio. Having the same radio address for CRTP and P2P simplifies things: one Crazyflie, one address.

To stay compatible with Crazyradio PA (nRF24LU1), the radio packet has a size field of 6 bits, which allows to encode a packet size of up to 63 bytes in the air when communicating Crazyflie to Crazyflie (Crazyradio PA is still limited to 32 Bytes packets). The nRF51 is capable of handling payload of up to 254 Bytes, though this would make the Crazyflie incapable of communicating with Crazyradio PA.

[MadeInPierre]

Hi,
I am currently interested in using a swarm of Crazyflies using P2P communication (with a custom movement algorithm running onboard), and would gladly try to help on this feature until it gets functional.

Has this been continued and just needs to be included in the official release? Or is it functional already? Otherwise, I will try to finish developing this feature and will let you know about it, I'd gladly accept any useful information if you have some.
Thanks!

[ataffanel]

Every work done on this ticket is pushed in the repos already, I have not had time to continue further. Any help on that would be great :-).

Currently there is code to receive P2P packets and send them to the STM32. What is missing is the other way around: implementing sending P2P packet from the STM32. The syslink packet ID have already been prepared (SYSLINK_RADIO_P2P, SYSLINK_RADIO_P2P_ACK, SYSLINK_RADIO_P2P_BROADCAST).

The sequence I am imagining for sending a P2P packet is:

• The STM32 sends SYSLINK_RADIO_P2P to the nRF51
• The nRF51 sends the packet in the air and wait for the ack
• The nRF51 sends SYSLINK_RADIO_P2P_ACK to the STM32 with the ack status

One question here is how to set the transmit address, one new syslink packet "SYSLINK_RADIO_P2P_ADDRESS" could be created, or the address could always be sent with the packet in SYSLINK_RADIO_P2P. Both would work for me, I guess it is mostly about optimisation (the former is good when sending multiple packets to the same peer, the later is better to send each packet to a different peer).

And for broadcast:

• The STM32 sends SYSLINK_RADIO_P2P_BROADCAST to the nRF51
• The nRF51 sends the packet in the air using the broadcast address

[MadeInPierre]

Hi,
Thank you for the information, I am currently implementing the message transferring in the STM32 from the syslink level to the 'user' services (I will work on the sending part when this is done). I have a few questions on how to implement this:

• Should the Raw and P2P packets have common ports (this means that a drone could directly connect to all the logging services etc, as well as setting setpoints in other drones), or should we keep the p2p communication separate in the system? If they are separate, I'll just create new queues for the p2p ports and new modules will subscribe to them in the future; if they're common, i'll recognize the port in the P2P message and send it to the existing queues.
• Inside the STM firmware, should we filter the P2P messages according to the port number, the sender's address or both?

I am still discovering the general project architecture so I don't really have a clear vision on how things should evolve yet.
Thanks!

Edit : This part is done, I chose to send P2P packets in new port queues and new services can subscribe to them.

[MadeInPierre]

Hi,
I have managed to receive P2P packets inside the firmware, it works perfectly as far as I tested (currently I put the P2P messages in their own port queues). I am now trying to send P2P packets from an STM32 until another drone's STM32, but my knowledge isn't very good about radio and transmission settings.

For now I am 'cheating' because I am sending fake P2P packets from the crazyradio (the STM32 receives them perfectly). I also completed the syslink sending part, so now I can ask to send a P2P packet from the STM and the NRF receives the correct Syslink packet (for now the destination address is set inside each P2P packet).

The part I am stuck with is sending the packet through the air so that it reaches the correct Crazyflie.

• Could you explain how I should set the NRFRADIO->PREFIXn, NRFRADIO->BASEn and NRFRADIO->TXADDRESS fields so that I can send a message to the P2P or Crazyradio address and then listen to the drone's address just after that?
• Do we have to completely reset the radio by caling the esbReset(); method each time we change the transmit address, or can we change the address dynamically ? If a reset is needed, this would probably decrease the radio bandwith, do you think the speed will still be acceptable?
• Once the basic transmission is done, we have to coordinate the radio states so that we can both use the radio in a start-network mode (for CRTP) as well as sending and receiving P2P packets whenever we want. Do you have an idea on how we should go on with this?

I think I would understand things better with this and anything else you could find useful to point out :-).
Thanks!

[HPeterPaulsen]

Hi MadeInPierre

In this mesh implementation for the chip https://github.com/NordicPlayground/nRF51-ble-bcast-mesh/blob/9647fd7357a6a66741f30ea97913874d4cb30bcc/nRF51/rbc_mesh/src/radio_control.c
They seem to be changing the prefix and base without reset (see the setup_event function), though I haven't checked it properly. And https://infocenter.nordicsemi.com/pdf/nRF51_RM_v3.0.pdf names START as decision point for the prefixes. But from the reset code I believe part of the reason for doing the reset is to clear interrupts?

About setting the address: For the PREFIX0 currently address 0 is used for the crazyflies own address, and 1 for the broadcast one, so you could use the logical address 2 it belongs to. For that the way they do it with the cfs own address shifted by 2 byte should work
NRF_RADIO->PREFIX0 = 0xC4C3FF00UL | ((bytewise_bitswap(otherCFAddress >> 32) & 0xFF)<<16); (or shift it less to begin with and use 0xFF0000 to mask)

You can change the baseaddress like this NRF_RADIO->BASE1 = bytewise_bitswap((uint32_t)otherCFAddress);
And this NRF_RADIO->RXADDRESSES = (1<<0) | (1<<1)|(1<<2); would make it listen to the address too. Though if you change BASE1 you also change the broadcast address so I guess there is no point in listening on 1 while it is active. (But there are only two base addresses so no way to avoid that I think. ) Then add something to the interrupt handler to take care of that address match.

I am using crazyflie with p2p but my code only broadcasts with no acks, and they basically constantly communicate once they start (and timeslots to avoid collisions are handled in the stm firmware), it works for my purpose because I do something with a group of CF that are supposed to constantly send data to all others. Also my code for the nrf doesn't work with BLE=1 (because I don't need it). So my code wouldn't be very useful for this, still in case something useful to you is in my code here is the : https://collaborating.tuhh.de/ICS/crazyflies/blob/p2p/src/modules/src/peertopeer.c

And since I don't have the nrf code online the releant src files as zip: src.zip

Good luck would be nice to have native support for p2p.

[MadeInPierre]

Hi @HPeterPaulsen,
Thanks a lot for the help! I actually managed to broadcast P2P messages for now too, and I'm happy to see that we have an almost identical implementation.

The problem I am now interested in is the synchronisation between CRTP and P2P (for all the drones we want to communicate with), because at the moment the crazyflie randomly shuts down if CRTP and P2P packets are being sent at the same time. I would like to see if I can find ideas from your peertopeer.c file in your crazyflie firmware but I am unfortunately getting a 404 error, could you fix the link (maybe your repo is private) ?

When the basics are done I will see if I have the time to do proper addressing for a clean P2P communication method :-). BTW, I am thinking about easily listening to broadcast and unicast messages and I think using the PREFIX bytes to identify Crazyflies can be really effective. This would mean we would have to change the addressing convention from 0xE7E7E7E7XX to 0xXXE7E7E7E7, maybe @ataffanel knows if this is a good idea and is feasible?
Thanks again!

[HPeterPaulsen]

@MadeInPierre

Ah sorry it is indeed set to private, hmm I will come back to you later on that. Not sure it would help anyway I haven't properly checked how it works when using normal communication at the same time. On the PC side to start the mode I send a broadcast (and later to stop it) and afterward switch the crazyradio to PRX mode and log all messages between the cf so my antenna is occupied during the time. Hmm will give it a try later today to see what happens. I think it shouldn't lead to random shutdowns, but there will be problems with message collisions since the timing won't match.

Thanks a lot for the help! I actually managed to broadcast P2P messages for now too, and I'm happy to see that we have an almost identical implementation.

Ah good, btw is yours working with BLE=1? Mine shuts down randomly with BLE probably because I give no matching commands to the softdevice. I don't need BLE so I never fixed it, but would be interested in an working implementation.

[MadeInPierre]

Hi @HPeterPaulsen, I think your code could work with BLE=1 since it is so similar to mine, I tried compiling it and you seem to have a stack overflow error, which i 'solved' by reducing the TX queue size (It seems that the NRF chip is unfortunately close to its stack limit). I have 8 paquets each for the P2P and CRTP queues.

About the time synchronisation between drones, I actually have something similar to do in my own use case, so I would really appreciate if there's a nice example around :-). For now I managed to keep both communication links by sending P2P paquets only when the CRTP link isn't active for some time (5ms seems to work fine), I don't know if there's a better solution to that. Maybe @ataffanel already had a solution in mind?

[HPeterPaulsen]

@MadeInPierre Sorry for the late answer. Hmm I think this issue was supposed to just provide the option to send p2p messages without taking care of synchronization issues etc. Which makes this a bit offtopic for the issue I suppose. I would send you a private message but github doesn't seem to have that. Well I suppose another comment won't be a problem.

I talked with other people at my uni and the git will stay private a while longer so I don't want to post the code publicly yet but I can tell you what I am doing (depending on your use case that might or might not be useful): Currently my group of cf are constantly sending messages and I organize it with time slots. This requires:
a) knowing who has what time slot (currently I initialize the mode with a command from the pc in which I also set which cf are active and they send in the order they are mentioned without a start command alternatives are necessary of course)
and b)time synchronization, considering the different clock speeds. They send every 1000/Hz ms with different offsets but that requires frequent adjustments if you want a decent speed. (2ms between messages (so for instance 10 cf at 50Hz each for instance) seems to be the tightest timing I can handle with acceptable loss.) In indoor test with limited room you can mostly assume that all hear all which makes synchronization much easier. My messages contain a 3 byte timestamp (because I needed to free one byte) when they arrive in the listening thread I note the time of arrival
messagerectime = xTaskGetTickCount()-startTime-2; (-2 because it seems to take 1-2ms from being sent to actually popping up in the receive thread of the other) and compare them to heir trasmitted timestamp
data1[cf_nr].tickdiff=ticks-messagerectime;
and in the send thread

xFrequency=1000/p2pHZ;
		delay=(startTime-xTaskGetTickCount())+(adjusted_send_order[crazyflie_nr]*xFrequency/(number_of_crazyflie));
		vTaskDelay( delay );
		xLastWakeTime = xTaskGetTickCount();
	startTime=xLastWakeTime-(adjusted_send_order[crazyflie_nr]*xFrequency/(number_of_crazyflie));
		delayModifier =0;
		while(!stop)
		{
			vTaskDelayUntil( &xLastWakeTime, xFrequency+delayModifier );
			xSemaphoreTake(p2pDataLock, portMAX_DELAY);

			delayModifier =0;
			//take the average and adjust to it on the next loop
			targetTimeAdjustment=timeadjustment;
			count=1;
			for(int i = 0; i < MAX_NUMBER_OF_CRAZYFLIE; i++)
			{
				if(data1[i].state && ((xTaskGetTickCount()-startTime)-data1[i].pos.timestamp)<5000){//ignore after 5 sec
					targetTimeAdjustment+=data1[i].tickdiff;
					count++;
				}
			}
			targetTimeAdjustment=round(targetTimeAdjustment/count);

			delayModifier=-((targetTimeAdjustment-timeadjustment)%(int)xFrequency);
			timeadjustment=targetTimeAdjustment;
			if(delayModifier>(int)(xFrequency/2))delayModifier-=xFrequency;
			else if(delayModifier<(int)-(xFrequency/2))delayModifier+=xFrequency;

			if(delayModifier>0)//can be done immediatly (place it directly before sending?)
			{
				vTaskDelayUntil( &xLastWakeTime, delayModifier );

				delayModifier =0;
			}

(I also apply the timeadjustment to the timestamp in the packet so that it could propagate if they didn't all hear each other)

I think your code could work with BLE=1 since it is so similar to mine, I tried compiling it and you seem to have a stack overflow error, which i 'solved' by reducing the TX queue size (It seems that the NRF chip is unfortunately close to its stack limit). I have 8 paquets each for the P2P and CRTP queues.

Ah thanks, I should (or whoever works with them next) get a debug adapter.^^

[knmcguire]

Hi @MadeInPierre and @HPeterPaulsen, thanks for looking into this. Me and @ataffanel are excited to get this into the firmware, so maybe it would be good to coordinate some kind of pull request? @MadeInPierre you mentioned that your code is not allowed to be public yet, but how about you @HPeterPaulsen ?

[knmcguire]

Currently we are working into getting at least the functionalities working from the first post of @ataffanel, so that the protocol can be added later (but that is something for another issue I think). Now we are focusing on simple receive and sending broadcast messages with intergration through the NRF and STM, so we will be able to have some commits the next few days.

[HPeterPaulsen]

@knmcguire "@MadeInPierre you mentioned that your code is not allowed to be public yet, but how about you @HPeterPaulsen ?" other way around. But maybe we can just make it public by now, will talk about it Monday.

Though mine is only really aimed at 1 use case (a crazyflie group that message each other constantly) so I don't know how useful it will be for providing p2p capability in general. (and comes with some algos for formation and flocking that use it but wouldn't be hard to separate)

[knmcguire]

Ah oke I understand. We currently implemented a very basic p2p broadcast messages system in both nrf and stm (bitcraze/crazyflie-firmware#484). We would like to keep this as generic as possible so we just made a function that makes it possible to send an broadcast P2P packet from a module or driver from the STM (which ports it through the NRF->NRF other CF-> NRF other CF). It's very simple now but this was the intention of this ticket anyway.

It would be good if you guys could review the changes and comment on what it is missing? We are planning to also add RSSI to it and to also enable p2p unicast messaging as well (specific address with ACK). For any protocol additions, we probably would need to make another issue for that.

[MadeInPierre]

Hi, thanks for addressing this issue!
I have worked until having broadcasted P2P packets working, I can subscribe to messages in the STM32 and send them as well.

• For the STM32 side, please see this dedicated p2p.c file in my fork. I have separate P2P queues and can create Callbacks just like with CRTP.
• For the NRF51 side, please see my main.c and esb.c files in my fork. I got inspired by @HPeterPaulsen's code so my version uses both of our brains :-)

However, I probably built a bit too much for this issue, as I started putting origin and destination IDs inside the packets (because i didn't have time left for working on the direct addressing for now).
If you find this code useful, please tell me what to change or remove and I'll gladly do a PR!

[knmcguire]

Ah great! Let me review the code but from a first view, how you implemented the NRF side is really similar to what has already been implemented by us before, so good we are on the same page. The only difference is that you also include the ID, while we were thinking to leave that up to the user to add it to the data array itself. What are your thoughts on that?

The p2p.c file on the STM side might come in very useful. Currently we have enabled STM receiving from radiolink, but parsing and such we haven't implemented. Also we would like to work with ports as well.

I will review your code and give you feedback soon on how to implement this.

[MadeInPierre]

The only difference is that you also include the idea, while we were thinking to leave that up to the user to add it to the data array itself. What are your thoughts on that?

I think these IDs could be completely removed if we had a direct addressing method between drones for P2P, I just included them in my code because I only made the broadcasts work. But I'd also think that this is something that varies between users :-) (e.g. I only use the last 4 bits of each drones' addresses to identify them, maybe this isn't enough for everyone).
So if we keep the simple broadcast method I think this is a good default solution for everyone, but if direct addressing is implemented the IDs wouldn't be that useful anymore.

Nice, glad I could help! I can simplify some things and create a single commit for this if needed.

One warning though, which is a bit off topic but may concern you later on: The more I add code in my firmware, the more it is difficult to flash the drones without having them shutdown immediately. I currently have to do multiple flash attempts before the drone starts successfully... Maybe creating separate queues for P2P messages takes too much space, i don't know...

[knmcguire]

I guess that is something that the user can add to the data array if they wanted it. currently we implemented the preamble of the package to be like this:

crazyflie2-nrf-firmware/src/esb.c

Lines 468 to 469 in 0649c56

	p2pPacket.data[0]= 0xff;
	p2pPacket.data[1] = 0x80|(port&0x0f);

Maybe that is enough for now.

I went through your code and there is seems to be enable p2p unicast messaging as well (currently we only implemented broadcast). We also see that you que the p2pmessages in the NRF, Very nice! But that the code is causing you trouble is a bit worrying though...

Maybe it would be better not to do it all in one go to be sure where it is going on. Here is maybe some good smaller intermediate goals for some stress-testing. Let's think about that first.

[knmcguire]

So the general idea now is:

• Finish broadcasting functionality and api on nrf and stm
• Finish functionality and api for unicast on nrf and stim
• Implement p2p ports
• ...

So we do it step by step, and do some intermediate stress-testing with our swarm test rig over here to make sure each step is not causing any errors or problems with the firmware. So probably a big pull request with everything might be too much at the moment but we can definitely use your implementation as inspiration on how to do this.

[MadeInPierre]

This is a great way of doing things. Just to clarify, the NRF51 code I use works like a charm and I never had a single problem with it, packets are always sent and received. The STM32 code with the p2p.c file alone worked perfectly too. It just started being more difficult to flash the STM32 when I added my own research algorithms on top of the P2P communication, so the problem may be unrelated. I just wanted to point the problem so that if you ever see the same thing, then the STM32 firmware with P2P communication may be too heavy for others to add software on top of the firmware, on the same chip (if others encounter the same problem, it would be good to put a warning about this on the website for example).

Otherwise, the code I presented without any additions works fine. I'd be happy to see unicast packets implemented as well!

[knmcguire]

Just a little update, we did some stress-testing and we found some preexisting bugs in the STL->NRF->ESB path way for the broadcasting. Kinda feels like we opened up pandoras box by adding peer to peer... However, still for some unknown reason, it is not possible to send a bigger buffer than 60 (not 61). So there is still something fishy going on, but we will write documentation mentioning this limitation.

[HPeterPaulsen]

Interesting I wonder whether it is related to the bug that doesn't allow using the whole 32 byte for the standard payloads bitcraze/crazyflie-firmware#48

[jonasdn]

@knmcguire @ataffanel what is the current status of P2P and this issue?

[knmcguire]

I guess the purpose was to implement the functionality for this particular ticket, and it is already there in the example folder of the crazyflie firmware. Biggest problem is, is that once the p2p is enabled, it's difficult to communicate with the PC from the client side

See here the forum post that notified that: https://forum.bitcraze.io/viewtopic.php?f=8&t=4596&p=21042&hilit=p2p#p21042

Maybe we should close this ticket as the functionality is already implemented, albeit only broadcast, and make a separate ticket for the client issues. What do you think @ataffanel ?

[ataffanel]

Yes, unicast is just broadcast with a receive filter, so the current functionality does implement P2P communication.

The link sharing issue might span on more than just the lib, but providing a relaxed mode in the lib and calling it P2P mode is a solution, imperfect but it would allow people to connect to Crazyflies using the P2P functionality ...