Implement new controller for autonomous flight

[alexandraH99]

Hi, I'm trying to implement a brand new controller on the crazyflie so that it can fly autonomously. Basically I'd like the controller to use position, linear velocity, angular velocity, and quaternion as its state, and generate required thrust for each motor as command. However, I'm constantly getting lost due to the complexity of firmware. I wonder whether you have any suggestions on which files to modify and where to start.

I've noticed that with the PID and INDI controller, position and attitude controller are implemented in different files. I'd like to implement the computation for everything in just one file, so I'm assuming I can follow the structure of Mellinger controller, not sure if this is right though.

In addition, I'm using the Loco Positioning System for positioning data, which is also part of the controller's state, how should I integrate the output of LPS with the controller?

Thanks!

[krichardsson]

Hi!

Controllers are plugins (sort of) and should be pretty straight forward to add.

Controllers are registered in https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/controller.c#L24-L29
As you can see you have to implement 3 functions (init, test and update) that you pass on to the system in the registration.
Add a new line in the registration after the INDI controller and add your functions here.

Add a new identifier in https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/interface/controller.h#L31-L37 (after the ControllerTypeINDI but before ControllerType_COUNT)

Add a new c-file for your controller and add it to the Kbuild file https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/Kbuild#L10-L13.

You can start from the mellinger controller if you like or just make a new file, the only thing that is important is that you implement the three functions.

Your init function should initialize data. It will be called when the controller is activated.
Your test function should just return true.

Your update function will be called from the stabilizer loop (at 1000 Hz) and the task is essentially to fill in the values in the control struct (roll, pitch, yaw and thrust). You get at bunch of information in the other structs that you can use to understand the state of the world.

In addition, I'm using the Loco Positioning System for positioning data, which is also part of the controller's state, how should I integrate the output of LPS with the controller?

In the standard implementation, the Loco data is fed to the estimator and is fused with other sensor data there. You will get the information implicitly in the controller through the state struct. If this is not working for you, you will have to invent a new way to access that information. How do you want to use the loco data in the controller?

[krichardsson]

My controller basically does some computation over position, angular velocity, linear velocity, and quaternion of the crazyflie to generate required thrust for each motor so that it can hover in the air

This sounds like what the code is designed to do, this should be possible.

that's why I want to use the loco data in the controller. I wonder if this is practical at all.

If the state estimate is not enough for what you want to do, and actually need the raw loco data I think it will become a bit messy. The Loco data is pushed to the estimator in a queue and you would have to figure out a way to store the information somewhere else to make it available in the controller. I'm not sure what data you are interested, you will either get a bunch of samples with the distances to anchors (in TWR mode) or differences in distances to anchor pairs (in TDoA). If you want the actual position it is available in the state.

the difficulty here is that the "state" I need is different from the state generated in the built-in estimator

What would you need in the state?

Also, my controller heavily relies on matrix computation, but the crazyflie firmware does not seem to include any linear algebra libraries. If an external library is used, there might be some cross compiling issues. What is the easiest way you would suggest to do matrix computation in an autonomous embedded flight system for the crazyflie?

There is support for optimized matrix operations in the CMSIS library (vendor/CMSIS/CMSIS/DSP/Source/MatrixFunctions). The most common functions are also wrapped in cf_math.h

[krichardsson]

The system design, from sensor to control is documented here https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/sensor-to-control/

[alexandraH99]

Hi, thank you so much for your reply.

I need position, linear velocity, angular velocity, and quaternion for the state, whereas the built in estimator uses attitude, position, velocity, and acceleration I think?

[krichardsson]

If I get this correctly you are missing the angular velocity. One option could maybe be to add a member for it in the state_t struct, and populate the field in the estimator. The state struct is filled in by the estimator, for the kalman estimator here and here and if you are using the complimentary estimator here

[alexandraH99]

Thank you so much!

[Carmol1]

Hi.

First of all, an apology for using this issue, but I am also looking for how to implement my own control law only that I am using optitrck cameras, so I would like to know, what lines of code do I have to use to program a new controller?, given that as mentioned, it is hard to understand the code in general.

I'd appreciate your help.

[knmcguire]

hi @Carmol1, technically kristoffer already provide all the instructions for that in this discussions, but I see that we still have your original question open here: https://github.com/orgs/bitcraze/discussions/64. So let's discuss there further.

[knmcguire]

Also, these are instructions for an onboard controller, but you are wondering how to do this offboard of the computer right @Carmol1 ?

[Carmol1]

Hi.

Very well, in relation with the second post, I am interesting in implement my control law in both cases, but for the moment I want to program an onboard controller.