Page 2 of 2

Re: Using v2 lighthouses facing upwards

Posted: Thu Feb 25, 2021 5:47 am
by cafeciaojoe
Hi Kristoffer,

I checked out the latest version of the lighthouse positioning tab today in response to Tobias's battery test function blog post.

It looks like the calibration and geometry data are no longer hard coded into the firmware?
How do you define the origin now?

I assume there is some documentation coming house soon for the new tab, so I am happy to just wait for that if it is coming soon. I just want to guage wether or not I move to this version of the cf-lib and draw inspiration from the lighhouse positioning tab for our solution? Rather than our current solution which is building a script that will generate an origin and rotation matrix that will later be hard coded.


Re: Using v2 lighthouses facing upwards

Posted: Thu Feb 25, 2021 7:48 am
by kristoffer
Hi Joe,
yes, we are implementing a proper user interface for managing lighthouse systems. Geometry and calibration data will be stored in the persistent memory in the CF instead of being hard coded in the firmware. This means the lighthouse system can be used with the stock firmware and there will no longer be necessary to build a special lighthouse firmware. The UI will be limited to 2 base stations for now but we will add support for 2+ base stations later. We want to get a first official version out and I think 2 base stations is the most common use case so we settled for that. Geometry estimation for 2+ base stations is more complex (as you know :-) ) and we will have to contemplate a bit on how to do that.

Even though the UI is designed for 2 base stations, most of the functionality (in CF and client) should actually work for 2+ base stations as well. In your case I think using the persistent storage for geo and calib data would be a convenient solution. You could probably use the stock firmware which would simplify things.

The geometry data is read from the persistent storage at start up (if available). The main use case for calibration data is to read it from the light sweeps and store it to persistent memory to make it available immediately after boot. If you want to read calib data from the USB on the base stations, that is also fine. Note that we added a UID to the calibration data.
If you have more than 2 base stations, I have a slight worry that there will be interference between base stations when receiving calibration data through the light sweeps. If this is a problem turn on one base station at a time and let the CF get the calib data (you will get a notification in the console log and the lighthouse.bsCalCon log variable) which automatically will be stored in persistent memory.

There is now a system config file format that contains geometry and calibration data, that enables a user to save and load full system configurations. The easiest way for you to understand the file format is probably to save a file from the lighthouse tab and look at the file. It is pretty straight forward.
I'm not sure how you designed you tool, but this file format may be a way for you to transfer information?

There is a script in the crazyflie-firmware repo (tools/lighthouse/ that reads a system config file and writes it to the CF that might be useful to you.

Documentation is on its way, but we want to finish the implementation first.

Let me know it you want to set up a meeting for a more detailed explanation

Re: Using v2 lighthouses facing upwards

Posted: Mon Mar 01, 2021 5:51 am
by cafeciaojoe
I can't even get the drone to fly with the latest pull, with unmodified firmware, client and cflib... So i will try to get it fly on a multi bs setup with hard coded geo/calib values first. Then we can talk about moving it over once the tab/lib and firmware is more stable and there are docs...

Sould like a good idea to have my script spit out values in the form of the .yaml system config file. I will aim for that.


Re: Using v2 lighthouses facing upwards

Posted: Thu Mar 04, 2021 1:57 pm
by kristoffer
That sounds bad! I will hopefully spend some time in the flight lab tomorrow and will do some testing

Re: Using v2 lighthouses facing upwards

Posted: Sun Mar 07, 2021 8:38 pm
by kristoffer
I tried it in our flight lab on Friday and it seemed to work fine both with V1 and V2. What kind of problems did you see?

Re: Using v2 lighthouses facing upwards

Posted: Thu Mar 11, 2021 7:21 am
by cafeciaojoe
I tried again in our lab, clean install new drone. all works ok! So not sure what that was.

Just waiting now for my engineer friend to finalise the modifications to the

Code: Select all

class LighthouseBsGeoEstimator
before we move on to integrating it into the newest version of the Lighthouse Tab.


Re: Using v2 lighthouses facing upwards

Posted: Thu Mar 11, 2021 9:37 am
by kristoffer

We are very close to finishing the SW for the first release of lighthouse, and the code should be more stable.

Re: Using v2 lighthouses facing upwards

Posted: Fri Mar 26, 2021 11:40 pm
by cafeciaojoe
Hi Kristoffer!

My engineering friend Tim has worked on this problem for the last couple of weeks in his spare time. We are at the point where we would like you to review the work and see if there is anything obvious that might have been missed. His message Below:
We’re trying to design and implement a system where we have BS’s on the ceiling and floor, to support tracking when the CF is upside down. We’ve gotten advice in previous posts about doing this, and our current high level approach is the following:

Update get_bs_geometry such that it’s able to accurately estimate BS locations with the CF on it’s side (as a sideways orientation puts the sensors in view of both BS’s on the floor and ceiling)
Be able to stitch together multiple runs of get_bs_geometry to put all the BS’s into a single world reference frame, as not all BS’ will be able to be in view of the CF sensors in just one run.

This post is basically to check in and make sure our approach for (1) makes sense, and we haven’t made any errors in interpreting the current code. Our approach is laid out in a series of Jupyter notebooks:

(1)Finding the initial yaw guess for the CF. ... uess.ipynb
(2) Converting the initial yaw guess to an initial pose guess for the OpenCV solver. ... n-cv.ipynb
(3) Solving for the BS’s pose with OpenCV ... -pnp.ipynb

(Note you may have to hit “reload” a few times on the notebooks in GitHub to get them to work, and that the 3D plots won’t render in GitHub)

We’ve tried to lay out our approach and assumption as clearly as possible - would it be possible to take a look at these, especially (1) and (2), to sanity check what we’re doing? We’ve done a fair bit of interpreting of the current code and what it’s doing in order to make changes, and want to make sure our interpretation is correct.
Also, if you have the time we should probably just meet over a video call :D

Thanking you in advance!

Joe and Tim

Re: Using v2 lighthouses facing upwards

Posted: Mon Mar 29, 2021 12:02 pm
by kristoffer
Hi Joe and Tim!

I have done some more testing and thinking and as far as I can see the estimated position of the base station is mirrored when the base station is upside down. In this case, the correct position seems to be (-X, -Y, Z) of the estimated position. I have not looked at the rotation, but I would expect it to be some sort of mirror solution as well.

The reason we get this is that the initial guess is wrong since the sweep is going in the "wrong direction" when the base station is up side down, and the estimation function in open CV ends up in the wrong local minima.

So when is the base station upside down? I think it is upside down when the Z axis of the CF and Z axis of the BS are pointing "in opposite directions", (Z_CF dot Z_BS < 0).

I looked at the pictures in (1) and I think that when the CF is on the side (Z-axis along the floor) the solution for the BS on the ground will be OK with the current code, while the BS on the tripod will get a mirror solution. Correct?

Maybe you can exploit this if you know which base station that is on the tripod?

Anyway, your suggested solution looks reasonable, even though I have not tested it. I would have expected that you only had to do something for the initial guess, but maybe the rotation must be handled in some way as well?

Let's have a meeting and discuss it, just drop me an email!