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) controller
Code 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!

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.

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.

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:

• The log and param framework with the Plotter tab and parameter tab in the client
• Wireshark
• Hardware debuggers

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!