Pointers for multiple CFs with the Loco?

All discussions related to the Loco Positioning system
Post Reply
ccpost
Beginner
Posts: 15
Joined: Mon Jan 23, 2017 7:35 pm

Pointers for multiple CFs with the Loco?

Post by ccpost »

Hi,

I'm starting to work on getting multiple Crazyflies in the air together using the Loco anchors. In general, I'm looking for any wisdom from people who've tried the same.

I understand I have 2 options: TWR-TDMA and TDoA. Which would you recommend exploring first? Is TWR-TDMA more supported / stable then TDoA?

For TWR-TDMA, it seems like I need to enable `LPS_TDMA_ENABLE`, set `TDMA_NSLOTS_BITS` to (for example) 3 if I want to fly 8 units, and set `TDMA_SLOT` for each individual unit.
Can the anchors just stay in their normal TWR mode (as if you're flying a single tag)?
What's the practical limit on how many tags you can fly at once this way?

For a single unit, I ended up calibrating `ANTENNA_OFFSET` in `locodeck.c` slightly to zero out my absolute ranging distances. I notice it's also defined in `lpsTwrTdmaTag.c`, and there's some code duplication between `lpsTwrTag.c` and `lpsTwrTdmaTag.c`. I assume the `locodeck.c` and `lpsTwrTag.c` code paths are the most up-to-date and those are the preferred implementations if I want to look into DRY-ing this up a bit?

It seems that TDoA mode acts as receive-only on the tag side. Is that true? If so, I guess there's not a real limit on the number of tags that can be running at once?

Thanks,
-Chris
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Pointers for multiple CFs with the Loco?

Post by arnaud »

Hi,

TWR-TDMA and TDoA are both equally experimental, they have different shortcoming:
- TDoA is a bit harder to get stable than TWR. For instance it is very important to be in the convex hull formed by the anchor and it is (in its current implementation) very sensitive to lost packets.
- TWR-TDMA has the advantage of TWR when it comes to anchor placement and packet loss resilience, though the implementation is now a bit outdated compared to the TWR implementation (I am currently fixing that ...), and when you add slots the update rate will very quickly become too low (this could be greatly optimized, hopefully I will have time to look at it this summer).

With the current implementation of TWR-TDMA, the anchor does not have to be changed, they can be in twr mode. So far we have been using it with 4 anchors, more might limit the update rate a bit too much, to be tested. The main problem with TWR-TDMA is that the performance will always depends with the number of tag present and so scaling the system can become hard. But I think it can still be greatly improved, for instance we could put each anchors in a different channel/preamble which, in theory, would allow to multiply by 6 the number of tags. The ranging algorithm could also be improved to use 2ms timeslot instead of the current 4ms (the current ranging takes just over 2ms).

Your assumption about `lpsTwrTdmaTag.c` vs `lpsTwrTag.c` is correct, this is what I am currently working on. Originally TDMA was implemented as a lot of #ifdefs in `lpsTwrTag.c`. At some point they where separated to remove the ifdefs but over time the result is that the TDMA implementation has been left behind. I want to merge back the two files by adding packet scheduling to the `lpsTwrTag.c` file.

Yes TDoA mode is receive only on tag side, this is its big advantage and makes it very appealing since it is scaling very well and does not require to setup IDs in the Crazyflies. Though TDoA is in theory a bit more noisy and has much more restrictions when it comes to anchor position.

I hope this helps, I should push some update to the TDoA code today or early next week, I will update this thread when it is done.
ccpost
Beginner
Posts: 15
Joined: Mon Jan 23, 2017 7:35 pm

Re: Pointers for multiple CFs with the Loco?

Post by ccpost »

Thanks, that's very helpful.

So, based on viewtopic.php?f=16&t=2527, the update rate for a full set of ranges is: 1 / (0.004 * 2 ^ TDMA_NSLOTS_BITS * N_ANCHORS). At what point do you think that rate gets slow enough that the Kalman filter has a hard time getting a good position estimate? (Just curious about a ballpark.)
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Pointers for multiple CFs with the Loco?

Post by arnaud »

I just pushed a commit that merges TWR and TWR-TDMA in the same file. Now TDMA can use the anchor position transmitted by the anchors (makes it much easier to test ...).

I just tested with 6 anchors running a small autonomous sequence and my results are:
- with TDMA_NSLOTS_BITS=1 I see no big difference than without TDMA
- with TDMA_NSLOTS_BITS=2 It starts to be a bit noisy
- TDMA_NSLOTS_BITS > 2 start to be very noisy!
- At TDMA_NSLOTS_BITS = 4 it still flies! Though it is very noisy

The first things that suffers is the altitude (Z position), this is though to be mainly due to the accelerometer affected by noise and the less UWB measurement we have the more the accelerometer affects the estimation.

In theory the number of anchor should not affect the result so much as long as the anchors are spread around (would be very interesting to test with different anchor scanning order): each measurement is pushed in the EKF independently and the EKF has no idea of how many anchors are present anyway.

There will be at least 2 ways of improving the situation: Improving the UWB measurements quality and rate and improving the EKF.

For the EKF I did a quick hack test: I increase the stddev of the UWB measurement with the number of TDMA bits (not sure if it makes any sense but intuitively it kind of did for me :-)):

Code: Select all

diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c
index 2397984..3687350 100644
--- a/src/deck/drivers/src/lpsTwrTag.c
+++ b/src/deck/drivers/src/lpsTwrTag.c
@@ -198,7 +198,11 @@ static uint32_t rxcallback(dwDevice_t *dev) {
         dist.x = options->anchorPosition[current_anchor].x;
         dist.y = options->anchorPosition[current_anchor].y;
         dist.z = options->anchorPosition[current_anchor].z;
-        dist.stdDev = 0.25;
+        if (options->useTdma && TDMA_NSLOTS_BITS>0) {
+          dist.stdDev = TDMA_NSLOTS_BITS * 0.25;
+        } else {
+          dist.stdDev = 0.25;
+        }
         estimatorKalmanEnqueueDistance(&dist);
       }
This seems to improve the situation a lot at TDMA_NSLOTS_BITS=3, above that the error in Z is anyway very high, but at least the flight is a bit less noisy.
ccpost
Beginner
Posts: 15
Joined: Mon Jan 23, 2017 7:35 pm

Re: Pointers for multiple CFs with the Loco?

Post by ccpost »

I see the commit, thanks so much! I'm looking forward to trying this out.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Pointers for multiple CFs with the Loco?

Post by arnaud »

For info: I pushed a commit that allows to set the slot using the Crazyflie radio address, this makes it much easier to work with TDMA. I also documented a bit on the wiki: https://wiki.bitcraze.io/doc:lps:twr-tdma.

This completes my clean-up round for TDMA, we will look at TDoA later in the summer.
Post Reply