Autonomous Crazyflie with Optical flow and ToF sensors

Introduction

A drone has multiple sensors which make it fly. But none of them could identify a close obstacle or the actual distance from the ground. This is why a drone needs distance measuring sensors. They provide an accurate distance reading to the closest object on their way. Further, these are useful for mapping purposes which allows path planning and autonomous navigation.

I have been using Crazyflie for several years for my university projects and working with the stock firmware. After finding out the Certyflie firmware implemented with Ada, I started testing it due to its simplicity. Not having a large number of files made it easy to get familiar with the firmware within a short time and implement my own functions and libraries. Here I'm planning to explain the following topics.

• A rough go through on the main functions of Certyflie firmware.
• Integrating a ToF sensor to measure the height
• Implementing an autonomous take off sequence
• Implementing an altitude hold function with the ToF sensor.
• An introduction to implementing a ToF sensor deck to map the environment.

Let's get started with setting up the background for these.

Setting up the background

These are the things that worked for me without giving much trouble to start programming the CF with Ada.

• OS - Windows 8.1
• GNAT Version - 2018 arm elf
• Firmware - Certyflie (ravenscar-cf-stable)

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

path C:\GNAT\2018\bin;%path%

To upload the firmware to CF, we need DFU-util. Simply we can use the CLI Installer to install it on windows. After installing you can type dfu-util -l in the command prompt to check the installation. It should return the installed version.

The Readme section of the Certyflie repo has provided clear instructions about uploading the compiled file. Since Windows doesn't have sudo commands make sure to remove that part in the last command.

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

Adding Z Ranger Deck

CF platform comes with several detachable sensor decks to expand its' capabilities. Z ranger deck has a Time of Flight sensor which is a distance measuring sensor to help the drone to maintain a constant height from the ground.

The Z ranger comes with the VL53L0X sensor which has a maximum sensing distance of 2m. The library for this sensor is included in the cloned repo inside Certyflie\Ada_Drivers_Library\components\src\range_sensor. However, a new version of this library is available in https://github.com/AdaCore/Ada_Drivers_Library. In this implementation, I replaced the existing library with the new one.

To define the sensor object and to set the I2C port, add the library to stm32-board.ads with,

with VL53L0X;        use VL53L0X;

and add the following line.

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

After powering up the CF, it initializes all the sensors and components. This process is available in the crazyflie_system.adb inside System_Init procedure. To add our sensor to this initializing process first we import the VL53L0X, STM32.board and STM32.I2C libraries. Then we add the following part inside the initializing procedure.

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 this, you can power up the CF and check the ToF sensor with the mobile phone camera to check whether it's working. You will notice a purple light if the sensor has successfully initialized and start working.

P.S. - Here we use the Optical Flow deck, not the Z ranger deck. So don't worry if your Z ranger deck looks different. The pmw3901 sensor is broken due to some serious crashes that happened while flying the drone.

Adding to logs

CF has a python library to perform various tasks. With the original firmware, we can communicate with the drone using a laptop through the CF radio module. Retrieving data logs is one of the most important functions of this library. It gives access to sensor readings, drone state, battery level and many other parameters. Fortunately, Certyflie firmware has a limited number of data logs and it works with the CF python library.

Open basiclogSync.py inside the examples folder. This code prints live roll, pitch and yaw values when 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 the CF logs that particular parameter. In the stabilizer.adb, you can find the Stabilizer_Init procedure, where all the log groups have defined. For example, the following shows how the yaw log is initialized.

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 will connect to the CF and start print values. Change the orientation of the drone and see how the values are changing.

Now let's add our ToF sensor measurements to logs. In that way we get the chance to see how the value changes when we change the altitude of the drone.

First we need to get measurements from the sensor when they are available. After initializing, the system runs the Stabilizer_Update_Attitude function in the stabilizer.adb. This updates the state parameters of the drone. We add our height measuring part inside this function. The received distance is in mm. Here we convert it into m.

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

I have added a separate state variable group called "Range_Measurements" to keep this variable. Then the following log is added to the Stabilizer_Init function.

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

To view these values through the PC, you can add the following line to the python script.

lg_stab.add_variable('stabilizer.yaw','float')

Implementing the Altitude Hold Function

Certyflie firmware already has an altitude hold function. It uses a predefined thrust as the base value. You can find this value inside commander.ads. I assume this value is nearly equal to the weight of CF without additional sensor decks.

ALT_HOLD_THRUST_F : constant := 32_767.0;

Since now we have a method to measure the distance from the ground with a resolution of 1mm, we can implement a separate altitude hold function using z measurements. For this, we need to find out a method to set the thrust value using our own function.

The Stabilizer_Control_Loop gives thrust, roll, pitch and yaw values to the stabilizer function to run motors to make the drone fly. If observe carefully, we could identify that the drone gets activated by two methods.

• From pilot commands
• When a free fall detected

Pilot commands are sent using the CrazyFlie mobile app. You can connect with Bluetooth and fly the drone using virtual joysticks in the mobile app. These commands are captured by the drone as CRTP packets and decode the message to get relevant parameters.

The free fall is detected using the Z acceleration value measured with the IMU. Once a free fall is detected, the drone starts generating thrust to recover from crashing and runs a loop to decrease the thrust to lower the altitude. The starting thrust and the decrement value can be found inside free_fall.ads. You can reduce the decremental value to get a smooth landing.

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

By imitating either of these functions we can pass thrust values to make our own altitude hold function. But I prefer imitating pilot commands for my implementation.

First I commented out Commander_Get_RPY, FF_Get_Recovery_Commands, Commander_Get_Thrust and FF_Get_Recovery_Thrust function calls inside the Stabilizer_Control_Loop. Then I implemented a separate function called "Autonomous_Sequence" inside the commander file, which turns on motors and runs a PID algorithm to maintain the height at the desired level. The following part is added to make sure that the drone is at a leveled position before running this function.

if abs (Euler_Pitch_Actual) < 2.0 and abs (Euler_Roll_Desired) < 2.0 then
         Activate_Autopilot := True;
end if;

The PID variables are defined inside commander.ads.

If you have ever tuned PIDs of a drone, you might know that it is a really troublesome process. Not having a precise horizontal position controller makes this even harder since the drone starts to drift sideways. To avoid this, I have attached two strings to the drone from the left and right sides. This allows the drone to move within a limited area while we tune the PID values.

Following is the initial test of the altitude hold function. The function runs for 8 seconds. This time can be increase by increasing the loop count. The desired height was set to 0.1 meters.

While tuning the PID I decided to upgrade the motors to get a higher payload capacity to attach more sensors and include a bigger battery. The N Channel Mosfet of the CF motor driver has a maximum current rating of 6A giving us more space to increase the motor size. Existing 0716 coreless motors were replaced with 0720 motors as shown in the cover image.

The following video shows the altitude holding function with new motors and further tuned PIDs. The desired height was 0.8m in this experiment. (Kp - 2000, Kd - 0, Ki - 500). The strings were attached to two chairs to get an elevated position.

VL53L0X and VL53L1X

The VL53L1X comes with a maximum sensing range of 4m while the maximum range of the VL53L0X which we used in this project, is 2m. As you can see in the above image, the VL53L1X has larger lenses than the other sensor.

Z Ranger deck V2 comes with this new sensor. After going through both datasheets, I noticed that the VL53L1X comes with the same default I2C address (0x29) which is the address of the VL53L0X too. Further, most of the important registers have the same addresses in both sensors. Therefore, the VL53L0X library in Ada can be used with the VL53L1X to run basic functions which are more than enough to cover our requirements.

Using Multiple ToF sensors

It is obvious that we need more sensors on the drone to cover the surrounding environment. But the problem is how can we communicate with several sensors with the same I2C address.

The STM32 chip on CF mainly has 2 I2C ports. But one port is used only for internal sensors such as the IMU. Other port is connected to the expansion pins to communicate with sensor decks. So we need to figure out a method 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 the new address to the sensor, it is stored in a volatile memory. Therefore, we need to connect sensors one by one every time we turn on the device and write I2C addresses. To avoid this process, ToF sensor comes 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 tied to Vcc permanently through a resistor. The solution is to write a program to keep the XSHUT pins low in all other sensors except the z ranger, write a different I2C address, set high one XSHUT pin, write a different I2C address and so on. This looks like a good method until you have enough GPIO pins to connect with all the ToF sensors.

To overcome the lack of GPIO pins, the CF Multiranger deck which has 5 ToF sensors, comes with an 8 bit GPIO expansion IC, PCA9534. This chip communicates with the drone through a different I2C address. When we write values to its' registries it sets its' 8 GPIO pins high and low according to the given value. In this way, we get the chance to control all the ToF sensors only with the I2C port. So, the same procedure with XSHUT pins mentioned above can be used without any difficulties.

The Ada library comes with several IO expander libraries. But unfortunately, it doesn't have the library to configure PCA9534 chip. Currently, I'm in the process of implementing a library for this chip. A beta library is available in the Ada library folder in the attached repo but it needs a lot more work to implement all the available functionalities. Again, I'm planning to update the repo as I make progress with the implementation.

After this implementation, we can implement an obstacle avoiding function using all the distance measurements.

Further assistance you may contact Professional Electronic Designing Service Firms to support with your developments.