Author: Kristoffer Richardsson

A common task with the Crazyflie is to add a new controller or estimator. As we get some questions on how to do this, we will outline the process in this post. We will show how to add a custom controller and estimator that runs in the Crazyflie, built as an out-of-tree build.

This post assumes some basic knowledge about the Crazyflie firmware, the C programming language, how to build the firmware and flash it to a Crazyflie. If you need some more information on these topics, please see the “Getting started with development” tutorial. For an overview of how estimators and controllers are used by the stabilizer module, please see the firmware documentation.

Overview

The Crazyflie firmware is designed to make it easy to add custom controllers and estimators, a plugin system keeps the code clean and well separated. We will look at the details later, but the basic principle is to first write your new controller or estimator and then register it in the firmware. When the code has been compiled and flashed to the Crazyflie, the new module is activated by setting a parameter from the client or a python script.

We will implement the example as an app, which is a great way to make sure you can upgrade the underlying firmware without messing up your code. An app is a piece of code that exists somewhere in you file system outside of the main firmware source code. This setup minimizes the dependencies and the main firmware source tree can be upgraded without affecting your app (in most cases). That means there is no need for merges or complex management of source trees.

Registration of modules

Let’s first look at how controllers and estimators are registered and called in the plugin framework. We will use the controllers to show how it works, but the estimators are implemented in a similar way and it should be easy to understand how it works.

Note that there has been some updates of the Crazyflie firmware source code lately and any reference to the source code will be to the latest version (as of today).

The starting point of the controller implementation can be found in the src/modules/src/controller.c file, here we can find an array called controllerFunctions that holds a list of all the controllers in the system.

static ControllerFcns controllerFunctions[] = {
  {.init = 0, .test = 0, .update = 0, .name = "None"}, // Any
  {.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
  {.init = controllerMellingerFirmwareInit, .test = controllerMellingerFirmwareTest, .update = controllerMellingerFirmware, .name = "Mellinger"},
  {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
  {.init = controllerBrescianiniInit, .test = controllerBrescianiniTest, .update = controllerBrescianini, .name = "Brescianini"},
  #ifdef CONFIG_CONTROLLER_OOT
  {.init = controllerOutOfTreeInit, .test = controllerOutOfTreeTest, .update = controllerOutOfTree, .name = "OutOfTree"},
  #endif
};Code language: PHP (php)

We can see that there is currently four controllers in the list: the PID controller, the Mellinger controller, the INDI controller and finally the Brescianini controller. There is also an “empty” controller at the top that is not important in this context and we will simply ignore it. At the bottom we find the out-of-tree controller, we will discuss this later.

Each controller must implement three functions: an initialization function, a test function and a controller function that performs the actual controller work. Signatures for the three functions are defined in controller.c. The functions are added to the list as function pointers that can be called by the stabilizer when needed.

There is a parameter, stabilizer.controller, in the stabilizer that tells the system which controller to use in the stabilizer loop. This parameter simply contains the index in the controllerFunctions list that will be used. For example, the default value 1 will make the stabilizer loop call the controllerPid function every iteration. If the value of the stabilizer.controller parameter is changed, the initialization function for the new controller will be called and subsequent calls from the stabilizer loop will be done to the new controller function.

We will not go into details of how to implement the actual controller here, but the existing controllers can be used as examples.

There is a similar list/implementation for estimators that can be found in src/modules/src/estimator.c.

Adding a new controller

Suppose you want to add a new controller. It would be possible to add a new file in the Crazyflie firmware with your new controller implementation, add the function pointers to the list in controller.c and that would work just fine. The problem with such implementation would be that it is hard to maintain, your new files would be mixed with the files in the main firmware file tree, and even worse, you would have to modify the controller.c file to add your controller. The next time there is a new awesome feature in the firmware source code and you want to upgrade to the latest version, you will run into problems as you have to handle the files you modified!

A better solution is to use an app instead as apps are built out-of-tree, that is not in the main source tree. This removes the problem of merging changes in the main source files, all you have to do is to pull in the new file tree and recompile.

But how to register your new controller in the controller list? This is what the last line in the list of controllers is for

// ...
#ifdef CONFIG_CONTROLLER_OOT
  {.init = controllerOutOfTreeInit, .test = controllerOutOfTreeTest, .update = controllerOutOfTree, .name = "OutOfTree"},
#endif
// ...Code language: PHP (php)

If CONFIG_CONTROLLER_OOT is defined we add a controller with the three functions controllerOutOfTreeInit, controllerOutOfTreeTest and controllerOutOfTree. All you have to do in your app is to define CONFIG_CONTROLLER_OOT and make sure the functions in your controller are named like above. That’s it!

Example implementation

Now we will create a new app and add a new controller, step by step. We assume that you have a newly cloned firmware repository in your filesystem to work on.

We will show the linux flavor of commands, but it should be easy to convert to other platforms.

Create a new app

The easiest way is to start from an existing app to get started, let’s use the hello world app. Copy the app and move into the new directory

cp -r examples/app_hello_world examples/my_controller
cd examples/my_controller/

Let’s rename hello_world.c

mv src/hello_world.c src/my_controller.c

We have to tell kbuild that we renamed the file. Open src/Kbuild in your favorite editor and update it to

obj-y += my_controller.o

Now let’s fix the basics in my_controller.c, open it in your editor and change according to the comments bellow:

#include <string.h>
#include <stdint.h>
#include <stdbool.h>

#include "app.h"

#include "FreeRTOS.h"
#include "task.h"

// Edit the debug name to get nice debug prints
#define DEBUG_MODULE "MYCONTROLLER"
#include "debug.h"


// We still need an appMain() function, but we will not really use it. Just let it quietly sleep.
void appMain() {
  DEBUG_PRINT("Waiting for activation ...\n");

  while(1) {
    vTaskDelay(M2T(2000));

    // Remove the DEBUG_PRINT.
    // DEBUG_PRINT("Hello World!\n");
  }
}Code language: PHP (php)

Now, lets add our new controller. We will not add a real implementation here as it would be a bit too large for this post, instead we will just call into the PID controller to make sure the Crazyflie still can fly. Add this code after appMain() in my_controller.c.

// The new controller goes here --------------------------------------------
// Move the includes to the the top of the file if you want to
#include "controller.h"

// Call the PID controller in this example to make it possible to fly. When you implement you own controller, there is
// no need to include the pid controller.
#include "controller_pid.h"

void controllerOutOfTreeInit() {
  // Initialize your controller data here...

  // Call the PID controller instead in this example to make it possible to fly
  controllerPidInit();
}

bool controllerOutOfTreeTest() {
  // Always return true
  return true;
}

void controllerOutOfTree(control_t *control, const setpoint_t *setpoint, const sensorData_t *sensors, const state_t *state, const uint32_t tick) {
  // Implement your controller here...

  // Call the PID controller instead in this example to make it possible to fly
  controllerPid(control, setpoint, sensors, state, tick);
}
Code language: PHP (php)

Finally we need to tell the firmware that we have implemented the out-of-tree controller and that it should be added to the list. We do this by adding CONFIG_CONTROLLER_OOT to the app-config file. When you are done it should look like this:

CONFIG_APP_ENABLE=y
CONFIG_APP_PRIORITY=1
CONFIG_APP_STACKSIZE=350
CONFIG_CONTROLLER_OOT=y

Testing it!

Build and flash the firmware to your Crazyflie:

make -j8
make cload

Start your Crazyflie and the python client. Connect the client to the Crazyflie and open the console log tab. Make sure you are running your app by looking for the line:

MYCONTROLLER: Waiting for activation ...Code language: HTTP (http)

Now let’s activate our new controller! Open the parameter tab, find the stabilizer group and the controller parameter. Set it to 5 and check the console log that the out-of-tree controller was activated:

CONTROLLER: Using OutOfTree (5) controllerCode language: HTTP (http)

That’s it! Your new controller is activated and the Crazyflie is ready to fly.

Note: In the client, the comment for the stabilizer.controller parameter will not contain the out-of-tree controller, and it will look like only values 0-4 are valid even though 5 also works.

Conclusions

In this post we have shown how to add a new controller to the Crazyflie firmware. The process for adding an estimator is very similar, and hopefully it should be easy to understand how to do it based on the example above.

As you can see, very little code (apart from the actual controller/estimator) is required to add your own controller or estimator, and we hope that it will enable you to put your energy into the actual control problem, rather than the nitty gritty details of the code.

Happy coding!

Santa is soon to be knocking on the door, hopefully with one or two exciting toys (with blinking LEDs) for us geeky people! There will not be a Christmas video in the Bitcraze gift this year, instead we’re wrapping up a new release that we hope will add to the Christmas fun!

We have been working on a secret project though and there might be a video for next week’s blog post showing what we have been up to…

The 2022.12 release

We are happy to announce that a new official release is out, 2022.12! We have mainly fixed bugs and stability issues but also added some new features, please see details below.

Crazyflie STM firmware (2022.12)

One of the main events in this release is that the Flapper Nimble+ has got official support with the flapper platform, it can now be flashed through the client like any other member of the Crazyflie family. A new controller, based on work by Brescianini has been added. The Kalman estimator and Lighthouse system have been tweaked to work better with the increased data volumes generated with 2+ base stations. Some improvements for brushless motors have been added. Finally there have been some general bug and stability fixes, including improvements for flashing of the AI-deck.

Please see the release notes for a list of all changes.

Crazyflie NRF firmware (2022.12)

The NRF firmware release mainly contains changes to support the new STM firmware.

Please see the release notes for a list of all changes.

Crazyflie lib python (0.1.21)

A blocking method has been added to upload trajectories to the high level commander, the various Uploader classes in the examples are not needed anymore. Stability and bug fixes related to deck flashing.

Please see the release notes for a list of all changes.

Crazyflie python Client (2022.12)

A button has been added in the console log tab to get statistics about persistent storage in the Crazyflie. The final traces of Windows and Mac builds have been cleaned out and some stability and bug fixes have been applied.

Please see the release notes for a list of all changes.

IROS in Kyoto is over and all Bitcrazers are finally back in Sweden again. We had a really good time in Japan and enjoyed all the interesting discussions we had with all of you, thanks!

In this blog post we will describe the demo we were running in the both and talk a bit about all the cool tech that was used. If you want to reproduce it at home or just take a look for inspiration, the code is available on github in the iros-2022 branch of our experimental firmware repo. There is also a page on our web for IROS 2022 with some more information.

The demo has similarities with our previous demo (see IROS 2019) but has been upgraded to be a fully autonomous and decentralized swarm with 9 Crazyflies buzzing around in a cage, going back to charging pads for wireless charging when the battery is running out. The demo supports multiple Crazyflies flying at the same time, avoiding collisions without a central authority, all decision making is done in each Crazyflie, that is fully decentralized.

The hardware is off-the-shelf products available in our store (links here). The software is obviously written specifically for the demo, but we wanted to use the building blocks already available in the system so the demo code is mainly “glue” to connect them together.

The cage/flying space

The flying space was box shaped, 3×2 meters in foot print and 2.5 meters high. We enclosed it in our lightweight travel cage made from aluminium pipes and a light net. It is a pretty small space to fly multiple Crazyflies in at the same time but it worked! The main problem with such a small space is down-wash from other Crazyflies and having enough room to avoid collisions. 3 Crazyflies worked pretty well, but had the space been larger it would have been possible to fly all nine.

Localization

Localization was handled by the Lighthouse positioning system. We used two base stations and the lighthouse deck on each Crazyflie which provides the Crazyflies with their current position with high accuracy.

Since the position is computed in the Crazyflie, using only data from on-board sensors, no external communication is needed in relation to the localization system. The only exception was that we uploaded the physical geometry of the system when setting up the cage.

Path planing

When a Crazyflie is flying in the demo, the standard mode of operation is to fly a randomized pattern of straight lines. From time to time (randomized) the Crazyflie can also chose to fly the spiral that we have used in earlier demos (see the IROS 2019 demo for instance).

When the battery is running out, the Crazyflie goes back to the charging pad for charging. The position is sampled before taking off and this coordinate is used as the landing point to find the charging pad. When landed the Crazyflie verifies that the battery is being charged. If the battery is not charging the Crazyflie assumes it missed the charging pad and it takes off again to adjust the position.

Charging

The Crazyflies were equiped with the Qi-charging deck for wireless charging. The charging pads are 3D-printed pads with a slope to make the Crazyflie slide into position also if the landing is not perfect. In the center of the pads there are standard Qi-chargers from IKEA mounted to provide power.

To fly continuously, the system charging rate must be higher than what is consumed by the flying Crazyflies. With a system of nine Crazyflies that are charging through Qi-chargers it is possible to keep one Crazyflie flying, just. To get some margin we increased the charging speed a bit, the down side being that the Crazyflies get warm and the batteries ware out faster.

Collision avoidance

We use the built in collision avoidance system contributed by James Alan Preiss at University of Southern California. Thanks James, it works like a charm!

There is no planing ahead, but each Crazyflie must know where the other Crazyflies are located. Based on this information they avoid each other and chose a new path to reach their target position. For this to work each Crazyflie is continuously broadcasting its position to the other Crazyflies using the peer-to-peer framework.

Swarm control and collaboration

As mentioned earlier there is no central authority that decides which Crazyflie that should take off or go to a specific position, instead this functionality is handled in each Crazyflie. To make it possible for each Crazyflie to have a rough idea of the system state, each Crazyflie is broadcasting its position and state (landed, flying etc) to the other Crazyflies. If a Crazyflie realizes that too few drones are flying, it will simply take off to fix the problem, if it sees that too many are flying it will go back to the charging pad. To avoid that all Crazyflies takes off or lands at the same time, a randomized hold-back time is used before the actions is executed. This does not fully prevent two individuals from taking off at the same time, but makes it less likely, and eventually the correct number of drones will fly.

The number of drones that should fly at the same time is a system wide parameter that can be set from one of the peers in the system. To make sure they all agree on the value, a simple mechanism is used based on the age of the data. The value and the age of the value is included in the broadcast data. When another Crazyflie receives the data it compares the age of the received data with the age of the data it already has and replaces it only if it is younger.

Sniffer

A tenth Crazyflie is used in the demo as a sniffer. It is essentially a non-flying member of the swarm that listens to the broadcast traffic and it is used to feed data to a GUI that displays the state of the system. It can also be used to inject a new value for the desired number of flying Crazyflies.

Implementation and how to run it

The code is mainly implemented as an app in the Crazyflie firmware, using the app layer. The main part is a state machine that keeps track of what to do next with some other modules handling communication and trajectories.

The code is available in the iros-2022 branch of the crazyflie-firmware-experimental repository, in the examples/demos/decentralized_swarm folder.

The examples/demos/decentralized_swarm/src/common_files/choose_app.h file controls if the code is compiled for a swarm member or the sniffer.

All Crazyflies should have the same radio channel and the same address, except the last byte. Swarm members must use addresses ending in 01 to 09 while the sniffer must use the address ending in 00.

The demo is based on the work that Marios did for a decentralized swarm this summer. Thanks Marios!

Our Ultra Wide Band (UWB) based positioning system, the Loco Positioning System, has been around for a long time and is still going strong! In this post we will tell you a bit about how it works (for those that don’t know about it yet), what research that is on-going in the field and new developments.

Crazyflie with Loco deck

Basics

UWB is using high frequency, low power, wide band radio where one of the most important properties is that it is possible to detect when a packet is received with very high accuracy. Combining this with very high frequency clocks, opens up the possibility to measure the time it takes for a radio packet to travel from a transmitter to a receiver. Since radio waves propagates with the speed of light in air we can convert the time into distance, and this is the basic idea in UWB positioning.

Not only is it possible to measure the timing of transmissions, the packets can also contain data, like in other radio standards. This property is extensively used to include time stamps of when a packet is sent, and also for instance the time stamp of when the transmitter received other packets or the position of an anchor.

This sounds pretty straight forward, but there are (of course) some complications. We will mention some of them but not go into the details.

  • Reflections – radio waves bounce around on walls and objects. Luckily, the nature of UWB actually uses this to its advantage and works better indoors than out side.
  • The clocks in the transmitter and receiver are not synchronized – the Time Of Flight can unfortunately not simply be measured by subtracting reception time from transmission time as the time stamps originate from two different clocks. The problem can be solved by sending some more packets back and forth though.
  • Packet collisions – two transmitters can not send at the same time, one or both packets will be lost. Transmissions must be scheduled or packet loss must be handled.
  • Obstacles – obstacles between the transmitter and receiver changes the transmission time.
  • Antennas – the propagation time through the antenna is substantial and changes depending on the angle to the transmitter/receiver.
  • Radio interference – other radio sources may interfere with the UWB radio signals and add noise or packet loss.

Modes

The Loco Positioning System can run in two fundamentally different modes: Two Way Ranging (TWR) and Time Difference of Arrival (TDoA).

Two Way Ranging (TWR)

In TWR the Crazyflie measures the distance to one anchor at a time, over and over again. Each measurement in initiated by the Crazyflie and requires 4 messages to be sent between the Crazyflie and the anchor, two request-response pairs. The position is estimated by pushing the measured distances into the kalman estimator.

This mode only supports one Crazyflie, but has the advantage of being very robust and also works pretty well some distance outside the system.

Time Difference of Arrival (TDoA)

In TDoA the setup is different, the anchors are transmitting packets while the Crazyflie is passively listening to the traffic. From the received information it is unfortunately not possible to measure the distance to the anchors, but what we can get is the difference in distance to two anchors. For example, we might know that we are 0.54 meters closer to anchor 3 than anchor 6, or similar. It is possible to calculate the position from this information and similarly to TWR the measurements are pushed to the kalman estimator for further processing.

This mode supports unlimited numbers of Crazyflies (swarms) but is less robust compared to TWR, especially outside the system. TDoA is similar to how GPS works.

Research

There are many researchers that use the Loco System, some use it as a positioning system and investigate topics like path planning or similar, while some others are looking at different questions related to the UWB positioning itself. We will not try to mention everyone, we probably only know of a small fraction of what is going on (please tell us!), but would like to point out two areas of research.

The first is related to improving the estimated position by handling measurement errors and the environment in a better way. Examples of this is to compensate for differences in reception angle or handling of obstacles in the space. We would like to mention Wenda Zhao’s work at the Dynamic Systems Lab, University of Toronto. He has contributed the robust TDoA implementation in the kalman estimator (blog post) as well as a public TDoA data set.

The second is inter drone ranging, that is measuring the distance between drones as an addition to, or instead of drone-to-anchor measurements. Examples in this are are the work by Dr Feng Shan at School of Computer Science and Engineering Southeast University, China (blog post) and professor Klaus Kefferpütz, Hochschule Augsburg, work on “Crazyflie quadcopter in decentralized swarming” as presented on the BAM days last year.

Experimental functionality

Even though there has not been a lot of code committed lately in our repositories related to the Loco Positioning System, it has been simmering in the background. We would like to mention what is cooking in the pots and some of the stuff that has been discussed or tested.

System size

An 8 anchor Loco Positioning System can cover a flight space of around 8×8 meters, but from time to time we get the question of larger systems. TDoA3 was designed with this in mind and supports up to 255 anchors, which in theory would make it possible to build larger systems. This functionality was implemented 4 years ago but we never really tested it(!). Finally we collected all anchors in the lab an set up 20 anchors in the same system, and it worked! This should make it possible to extend systems to at least 15×15 meters, but maybe even more with some clever radio cell planing.

Another possibility to enlarge a system is to tweak the radio settings to make them reach longer. There is a “Longer range” mode in TDoA3 that lowers the bit rate, but again it has not really been verified. This was also tested in the latest Loco frenzy and with some minor modifications it worked the way we hoped, with 20 anchors! The tests mainly verified that the anchors play nicely together, and we are not sure about the maximum range (to be tested) but we believe distances of up to 40 meters between anchors is possible. To use this feature you should make sure to use the latest firmware for the Loco Nodes as well as the Crazyflie.

The two features mentioned above should hopefully make it possible to go big and we hope it could be used for shows for instance.

TDoA3 hybrid mode

If one looks at the messages sent in a TDoA system, the anchors are actually doing TWR with each other, while the Crazyflie(s) are just listening to the traffic and that the possibility to extract the position is a nice “side effect”. Now imagine if the Crazyflies were to send some messages from time to time, then they could act as “dynamic” anchors, or do inter-drone ranging with each other. This is something we call TDoA3 hybrid mode.

Currently there is no official implementation of the Hybrid mode, only some experimental hacks. Some researchers have done their own implementations, but we hope, at some point, to generalize the functionality and integrate it into the firmware.

Read more

If you are interested to read more about positioning and the Loco system, you can take a look at the following link list.

Summer time!

Summer is coming and with that vacations, yeay! There will always be someone at the office to help you if you need help, and we will handle shipping through out the summer, but it might take a bit longer than usual.

We hope you all have some great summer months!

We recently added improved support for assert information in the client and wanted to take this opportunity to describe some of the features in the console tab of the client that are useful for debugging and profiling.

Example of the console tab

The console tab in the python client is where you can get real time logs from the Crazyflie when connected. Any DEBUG_PRINT() statements in the Crazyflie firmware will popup here and is obviously a simple way of adding debug information to your firmware. The console logs are buffered in the Crazyflie and dumped to the client when you connect, this is why you will see the start up information when connecting to a Crazyflie. If too much information is logged and the buffer is full, you will unfortunately loose some of it but you will be notified by a “<F>” marker in the console window.

On the right side of the console tab window you will find some useful buttons, the first being the “Clear” button that simply clears the console window.

Task dump

The “Task dump” button will print a list with information about the FreeRTOS tasks running in the system, for instance something like this.

SYSLOAD: Task dump
SYSLOAD: Load	Stack left	Name
SYSLOAD: 0.19 	205 		Tmr Svc
SYSLOAD: 83.70 	127 		IDLE
SYSLOAD: 0.01 	213 		CRTP-RX
SYSLOAD: 0.0 	54 		PWRMGNT
SYSLOAD: 0.70 	131 		LH
SYSLOAD: 0.0 	117 		CRTP-SRV
...Code language: CSS (css)

The “load” column contains how much of the total time that was spent in each task, since the previous measurement (or boot). To get useful values when performing some task, you probably want to make a dump at the start of your measurement and a second one at the end to get the average during this specific time.

The “Stack left” shows how many bytes of stack that is left for each task, this is the worst recorded number in the period. Stack size is recorded at task switch time which means it is possible that more stack actually was used at some point, but it should give a good indication if a task is running out of stack.

Assert info

Next up is the new “Assert info” button, it will dump assert or crash info to the console. When the STM CPU encounters a hard fault or some other condition that resets the CPU, it will record some basic crash information in a specific part of the RAM. This special RAM is not reset when the STM re-boots and it will automatically be dumped to the console log for investigation during the start up sequence. The “Assert info” button simply dumps the same information again, which may not sound very useful. But in some cases a client may auto-reconnect to a crashed Crazyflie, consume the console log and dispose of it before a human had the opportunity to look at it. In this case you can simply connect the client to the Crazyflie and click the “Assert info” button to get the information again.

Propeller test

The “Propeller test” button runs a automated test of the propellers and measures vibrations in the platform to determine if they are well balanced or not. The result is printed in the console window, like this: (looks like it is time to change one of my propellers!)

HEALTH: Acc noise floor variance X+Y:0.004469, (Z:0.002136)
HEALTH: Motor M1 variance X+Y: 4.17 (Z:0.55), voltage sag:0.35
HEALTH: Motor M2 variance X+Y: 0.22 (Z:0.42), voltage sag:0.37
HEALTH: Motor M3 variance X+Y: 1.23 (Z:0.21), voltage sag:0.35
HEALTH: Motor M4 variance X+Y: 1.09 (Z:0.17), voltage sag:0.31
HEALTH: Propeller test on M1 [FAIL]. low: 0.0, high: 2.50, measured: 4.17
ESTKALMAN: WARNING: Kalman prediction rate low (82)Code language: CSS (css)

Battery test

The final button is the “Battery test”. It tests if the battery is worn out by spinning the motors and measuring the drop in voltage. A drop in the voltage indicates that the battery probably is bad, but it can also be caused by other sources of extra resistance in the power path, for instance oxide on the battery connector. Use it as an indication only!

Note: Only use this test for the Crazyflie 2.x, not the Bolt or BigQuad.

The result of this test is printed in the console log:

HEALTH: Idle:4.15V sag: 0.67V (< 0.95V) [OK]Code language: CSS (css)

The console side-by-side other tabs

It is possible to add the console log as a tool box at the bottom or one of the sides of the client. In the “View” menu, choose “toolboxes” and click “Console”. A toolbox window with the console log will appear at the bottom of the screen which can be handy as it will be visible even if you switch to another tab.

The Plotter tab with the console as a toolbox

Other debug tools

This post has been focused on the console tab, but there are of course other functionality that is useful when debugging your system. We will end by quickly mention some of them:

There has been some background work going on related to the Lighthouse system, as mentioned in a previous blogpost. The solution has been improved since that blog post and we believe the functionality is now on a level where it works pretty well and can add value to most Lighthouse users.

How to use it?

We have added a brief documentation to get you started. Though the solution has been stabilized, it is still a bit experimental and it has not been fully integrated into the client yet. The base station geometry estimator still has to be run as a python script from the command line, and a reconfigured version of the Crazyflie firmware has to be built and flashed.

We have added some improvements to the client thought to enable it to display base station status for 2+ base stations. This was the final part of the client UI that did not support 2+ base stations, and now remains only the possibility to run the new geometry estimation from the client.

Benefits

What kind of improvements does it bring?

First of all, the functionality to use more than 2 base stations and the possibility to cover a larger flight space. It also makes it possible to set up multi-room systems to support flight from one room to another.

Secondly an improved estimation of the base station geometry (also when using 2 base stations) that generally reduces the errors and improves the position estimation of the Crazyflie when flying. “Jumping” of the estimated position when one base station is occluded should be reduced. When following a trajectory that is straight line through space, the Crazyflie should now actually fly on a fairly straight line, previously the flown path might be a bit curved.

The new solution has a better match to the physical world and hopefully the estimated Z will be closer to zero when the Crazyflie is on the floor, with the “old” method, the solution sometimes is slightly tilted with a Z != 0 in some areas.

Problems

Most of the Lighthouse system works just like before, the new functionality is related to base station geometry estimation. The “standard” geometry estimation is still available in the client and if you continue to use this nothing is changed, the following list is for the new estimation method.

  • The new geometry estimation is a bit clunky to use and the user still has to rebuild the firmware and run a python script.
  • Lighthouse 1 is not fully supported
  • The new geometry estimation does not work with one base station.

We hope to address the above problems in future releases.

Release

Talking about releases, we are working on a new official release. If no unforeseen obstacles are found, we plan to make a new release within a week or two.

The functionality discussed in this blog post is still only in source code, on master or possibly in some pull requests. If you wait for the release all repositories should be syncronized and make it a bit easier to try out.

Feedback

As the environment of the system has an impact on this type of functionality, we would love to get feedback from you if you try it out. We’d love to hear how it works for you!

The Toolbelt has been around for a pretty long time but we have not been that good at promoting it and documentation is unfortunately a bit sparse. In this blog post we will talk a bit about the Toolbelt and how to use it.

The basic idea behind the Toolbelt is to provide an easy to access tool that helps the user to do common tasks in Bitcraze projects, without installing a lot of special tool-chains, libs or programs. The intention is also to harmonize the use in all our projects to make them as similar as possible and reduce the cognitive load when switching between repositories.

A functional view

After a standard installation (see below), the Toolbelt is available using the tb command and it is intended to be executed from the root of (almost) any Bitcraze repository file tree. You can run tools (commands) from the toolbelt with the extra spice that they run in the required environment, for instance when building the firmware the correct compiler is automatically available.

Without any arguments the Toolbelt will display a brief help. For instance if I run it in the root of the crazyflie-firmware repository I get this:

kristoffer@kristoffer-XPS-13-9310:~/code/bitcraze/crazyflie-firmware$ tb
Usage:  tb [-d] tool [arguments]
The toolbelt is used to develop, test and build Bitcraze modules. When the toolbelt is called, it will first try to find the tool in the belt, after that it will try the tools in the module if the working directory is the root of a module. Module tools are executed in the context of a docker container based on the module requirements configured in the module.json config file.

-d:  print the docker call that executes the tool

Tools in the belt:
  help, -h, --help - Help
  update - Update tool belt to latest version
  version, -V, --version - Display version of the tool belt
  ghrn - Generate release notes from github milestone
  docs - Serve docs locally

Tools in the current module:
  build
  test
  compile
  check_elf
  make
  test_python
  clean
  build-docs
Code language: JavaScript (javascript)

We can see that there are two groups of tools; “Tools in the belt” that are available in all repositories and ” Tools in the current module”, these are tools that are specific to the current repository.

To run a tool, simply use tb and the command. To build the firmware for instance, you can use make

kristoffer@kristoffer-XPS-13-9310:~/code/bitcraze/crazyflie-firmware$ tb make
Running script tools/build/make in a container based on the bitcraze/builder docker image as uid 1000
Using default tag: latest
latest: Pulling from bitcraze/builder
Digest: sha256:bee591d94db757465b88338c69be847cdf527698f0270ea1a86a2ccaa3c9845d
Status: Image is up to date for bitcraze/builder:latest
docker.io/bitcraze/builder:latest
make: Entering directory '/module'
  CLEAN_VERSION
  CC    stm32f4xx_dma.o
  CC    stm32f4xx_exti.o
  CC    stm32f4xx_flash.o
  CC    stm32f4xx_gpio.o
...Code language: JavaScript (javascript)

We can see that the firmware is built but we did not have to set our environment beforehand like instructed in the build/install page. The Toolbelt handled that by providing an precompiled development environment to do the firmware-compiling for us.

I will not go through all the tools but I’d like to mention the docs command. The docs command starts a web server and renders a simplified version of the documentation for a repository (in the docs directory). It is useful when browsing or editing the documentation.

Implementation

The Toolbelt is based on Docker and it runs in container. When executing a tool, the Toolbelt starts a second container where the tool runs. This second container is called a builder and it contains all the software required to execute the tool. There are a few different builders with tool-chains that are appropriate for various languages, CPUs and so on, luckily the Toolbelt picks the correct one automatically.

The directory that the tool is executed from is mapped into the docker containers and this is how the tools access the files, for instance when compiling.

In the example above we can see that the Toolbelt is pulling the latest version of the bitcraze/builder image from docker hub to have the latest and greatest builder when running make. It will take a while to download the builder image the first time (or when it has been updated) but usually this is not necessary and usually starting a tool takes only around 1 second.

The builder images are also used by our build servers for CI and release builds, this means that building with the Toolbelt replicates the exact same environment as on our builder servers.

The tools that are specific to a repository can be found in the tools/build and tools/build-docs directories. They are usually bash or python scripts and often they can also be executed without the toolbelt if you have the appropriate software installed on your system.

The source code for the Toolbelt is available on github. You can also find the source code for the builders on github, search for “builder”

Installation

The Toolbelt is mainly designed for Linux like environments and works on MacOS and in WSL (Windows Subsystem for Linux). Some operations are slowish on Mac as file access is a bit slower from docker containers.

To run the Toolbelt you need to have Docker installed on your system, after that installation is as simple as adding an alias to your .bashrc (or similar). For instructions run:

docker run --rm -it bitcraze/toolbelt

Native installation VS Toolbelt VS Virtual machine

There are three paths for building and working with Bitcraze source code; native install, the Toolbelt and the VM (Virtual machine). They all have their pros and cons.

Native installation

All build tools installed on the machine.

Pros: fast, access to USB and Crazyradio which enables flashing of firmware, can use your standard development environment

Cons: Possible compatibility issues with other software on the system. Must maintain installation and upgrade from time to time.

Toolbelt

Pros: highly separated from the OS, automatically updated with the appropriate tools and versions

Cons: can not access USB and the Crazyradio – flashing not possible. No access to GUIs – can not run the client

VM

Pros: Everything ready in one place, also supports USB, Crazyradio and flashing. Client works. Highly separated from the OS.

Cons: A bit bulky

Conclusions

The Toolbelt is an option for users that are interested in working with the source code for the Bitcraze ecosystem, but do not want to put too much time into installing tool-chains and setting up environments. It does not solve all problems but hopefully simplifies some tasks.

Any feedback is welcome!

Base station geometry estimation is a function in the python client (in the lighthouse tab), where the system estimates the position and orientation of the base stations. The user places the Crazyflie on the floor (in the desired origin) and clicks a button to measure the angles to the base stations, which are used to estimate the geometry. The current implementation is fairly basic and has some issues associated with it:

  1. All base stations must be received from the point where the Crazyflie is located
  2. Only 2 base stations are supported
  3. The coordinate system is not properly aligned with the room
  4. The generated geometry is not as good as it could be, that is the position/orientation is sub-optimal
  5. The code has a dependency to OpenCV which causes problems for ROS users

I have been working on a solution for these problems as my fun Friday project and in this blog post I will tell you a bit more about the problems and a possible solution.

Screenshot from the client of a geometry with 4 base stations.

What are the problems to be solved?

In the current implementation, the user places the Crazyflie in the origin, with the front of the Crazyflie pointing in the direction of the positive X-axis. When the user hits the “Estimate Geometry” button, the angles to the visible base stations are recorded and the solvePnP() function in OpenCV is used to estimate their poses (position and orientation). This is all fine but it also has its limitations and in the following section we will outline what the limitations are and how to solve them.

All base stations must be received in the origin and only 2 base stations are supported

Currently the Crazyflie ecosystem supports up to 2 base stations and this works fine for a flight space of around 4×4 meters. With more base stations it would be possible to cover larger areas or multiple rooms, which is a feature that many users have been asking for. In these scenarios it will not be possible to receive all base stations from one position any more though, and it will require a new method for geometry estimation using multiple measurements. Suppose base station 1 and 2 are received in one position and 2 and 3 in another, then we can map the measurements together since we know base station 2 must have the same pose in both measurements. This way it is possible to relate all base station poses to each other, provided there are measurements that link them together.

The coordinate system is not properly aligned with the room

When generating the geometry in the current implementation, the orientation of the Lighthouse deck is used to define the coordinate system: forward of the deck defines the X-axis, left defines the Y-axis and up the Z-axis. The problem is that the deck might not be perfectly aligned with the Crazyflie, the floor might not be completely flat or the Crazyflie might not point exactly in the desired direction. A pretty small misalignment will result in fairly large errors a couple of meters away, resulting in unexpected behavior, for instance not flying at constant height. Expanding to more base stations and larger systems, the problem will become even bigger and a better solution is clearly needed.

If we used the position of the Crazyflie when placed at multiple positions, we could use this information to rotate the coordinate system to be better aligned. For instance, suppose we measured some points on the floor of the flight space, then we could make sure the XY-plane of the coordinate system goes through those points, or at least as close as possible. Similarly one or more measurements along the X-axis would help to define the rotation around the Z-axis.

The generated geometry is not as good as it could be

The lighthouse positioning system is based on measuring the angles between the sensors on the Lighthouse deck and the base stations. One can think of it as four beams or rays, going from each base station to the sensors on the deck, for which we measure the direction very precisely seen from the base station’s point of view. The purpose of the geometry estimation is to figure out the position and orientation of the base stations so that we can calculate how the beams are oriented in the flight space instead. By looking at where the beams from two base stations intersect we know where the sensors are located and can calculate the position of the Crazyflie. This is a somewhat simplified picture of how it works but it is sufficient for the following discussion.

So what happens if the geometry is not completely correct? If the estimated positions or orientations of the base stations are slightly off, the beams will not intersect and we have to use some method to find the point closest to the two beams instead to use as the sensor position. In a real world system there will always be errors and the implementation must be able to handle them, but we want to keep them as small as possible. Further more we want to make sure the errors are uniformly distributed in the flight space so that we get equally good results everywhere.

In the current estimation process, where we take a measurement in one position, we are able to generate a geometry that is good at that point, but due to noise in the measurements and other subtleties the error at the edges of the flight space might be several centimeters.

The solution to this problem is to measure the angles in multiple positions and try to find a geometry where the error is equally small for all of them. It does not guarantee that the error will be equal everywhere, but if we make measurements in the volume we plan to fly in we know it will be OK where we need it to be. It should also be a much better geometry, for the full covered volume, than what we can be achieved by measuring in one point only.

One bonus problem that hopefully will be solved by this approach is the moving back and forth that sometimes can be seen in a Lighthouse 2 system. What happens is that the base stations interfere with each other from time to time (by design) and most of the time the Crazyflie gets positioning information from both base stations, but every couple of seconds only from one of them. When both are available the “average” position is used, but when only one is received, the Crazyflie will “jump” to the position indicated by that base station (the simplified model from above with crossing beams does not hold in this case, sorry!). If the difference between the suggested positions of the two base stations (the error in the geometry) is large there will be a noticeable motion in the Crazyflie.

The code has a dependency to Open CV

In the current solution we use the solvePnP() function in Open CV to estimate the geometry. Open CV is an awesome library but unfortunately it has turned out that this dependency interferes with ROS, and since a fair amount of our users also use ROS, we would like to get rid of it if possible.

Luckily I found an open source implementation of IPPE, an algorithm that finds the pose of an object based on points seen by a camera, that we can use instead. There is actually an option to use Ippe in OpenCV’s solvePnP(), but we used another flavor.

The solution

The core idea is to first collect measurements of beams in many positions in the flight space by moving a Crazyflie around and record the lighthouse angles. Secondly an equation system is created that takes the poses of the base stations and all the recorded Crazyflie poses as input and as output calculates the lighthouse angles those poses would correspond to for all the sensors. Finally the output is compared to the recorded values and poses are adjusted using the least-square solver in scipy to find the poses that minimizes the difference between the measurements and the output from the equation system.

Before we can solve the equation system we have to record the angles from the base stations. There is a handy function in the Crazyflie that pushes measured lighthouse angles to the PC via the radio, and by letting the user move the Crazyflie around in space we get the angles along that path. What we are looking for though are angles collected in discrete positions and as an approximation I group measurements together based on time. The assumption is that if two angle measurements are closer than 10 ms in time, the Crazyflie did not move very far and they can be considered to be taken in the same position. The output of this process is a list of samples where each sample contains the measured lighthouse angles of one or more base stations for one specific Crazyflie pose. After this has been done, the list is filtered to only contain samples with two or more base stations.

We also need an initial guess of the base station and Crazyflie poses for the least-square solver to make the solution converge. I use IPPE for this and use the first sample as the reference to define a temporary global reference frame. Suppose the first sample contains angles for base stations 2 and 3, we can then use IPPE to calculate an estimate of the pose of the two base stations, in the Crazyflie reference frame of this sample. Since we use the first sample as the reference for the global reference frame (that is the pose of the Crazyflie in this sample is the origin by definition), those poses are also equal to the base station poses in the global reference frame.

Suppose the next sample contains lighthouse angles for base stations 1 and 2, using IPPE we can estimate the base station poses for base stations 1 and 2 in the reference frame of the Crazyflie in this sample. Since the relative positions of the base stations is the same, regardless of reference frame, we can rotate/translate the poses of the base stations so that base station 2 pose matches the pose of base stations 2 in the first sample. We now have an estimate of the poses of base stations 1, 2 and 3, further more the transformation used represents the pose of the Crazyflie in sample 2. Repeating the process for all samples gives us a pretty good idea of where all the base stations are located as well as the pose of the Crazyflie in all the samples.

We can now feed the initial guess and the equation system into scipy and hopefully get a refined solution back. From the estimated poses of the base stations and Crazyflie samples it is possible to calculate the distance between sensors and beams which gives us an approximation of how good the solution is.

The final step is to align the coordinate system with the room, as mentioned earlier the solution we have this far is based on the pose of the Crazyflie in the first sample. The way it is done in the suggested implementation is to ask the user to place the Crazyflie at some points in the desired origin, on the positive X-axis and in the XY-plane and measure the angles in these positions. The measurements are included as samples in the above process which means we will get the estimated positions as a part of the over all solution, in the temporary global reference frame. The task at hand is then to find the rotation/translation from the temporary global reference frame to the one indicated by the positions sampled by the user. Again we do a least-square optimization to find the transformation that minimizes the error in the sampled points. We can now calculate the final solution by applying the transformation to the base station poses we got earlier.

Does it work?

Yes, it seems to work pretty well, I have not had the time to do extensive testing yet but the results looks promising. In our flight arena with 4 base stations, the solution seems to generally be acceptable. We don’t know the exact poses of our base stations since it is very hard to measure, but they are mounted in the same truss and should be at similar heights.

Base stations at:
1: (-3.7104629351065146, -0.27330674065567867, 2.960720481536423)
2: (-0.9233909349006646, -2.9651389799486356, 2.9781503155699176)
3: (-0.12450705551081238, 3.430497907723026, 3.011201684709142)
4: (2.74012584124908, 0.5856524795079388, 3.023133069381165)
Solution match per base station:
1: {'mean_error': 0.0026020322270174697, 'max_error': 0.013310934923630531, 'std_error': 0.0028768923969783836}
2: {'mean_error': 0.0015240237742724164, 'max_error': 0.005526851773945277, 'std_error': 0.0011560341160273498}
3: {'mean_error': 0.002193101969828834, 'max_error': 0.006778096051979129, 'std_error': 0.0015768914826109067}
4: {'mean_error': 0.0033752667182490796, 'max_error': 0.014997173956894249, 'std_error': 0.00354931189334688}

The above snippet is part of the output from one run and as can be seen the estimated height is between 2.96 and 3.02 m. You can also see that the estimated average error for sensor positions is in the order of 2-3 mm while the maximum error is 1.5 cm.

Below is graph of the recorded Crazyflie positions in the final solutions. Note the three single points at the bottom that are from the origin, the X-axis and XY-plane.

Estimated positions of the Crazyflie

I did some testing on larger systems with 6-8 base stations this Friday and it seemed to be harder to get a solution that converges which indicates that there might be something to look into here.

Try it out

This is still work in progress, but if you want to try it out, you can find the code in this pull request. Run the examples/lighthouse/bs_geometry_estimation.py script, you will get instructions on the screen as you go.

Officially the firmware supports 2 base stations , but most of the code is designed to handle up to 16 and if you want to test the functionality with more than two base stations you have to update PULSE_PROCESSOR_N_BASE_STATIONS and re-flash your Crazyflie.

Any feedback is welcome, please use the pull request.

Sometimes we get the question of where to modify or add code to change some behavior of the Crazyflie. There is no quick answer to this question but we thought that we should write a post to clear up some question marks and give a better idea of how to approach the problem.

There are quite a few repositories on the Bitcraze github page but there are two that are the main focal point for almost any Crazyflie work, those are the crazyflie-firmware and the crazyflie-lib-python. The crazyflie-firmware contains the source code (written in C) for the firmware that runs in the Crazyflie, that is the code responsible for flying, blinking LEDs, communicating with the radio, scanning sensors and so on. The crazyflie-lib-python (often called the python lib) on the other hand is running on the PC side and is the API to use to communicate with the Crazyflie from a script. The crazyflie-lib-python is also used by the python client which means that anything you see in the client can be done by a script using the python lib.

Let’s assume we have a system of one Crazyflie connected to a computer using a Crazyradio. Now we want to control the Crazyflie and make it take off for instance, how should this be done?

The easiest way would be to use the python lib. The python lib is used to communicate with the Crazyflie and we can use it to send instructions to the Crazyflie, for instance to take off or fly a trajectory. It is also possible to use the parameter framework to change values in the Crazyflie. The main way of monitoring what is going on in the Crazyflie is to use the log framework to read variables from the Crazyflie. The python lib is perfect for controlling the Crazyflie or prototype ideas as it is very fast to make changes and try things out. The best to get started with the python lib is to start from an example that already uses functionalities you want to use.

Another option is to add code in the firmware. Originally this has been quite hard since the firmware has not been initially designed to accept user code. This means that unless you want to modify an already existing code, it is quite hard to find where to add your code so that it runs in the Crazyflie firmware and you would have to make a fork of the firmware which can he hard to maintain and keep up to date in the long run. This is one of the things the out-of-tree build and the app layer is solving, it is now quite easy to add and run your own C files to the firmware in your own project without having to fork the Crazyflie firmware. There is a bunch of examples in the firmware that shows how to implement autonomous behavior as an app. The easiest is to start with the hello world example. When it comes to modifying exiting functionality in the firmware code, most of the time forking and modifying the official firmware unfortunately is the only solution. We are however working our way to make more and more of the firmware modular so that it can be expanded out of tree. For example there has been work to make an out of tree estimator possible to implement.

A prototype written as a python script is often pretty easy to move to the Crazyflie firmware. This is a good pattern when writing an application, rapid prototyping in python and then finalizing in firmware if needed. The best example of that is the push demo. It is a demo where the Crazyflie can be pushed-around with the help of the flow deck for autonomous flight and the multiranger deck for detecting obstacle/hands. We have a python cflib push demo as well as a Crazyflie firmware push demo app.

There is some support in the python lib for interacting with multiple Crazyflies and it is probably a good start point for simple swarms. For more advanced swarm work Crazyswarm may be a better option.

If you would like to see some of the process in action, we have made a workshop during our BAM days about implementing functionality both using the python lib and in the firmware as an out of tree app:

We’re happy to announce the availability of the 2021.01 release! The release includes the Crazyflie firmware (2021.01), the python library (0.1.13.1) and the python client (2021.1). The firmware package can be downloaded from the Crazyflie release repository (2021.01) or can be flashed directly using the client bootloader window.

Most of the improvements have been done in the Crazyflie firmware and include:

The App API in the Crazyflie firmware has been extended and improved to be able to handle a wider range of applications. The goal is to enable a majority of users to implement the functionality they need in an app instead of hacking into the firmware it self.

We have improved the Lighthouse support in the firmware and both V1 and V2 are now working well. Even-though everything is not finished yet, we have taken a good step towards official Lighthouse positioning.

A collision avoidance module has kindly been contributed by the Crazyswarm team.

A persistant storage module has been added to enable data to be persisted and available after the Crazyflie is power cycled. It will initially be used to store Lighthouse system information, but will be useful for many other tasks in the future.

Basic arming functionality has been added for platforms using brushless motors.

In the client the LPS tab now has a 3D visualization of the positioning system and a new tab has been added to show the python log output.

Unfortunately we have run into some problems for the Windows client build which is not available for this release.

Finally we have fixed bugs and worked to improve the general stability.

We hope you enjoy it!