Inter ranging between multiple crazyflies by using the Loco decks

shushuai3 commented 4 years ago

This issue will discuss a new feature of inter two-way-ranging between multiple crazyflies, which will lead to relative localization and control between multiple crazyflies without relying on the loco positioning nodes on the ground.

knmcguire commented 4 years ago

Thanks @shushuai3 ! This seems like it could be combined with the PR #574 as indicated in #567. Could you tell a bit how you would like to implement it? Then we can comment as well on what is handy in the firmware

Would this function be a part of the LPS-deck driver?
Is this considered to be always on or should it be activated by a parameter or flag?
How many crazyflies should communicate/range with eachother. What is the limit?

keffklau commented 4 years ago

Hello Kimberly, hello shushuai3,

such a function would be really great. We also plan to make use of inter-agent range measurements for improving navigation quality. However, we would like to use CF as mobile anchors in addition to the fixed mounted LPS nodes. So we would prefer a solution combining both distance measurements TDoA3 with anchors, ToA/TWR in between Crazyflies.

In order to combine those measurements, state&covariance information concerning the navigation filters would have to be exchanged. Is there a possibility to encode those within the UWB messages since this would reduce time-drifts between measurement and filter data?

We would like to pass the inter-agent range measurements (and agent data) to the navigation filter but the measurement itself could be done within the deck driver.
We would like to be able to switch it on and off (in order to show the effect on navigation quality)
It's important for us, that this approach does not cause issues with the flow deck, since we want to combine information
From our point of view, CFs should randomly initiate range measurements with each other so the number of CFs can be arbitrary

@shushuai3 I am not sure, whether this is in line with your objectives, hopefully there is no contradiction

krichardsson commented 4 years ago

Hi all!

The idea of (sometimes) ranging between Crazyflies and have them act as anchors from time to time, is something we discussed when we wrote TDoA3 (internally we usually call it TDoA4). In my opinion, the best way to implement such a functionality would be to reuse the TDoA3 protocol, which would make it compatible with anchors running TDoA3. In the TDoA modes, anchors are constantly ranging with each other and the TDoA part is a "side effect", so one way to look at this is that the only thing that needs to be added is that Crazyflies sometime also may transmit packets.

@keffklau One solution to add new data (for instance state&covariance information) to the protocol, and still staying backwards compatible would be to use LPP data. It is currently used for anchor position data but other formats could be added.

krichardsson commented 4 years ago

By the way, I think it is very exciting that you are looking at this functionality!

shushuai3 commented 4 years ago

TDoA between crazyflies mentioned by @keffklau and @krichardsson must be interesting and have certain advantages than TWR. Currently, I only get the TWR method worked between crazyflies. Based on the distance, relative localization and formation flight of 5 crazyflies are achieved, and the paper link is https://arxiv.org/abs/2003.05853

@knmcguire Thanks for your reply. As for how I will implement it, 1) I propose that it could be a new uwbAlgorithm such as lpsMode_TWR_ICs (inter-crazyflies), following by lpsTwrIcs.c and lpsTwrIcs.h files. 2) It is better to have a flag to activate it at first. 3) Currently, a fully connected communication loop can be running on an arbitrary number of crazyflies. The limits are: the ranging frequency decreases as the number increases; all crazyflies have to fly within the maximum ranging distance.

@keffklau Thanks for your interest. As said by krichardsson you could use LPP data or customized package to communicate with the UWB. All characteristics you mentioned are in line with my objectives.

@krichardsson Thanks for your reply. TDoA between crazyflies looks like easier to be compatible with anchors. But the compatibility of TWR between crazyflies is difficult to maintain since the communication topology may be variant.

krichardsson commented 4 years ago

TDoA between crazyflies looks like easier to be compatible with anchors. But the compatibility of TWR between crazyflies is difficult to maintain since the communication topology may be variant.

I'm not sure I was clear in what I meant. What I suggest is to do two way ranging between Crazyflies, but using the TDoA3 radio protocol. When anchors are running in TDoA3 mode, that is exactly what they do, they do TWR with each other. The TDoA information that is extracted by Crazyflies in normal TDoA3 is just a bonus, something that can be extracted by anyone listening to the packet flow in the air.

The memory structures in the Crazyflie for the TDoA engine (TDoA3) are designed to be fairly dynamic when it comes to anchor topology. The idea was that if a crazyflie flies through a large TDoA3 system it will see new anchors as it moves on, as well as lose old ones. This is accounted for and the structures are updated continuously. I have not looked at exactly what would have to be changed, but I think there is a pretty good chance the structures could be modified to handle TWR between Crazyflies as well.

keffklau commented 4 years ago

Thank you all for your advices - good to know, that LPP data could be used also for communicating the required data for fusing information. On our side cooperative navigation is a project we intend to launch in the the end of 2020/ early 2021.Our aim is to improve positioning accuracy for a heterogenous team, some CFs acting as additional anchor (especially those being equipped with an additional Flow Deck) and others only being equipped with LPS decks and maybe the multiranger deck to take the role of a scout detecting obstacles - so combining different abilities. Honestly, its currently more in a "vision-status" since my lab is just in an early ramp up phase. But thanks to what I got from your posts I am much more convinced that it could be done with reasonable effort with a small team.

As a remark: My first try would also have been to use ToA measurements in between two CFs but due to a potential higher accuracy (at least from what I have read so far on UWB positioning). Having TDoA is available for the other listening CFs is quite a nice add-on, I haven't thought of this yet.

@shushuai3 its really impressive what you already did at TU Delft and your Lab, thanks for providing the paper link, I will certainly take a closer look on your results and read it in detail. I found other papers from your group but I have to check whether our library has access to Science Robotics - I am afraid this might not be the case.

shushuai3 commented 4 years ago

I have not looked at exactly what would have to be changed, but I think there is a pretty good chance the structures could be modified to handle TWR between Crazyflies as well.

I just read through the TDoA2 and TDoA3. The random communication part is definitely helpful for a large swarm of crazyflies. But first I will provide the ranging code of what I did before, i.e., pairwise ranging where only one crazyflie is in transmitting mode at a specific time while the others are in receiving mode.

knmcguire commented 4 years ago

Hi @shushuai3, It would be good to intergrate it in existing structures of the firmware. We are starting to run out of memory so its good to reuse stuff that is already there. But this seems light weight enough, so you could already start a pull request with this implementation on the LPS deck on the latest master, and then we have a better view of what your vision is on how to implement this :)

krichardsson commented 3 years ago

We have got pull requests #580 and #656 that are touching this area (thanks!) and also interest through email and other contacts which shows that this is functionality that is desired by a part of the community. It is a complex area but I have been trying to figure out a way forward to implement a basic functionality that is useful for "normal" users while it still can be extended by power users to provide the required flexibility. Ideally the implementation should be done in smaller steps that adds value and moves the firmware in the right direction to enable us to learn as we go. My suggestion is to divide the work into two major steps and first implement extensions to LPS and secondly add functionality to handled and consume inter drone ranging data. The major steps should probably be split into multiple smaller steps.

Step 1 LPS extensions

There are two basic approaches to add inter drone ranging (internally called TDoA4 by tradition) to LPS, either we could add a new ranging mode or we could extend the current TDoA3 mode. There is a bit of mix up in terminology when it comes the the ranging modes (TWR, TDoA2 and TDoA3), do we mean the implementation or the protocol on an UWB level? To me the answer is a bit of both but mainly the protocol, and it turns out that we can do most of what we want using the TDoA3 protocol (with some extensions). This is also the solution that I propose. Some of the benefits are:

It minimizes the required changes in the client, anchors and Crazyfie firmware. This is mostly important for the anchors where we are almost out of both flash and RAM.
It will (probably) be backwards compatible with the current anchor firmware for users that are only interested in the ordinary TDoA3 functionality, which means they will not be forced to re-flash anchors.

The proposed name of the extended functionality is TDoA3 Mixed Mode and is essentially TDoA3 but where the Crazyflies also have the ability transmit TDoA3 packets and thus range with devices around them (anchors or other Crazyflies).

The default functionality of TDoA3 will be identical to the current functionality, a Crazyflie passively listens to anchors to determine its position.

When Mixed Mode is enabled in a Crazyflie, it will transmit packets the same way as anchors do while it still is listening to the traffic, interacting with the anchors or other Crazylies with Mixed Mode enabled. The result is that it will get ranging information to other devices (essentially TWR) while it sill can use the TDoA information. It will also act as an anchor and other Crazyflies can listen to the transmission and use them for TDoA, that is the Crazyflie acts as a mobile anchor.

The default use case for TDoA3 Mixed Mode would be to enhance positioning where TDoA degrades, for instance outside the convex hull.

Step 2 Inter drone ranging handling

When there is ranging information to other drones the next step is to make it available for consumption by an application. The solution probably depends on the needs but one option could be to add it to the peer localization module for instance.

Configuration

It is easy to imagine many ways to use a system like this (when to range, how often, where to send ranging or TDoA information to the state estimator or not and so on). Configuration through parameters is probably too limiting in this cases and I suggest to also use extension points where the user can plug in functions to get the desired behaviour. A default set of functions would provide the standard functionality described above.

The way forward

I think the next step should be to split up the work in smaller steps that can be implemented in individual pull requests. Smaller pull requests are easier to handle and understand for the maintainers, and it will be possible to discuss and tweak the implementation as a part of the process.

I would be happy to write up a backlog with the smaller steps if this is the route we want to take.

We have also added a contribution guide to help contributors write high value pull requests.

krichardsson commented 3 years ago

We discussed the name of the functionality and the new suggested name is "TDoA3 Hybrid Mode", I think it sounds better than "Mix Mode" :-)

shushuai3 commented 3 years ago

Nice solution. I will also look into the code soon.

krichardsson commented 3 years ago

Thanks @shushuai3 ! Do you think it would work for what you want to do?

krichardsson commented 3 years ago

I added an example implementation of TDoA3 Hybrid mode here: https://gist.github.com/krichardsson/c1a51642d2d560d825f7bee8577ac6c6

It is only briefly tested on my desk with a couple of anchors and I have not had the possibility to try it out in our flight lab yet, it is possible that it does not work that well.

Enable TWR by setting an unused id in the tdoa3.hmId parameter and setting tdoa3.hmTwr=1. TDoA can be turned off by setting tdoa3.hmTdoa=0

shushuai3 commented 3 years ago

I found there are lots of differences between the Hybrid example file and the lpsTdoa3Tag.c in the master branch. Do I have to use the complex example file? If not, it would be easier to incorporate the TWR code into the original lpsTdoa3Tag.c file.

krichardsson commented 3 years ago

I think the example code in the gist is compatible with the current version of lpsTdoa3Tag.c in master if you want to try it out. There was one function that had changed since I created the gist but I fixed that so all should be good now.

It should be possible to simply replace the contents of lpsTdoa3Tag.c with the code in the gist.

The example I created was mainly intended to indicate the type of integration that we would prefer for a hybrid mode implementation when it comes to modifications of the existing system. The example implementation will not change the behaviour of the system for "normal" usage, but for advanced users it is possible to enable the hybrid mode via a parameter. The protocol on the UWB level is also unchanged which is nice since there are lots of systems used in the world that would have to upgrade the anchors if the protocol is not backwards compatible.

If it is easier for you to start from the version in master that is fine too.

HaoFangHowFun commented 1 year ago

Hi! I'm currently working on the implementation of the TDOA3 mode for the crazyflies. My goal is to get both the distance difference from anchors and the distance between each flies. To achieve this, I'm utilizing the TDoA3 Hybrid Mode. However, I'm facing two issues related to this topic.

I have two crazyflies operating in the designated area, and they are transmitting packages to each other. The blinking TX LED indicates that the transmission is functioning properly. However, I'm uncertain about the log variable "hmest." It consistently provides fluctuating values such as 40, 160, 130, and so on. Is there another log variable that can directly provide me with the distance and ID information originating from the other crazyflie?
Additionally, I'm wondering if there is a log variable available that can provide the difference in distance from each anchor in the TDOA3 mode. I noticed that such a variable exists for TDOA2, but I haven't found an equivalent for TDOA3.

I appreciate any guidance or assistance you can provide regarding these concerns. Thank you!

krichardsson commented 1 year ago

It was a while ago I wrote that gist, I hope it is still OK (I have a faint recollection there was something missing...). Anyway, the hmest log looks like it shows the rate of TWR measurements sent to the estimator (in measurements/s).

It does not look like I added any log for distance, but you could add that around line 253

Additionally, I'm wondering if there is a log variable available that can provide the difference in distance from each anchor in the TDOA3 mode. I noticed that such a variable exists for TDOA2, but I haven't found an equivalent for TDOA3.

You'll find it in tdoaEngine.tdoa. This will give you the difference in distance for the anchors set in parameters tdoaEngine.logId and tdoaEngine.logOthrId

HaoFangHowFun commented 1 year ago

Hello! Thank you for your response. I have made an attempt to add additional distance logging data around line 253. However, it seems that the TWR ranging results are only coming from the anchor and not between the crazyflies themselves. My intention is to obtain the distance between the crazyflies specifically. The data I'm currently logging includes distance, x, y, and z coordinates.

Regarding the second part, I'm able to obtain the difference in distance between the primary and secondary anchors by setting the ID parameter in TDOA. Now, I'm wondering that is that a way to acquire all of the distance differences over time without constantly changing the ID parameter.

Allow me to briefly describe the project I'm working on. My objective is to treat the crazyflie as a sensor on my mobile robot. I want the crazyflie to receive the distance differences from all anchors using TDOA, as well as obtain the distance between other crazyflies using TWR simultaneously.

krichardsson commented 1 year ago

However, it seems that the TWR ranging results are only coming from the anchor and not between the crazyflies themselves. My intention is to obtain the distance between the crazyflies specifically.

Have you set ctx.isTwrActive to true? And also ctx.anchorId to an id that is not used by an anchor?

Regarding the second part, I'm able to obtain the difference in distance between the primary and secondary anchors by setting the ID parameter in TDOA. Now, I'm wondering that is that a way to acquire all of the distance differences over time without constantly changing the ID parameter.

No, there is no such functionality. A useful tool when working with a system like this is to configure an anchor as a sniffer. This enables you to see all the traffic in the air (at least everything the sniffer receives) on a PC. You can configure it as a sniffer using USB and the low level config. When this is done, you will get all packets on USB. You can use the scripts in https://github.com/bitcraze/lps-node-firmware/tree/master/tools/sniffer to unpack and look at data.

I can not find any instructions on how to use the tools but the idea is to pipe the output of one tool into the next, roughly something like:

python sniffer_binary.py [some arguments] | python tdoa3_decoder.py

bitcraze / crazyflie-firmware