Implementation of spin stop effect based on Crazyflie and TOF sensor

Infineon / Mitsubishi / Fuji / Semikron / Eupec / IXYS

Implementation of spin stop effect based on Crazyflie and TOF sensor

Posted Date: 2024-01-26

Today, the editor brings to you a project by Singapore's Maker Chathuranga Liyanage to use TOF sensors to achieve a high degree of self-sustainment for Crazyflie.

Device List



Bitcraze Crazyradio PA

Z-ranger deck


AdaCore GNAT Community

Drones fly with multiple sensors, but none of them can identify obstacles at close range or actual distance to the ground. This is why drones need ranging sensors. They provide accurate distance readings to the nearest object in the way. Additionally, these are useful for mapping purposes, allowing path planning and autonomous navigation.

I've been using Crazyflie for a few years on my college projects, with stock firmware. After discovering the Certyflie firmware implemented in Ada, I started testing it due to its simplicity. Since there are not a lot of files, it was easy for me to get familiar with the firmware in a short time and implement my own functions and libraries. Here I plan to explain the following topics.

Get a rough idea of ​​the main features of Certyflie firmware.

Integrated ToF sensor to measure height

Achieve autonomous take-off sequence

Use ToF sensor to implement height holding function.

Describes how to implement a ToF sensor deck to map the environment.

Let's start setting the context for these.

set background

These are useful to me without causing too much trouble to start programming CF using Ada.

Operating System - Windows 8.1

GNAT version - 2018 arm elf

Firmware - Certyflie (ravenscar-cf-stable)

To set the compiler path, open a command prompt, navigate to the root directory of the cloned repository, and type the following command. We need to set the path to the GNAT bin folder. Change the path according to your installation directory.

path C:GNAT2018in;%path%

To upload firmware to CF we need DFU-util. Simply, we can install it on Windows using CLI installer. After installation, you can check the installation by typing at the command prompt. It should return the installed version. dfu-util -l

The Readme section of the Certyflie repository provides clear instructions for uploading compiled files. Since Windows doesn't have a sudo command, make sure to remove that part from the last command.

dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D obj/cflie.bin


The CF platform comes with several removable sensor extensions to extend its functionality. The Z ranger deck has a time-of-flight sensor, which is a distance measuring sensor that helps the drone maintain a constant altitude above the ground.

Z ranger is equipped with VL53L0X sensor, with a maximum sensing distance of 2m. The library for this sensor is contained in the cloned repository within CertyflieAda_Drivers_Librarycomponentssrc ange_sensor. However, a new version of this library is available at In this implementation, I replace the existing library with a new library.

To define the sensor object and set the I2C port, add the library to using the following command:

with VL53L0X; use VL53L0X;

and add the following lines.

Z_Ranger_Device : VL53L0X_Ranging_Sensor(I2C_EXT_Port'Access, Ravenscar_Time.Delays);

After powering up the CF, it initializes all sensors and components. This procedure is available within internal procedures. To add our sensors to this initialization process, we first import VL53L0X, STM32.board, and STM32. I2C library. Then we add the following part during the initialization process.


Initialize_I2C_GPIO (STM32.I2C.I2C_Port (Z_Ranger_Device.Port.all)); Configure_I2C (STM32.I2C.I2C_Port (Z_Ranger_Device.Port.all)); Set_Device_Address (Z_Ranger_Device, 16#52#, Status); Data_Init (Z_Ranger_Device, Status) ; Static_Init (Z_Ranger_Device, New_Sample_Ready, Status); Perform_Ref_Calibration (Z_Ranger_Device, Status); Set_VCSEL_Pulse_Period_Pre_Range (Z_Ranger_Device, 18, Status); Set_VCSEL_Pulse_Period_Final_Range (Z_Ranger_Device, 14, Status); Start_Continuous (Z_Ranger _Device, 0,Status);

After uploading, you can power on the CF and check the ToF sensor with your phone camera to check if it is working properly. If the sensor has successfully initialized and started working, you will notice that the purple light turns on.

PS - Here we use optical flow deck, not Z ranger deck. So don't worry if your Z Ranger deck looks different. The pmw3901 sensor was damaged due to some serious crashes while flying the drone.

add to log

CF has a python library to perform various tasks. Using the original firmware we can communicate with the drone using the laptop via the CF radio module. Retrieving data logs is one of the most important functions of this library. It provides access to sensor readings, drone status, battery level and many other parameters. Fortunately, the Certyflie firmware has a limited number of data logs and can be used with the CF python library.

Open in the examples folder. This code prints real-time roll, pitch, and yaw values ​​while running. lg_stab.add_variable('stabilizer.roll', 'float') lg_stab.add_variable('stabilizer.pitch', 'float') lg_stab.add_variable('stabilizer.yaw','float')

In the first line, "stabilizer" is the log group, "roll" is the parameter, and "float" is the data type, which is the same data type that CF records for that particular parameter. In you can find the procedures for which all log groups have been defined. For example, below shows how to initialize the yaw log.

stabilizer.adb,Stabilizer_Init Log.Add_Log_Variable (Group => "stabilizer", Name => "yaw", Log_Type => Log.LOG_FLOAT, Variable => Euler_Yaw_Actual'Address, Success => Dummy)

When you run the python code, it connects to CF and starts printing values. Change the drone's orientation and see how the values ​​change.

Now, let's add the ToF sensor measurements to the log. This way we have the opportunity to see how this value changes when we change the altitude of the drone.

First, we need to get measurements from the sensor when it is available. After initialization, the system is in. This will update the drone's status parameters. We've added a height measurement section to this feature. Reception distance is in millimeters. Here we convert it to m.

Stabilizer_Update_Attitudestabilizer.adb if Range_Value_Available (Z_Ranger_Device) then Z_Height := 0.001 * Float (Read_Range_Millimeters (Z_Ranger_Device)); end if;

I added a separate state variable group called "Range_Measurements" to hold this variable. Then, add the following log to the function.

Stabilizer_Init Log.Add_Log_Variable (Group => "range", Name => "z_range", Log_Type => Log.LOG_FLOAT, Variable => Z_Height'Address, Success => Dummy);

To view these values ​​via PC, you can add the following lines in your python script.


Implement height maintenance function

Certyflie firmware already has altitude hold functionality. It uses a predefined thrust force as the base value. You can find this value in . I assume this value is almost equal to the weight of the CF without the additional sensor deck. ALT_HOLD_THRUST_F : constant := 32_767.0;

Since we now have a way to measure the distance above the ground with a resolution of 1mm, we can use the z-measurement to implement a separate height-holding function. To do this we need to find a way to set the thrust value using our own function.

Provide thrust, roll, pitch and yaw values ​​to the stabilizer function to run the motors to fly the drone. If we look closely, we can determine that the drone is activated via two methods.


orders from pilot

When free fall is detected

Pilot commands are sent using the CrazyFlie mobile app. You can connect via Bluetooth and fly the drone using the virtual joystick in the mobile app. These commands are captured by the drone as CRTP packets, and the messages are decoded to obtain the relevant parameters.

Detect free fall using the Z acceleration value measured by the IMU. Once free fall is detected, the drone begins generating thrust to recover from the crash and runs a cycle to reduce thrust to lose altitude. The starting thrust and falloff values ​​can be found in You can reduce the decrement value to get a smooth landing.

MAX_RECOVERY_THRUST : constant T_Uint16 := 48_000; RECOVERY_THRUST_DECREMENT : constant T_Uint16 := 100;

By mimicking any of these functions, we can pass the thrust value to make our own altitude holding function. But I prefer to imitate the pilot command in my implementation.

First, I commented out the Commander_Get_RPY, FF_Get_Recovery_Commands, Commander_Get_Thrust, and FF_Get_Recovery_Thrust function calls. I then implemented a separate function called "Autonomous_Sequence" in the commander file that turns on the motors and runs the PID algorithm to maintain the height at the desired level. Add the following section to ensure the drone is in a level position before running this function.

Stabilizer_Control_Loop if abs (Euler_Pitch_Actual)

The PID variable is defined in

If you've ever adjusted a drone's PID, you probably know that it's a very cumbersome process. Not having a precise horizontal position controller makes this more difficult, as the drone starts to drift sideways. To avoid this, I tied two ropes to the drone from the left and right sides. This allows the drone to move within a limited area while we adjust the PID value.

Below is an initial test of the altitude hold feature. This function runs for 8 seconds. This time can be increased by increasing the loop count. The required height is set to 0.1 meters.

While tuning the PID, I decided to upgrade the motor for a higher payload capacity, to connect more sensors and include a larger battery. The CF motor driver's N-channel MOSFET has a maximum current rating of 6A, giving us more room to increase motor size. As shown in the cover image, the existing 0716 coreless motor is replaced by the 0720 motor.

The following video shows the altitude hold function using a new motor and a further tuned PID. In this experiment, the required height is 0.8m. (Kp - 2000, Kd - 0, Ki - 500). Ropes were tied to two chairs to gain a higher position.

VL35LOX and VL35L1X

The VL53L1X has a maximum sensing distance of 4m, while the VL53L0X we used in this project has a maximum range of 2m. As shown in the image above, the VL53L1X has a larger lens than other sensors.

The Z Ranger deck V2 is equipped with this new sensor. After browsing both datasheets, I noticed that the VL53L1X has the same default I2C address (0x29), which is also the address of the VL53L0X. Additionally, most important registers have the same address in both sensors. Therefore, the VL53L0X library in Ada can be used with the VL53L1X to run basic functions, which are sufficient for our needs.

Use multiple TOF sensors

It is clear that we need to install more sensors on drones to cover the surrounding environment. But the problem is how we can communicate with multiple sensors with the same I2C address.

The STM32 chip on CF mainly has 2 I2C ports. But one port is only for internal sensors such as IMU. Other ports are connected to expansion pins for communication with the sensor card. Therefore, we need to find a way to communicate with multiple ToF sensors using this I2C port.

Both ToF sensor models have the ability to change their I2C address. When we write a new address to the sensor, it is stored in volatile memory. Therefore, we need to connect the sensors one by one every time we open the device and write the I2C address. To avoid this process, ToF sensors come with an extra pin called XSHUT.

To turn on the sensor we need to pull up this pin. In the Z ranger deck, this pin is permanently connected to Vcc via a resistor. The solution is to write a program that holds the XSHUT pin low for all other sensors except the z ranger, writes a different I2C address, sets one XSHUT pin high, writes a different I2C address ,So on and so forth. This seems like a good approach until you have enough GPIO pins to connect all the ToF sensors.

To overcome the lack of GPIO pins, the CF Multiranger deck with 5 ToF sensors is equipped with an 8-bit GPIO expansion IC, PCA9534. The chip communicates with the drone through different I2C addresses. When we write a value to its registry, it sets its 8 GPIO pins high and low based on the given value. This way we have the opportunity to control all ToF sensors only through the I2C port. Therefore, the same process for the XSHUT pin described above can be used without any difficulty.

The da library comes with several IO expander libraries. But unfortunately it doesn't have the library to configure the chip PCA9534. Currently, I'm implementing a library for this chip. The beta library is located in the Ada libraries folder of the add-on repository, but it requires more work to implement all the features available. Again, I plan to update the repository as implementation progresses.

After achieving this result, we can implement obstacle avoidance using all distance measurements.

Review Editor: Liu Qing

#Implementation #spin #stop #effect #based #Crazyflie #TOF #sensor