AdaCore Blog

How to prevent drone crashes using SPARK

How to prevent drone crashes using SPARK

by Anthony Leonardo Gracio

Introduction

I recently joined AdaCore as a Software Engineer intern. The subject of this internship is to rewrite a drone firmware written in C into SPARK.

Some of you may be drone addicts and already know the Crazyflie, a tiny drone whose first version has been released by Bitcraze company in 2013. But for all of those who don’t know anything about this project, or about drones in general, let’s do a brief presentation.

The Crazyflie is a very small quadcopter sold as an open source development platform: both electronic schematics and source code are directly available on their GitHub and its architecture is very flexible. These two particularities allow the owner to add new features in an easy way. Moreover, a wiki and a forum have been made for this purpose, making emerge a little but very enthusiastic Crazyflie community!

Now that we know a little more about the Crazyflie, let me do a brief presentation of SPARK and show you the advantages of using it for drone-related software.

Even if the Crazyflie flies out of the box, it has not been developed with safety in mind: in case of crash, its size, its weight and its plastic propellers won’t hurt anyone!

But what if the propellers were made of carbon fiber, and shaped like razor blades to increase the drone’s performance? In theses circumstances, a bug in the flight control system could lead to dramatic events.

SPARK is an Ada subset used for high reliability software. SPARK allows proving absence of runtime errors (overflows, reading of uninitialized variables...) and specification compliance of your program by using functional contracts.

The advantages of SPARK for drone-related software are obvious: by using SPARK, you can ensure that no runtime errors can occur when the drone is flying. Then, if the drone crashes, you can only blame the pilot!

After this little overview, let’s see how we can use SPARK on this kind of code.

Interfacing SPARK with C

Being an Ada subset, SPARK comes with the same facilities as Ada when it comes to interfacing it with other languages. This allowed me to focus on the most error-sensitive code (ex: stabilization system code), prove it in SPARK, let the less sensitive or proven-by-use code in C (ex: drivers), and mix SPARK code with the C one to produce the final executable.

Let’s see how it works. The Crazyflie needs to receive the commands given by the pilot. These commands are retrieved from a controller (Xbox, PS3, or via the Android/iOS app) and are sent via Wi-Fi to the Crazyflie. This code is not related with the stabilization system and works great: for now, we just want to call this C code from our stabilization system code written in SPARK.

Here is the C procedure that interests us. It retrieves the desired angles, given by the pilot via his controller, for each axis (Roll, Pitch and Yaw).

void commanderGetRPY
   (float* eulerRollDesired, 
    float* eulerPitchDesired, 
    float* eulerYawDesired);

And here is the Ada procedure declaration that imports this C function.

procedure Commander_Get_RPY_Wrapper
   (Euler_Roll_Desired  : in out Float;
    Euler_Pitch_Desired : in out Float;
    Euler_Yaw_Desired   : in out Float);
pragma Import (C, Commander_Get_RPY_Wrapper, "commanderGetRPY");

Now we can use this function to get the commands and give them as inputs to our SPARK stabilization system!

Helping SPARK: constrain your types and subtypes!

Ada is well known for its features concerning types, which allow the programmer to define ranges over discrete or floating-point types. This specificity of Ada is very useful when it comes to prove absence of overflow and constraint errors using SPARK: indeed, when all the values used in the program’s computations are known to be in a certain range, it becomes easy to determine if these computations will cause a runtime error!

Let’s see how it works in practice. The Crazyflie comes with a PC client used to control and track the drone’s movements. A lot of physical values can be seen in real-time via the PC client, including the drone’s acceleration magnitude.

To calculate this magnitude, we use the accelerometer measurements given by the IMU (Inertial Measurement Unit) soldered on the Crazyflie.

Let’s see how the magnitude was calculated in the original C code. The accelerometer measurements are hold in a structure containing 3 float fields, one for each axis:

typedef struct {
   float x;
   float y;
   float z;
} Axis3f;

static Axis3f acc;

In the stabilization loop, we get the fresh accelerometer, gyro and magnetometer measurements by calling a function from the IMU driver. Basically, this function simply reads the values from the IMU chip and filters the possible hardware errors:

imu9Read(gyro, acc, mag);

Now that we have the current accelerometer measurements, let’s calculate its magnitude:

accMAG = (acc.x*acc.x) + (acc.y*acc.y) + (acc.z*acc.z);

The type of each ‘acc’ field is a simple C ‘float’. This means that, for instance, ‘acc.x’ can possibly be equal to FLT_MAX, causing an obvious overflow at runtime… Without knowing anything about the return values of imu9Read, we can’t prove that no overflow can occur here.

In SPARK, you can easily prove this type of computations by constraining your ADA types/subtypes. For this case, we can create a float subtype 'T_Acc' for the IMU acceleration measurements. Looking in the Crazyflie IMU documentation, we discover that the IMU accelerometer measurements are included in [-16, 16], in G:

--  Type for acceleration output from accelerometer, in G
subtype T_Acc  is Float range -16.0 .. 16.0;

type Accelerometer_Data is record
   X : T_Acc;
   Y : T_Acc;
   Z : T_Acc;
end record;

Now that we have constrained ranges for our accelerometer measurements, the acceleration magnitude computation code is easily provable by SPARK!

Constrained subtypes can also be useful for cascaded calculations (i.e: when the result of a calculation is an operand for the next calculation). Indeed, SPARK checks each operand type's range in priority in order to prove that there is no overflow or constraint error over a calculation. Thus, giving a constrained subtype (even if the subtype has no particular meaning!) for each variable storing an intermediate result facilitates the proof.

Ensuring absence of constraint errors using saturation

We have seen that defining constrained types and subtypes helps a lot when it comes to prove that no overflow can occur over calculations. But this technique can lead to difficulties for proving the absence of constraint errors. By using saturation, we can ensure that the result of some calculation will not be outside of the variable type range.

In my code, saturation is used for two distinct cases:

  • Independently from SPARK, when we want to ensure that a particular value stays in a semantically correct range
  • Directly related to SPARK, due to its current limitations regarding floating-point types

Let's see a code example for each case and explain how does it help SPARK. For instance, motor commands can't exceed a certain value: beyond this limit, the motors can be damaged. The problem is that the motor commands are deduced from the cascaded PID system and other calculations, making it difficult to ensure that these commands stay in a reasonable range. Using saturation here ensures that motors won't be damaged and helps SPARK to prove that the motor commands will fit in their destination variable range.

First, we need the function that saturates the motor power. Here is its defintion: it takes a signed 32-bit integer as input and retrieves an unsigned 16-bit integer to fit with the motors drivers.


--  Limit the given thrust to the maximum thrust supported by the motors.
   function Limit_Thrust (Value : T_Int32) return T_Uint16 is
      Res : T_Uint16;
   begin
      if Value > T_Int32 (T_Uint16'Last) then
         Res := T_Uint16'Last;
      elsif Value < 0 then
         Res := 0;
      else
         pragma Assert (Value <= T_Int32 (T_Uint16'Last));
         Res := T_Uint16 (Value);
      end if;

      return Res;
   end Limit_Thrust;

Then, we use it in the calculations to get the power for each motor, which is deduced from the PID outputs for each angle (Pitch, Roll and Yaw) and the thrust given by the pilot:

procedure Stabilizer_Distribute_Power
     (Thrust : T_Uint16;
      Roll   : T_Int16;
      Pitch  : T_Int16;
      Yaw    : T_Int16) 
   is
      T : T_Int32 := T_Int32 (Thrust);
      R : T_Int32 := T_Int32 (Roll);
      P : T_Int32 := T_Int32 (Pitch);
      Y : T_Int32 := T_Int32 (Yaw);
   begin
       R := R / 2;
       P := P / 2;

      Motor_Power_M1 := Limit_Thrust (T - R + P + Y);
      Motor_Power_M2 := Limit_Thrust (T - R - P - Y);
      Motor_Power_M3 := Limit_Thrust (T + R - P + Y);
      Motor_Power_M4 := Limit_Thrust (T + R + P - Y);

      Motor_Set_Ratio (MOTOR_M1, Motor_Power_M1);
      Motor_Set_Ratio (MOTOR_M2, Motor_Power_M2);
      Motor_Set_Ratio (MOTOR_M3, Motor_Power_M3);
      Motor_Set_Ratio (MOTOR_M4, Motor_Power_M4);
  end Stabilizer_Distribute_Power;

That's all! We can see that saturation here, in addition to ensure that the motor power is not too high for the motors, ensures also that the result of these calculations will fit in the 'Motor_Power_MX' variables.

Let's switch to the other case now. We want to log the drone's altitude so that the the pilot can have a better feedback. To get the altitude, we use a barometer which isn't very precise. To avoid big differences between two samplings, we make a centroid calculation between the previous calculated altitude and the raw one given by the barometer. Here is the code:

subtype T_Altitude is Float range -8000.0 .. 8000.0;  -- Deduced from the barometer documentation

   --  Saturate a Float value within a given range
     function Saturate
        (Value     : Float;
         Min_Value : Float;
         Max_Value : Float) return Float 
     is
     (if Value < Min_Value then
         Min_Value
      elsif Value > Max_Value then
         Max_Value
      else
         Value);
     pragma Inline (Saturate);

      --  Other stuff...
      
      Asl_Alpha            : T_Alpha       := 0.92;  --  Short term smoothing
      Asl                  : T_Altitude    := 0.0;
      Asl_Raw              : T_Altitude    := 0.0;
 
      --  Other stuff...

      --  Get barometer altitude estimations
      LPS25h_Get_Data (Pressure, Temperature, Asl_Raw, LPS25H_Data_Valid);

      if LPS25H_Data_Valid then
         Asl := Saturate 
                      (Value          => Asl * Asl_Alpha + Asl_Raw * (1.0 - Asl_Alpha),
                       Min_Value  => T_Altitude'First,
                       Max_Value =>T_Altitude'Last);
      end if;

Theoretically, we don't need this saturation here: the two involved variables are in T_Altitude'Range by definition, and Asl_Alpha is strictly inferior to one. Mathematically, the calculation is sure to fit in T_Altitude'Range. But SPARK has difficulties to prove this type of calculations over floating-point types. That's why we can help SPARK using saturation: by using the 'Saturate' expression function, SPARK knows exactly what will be the 'Saturate' result range, making it able to prove that this result will fit in the 'Asl' destination variable.

Using State Abstraction to improve the code readability

The stabilization system code of the Crazyflie contains a lot of global variables: IMU outputs, desired angles given by the pilot for each axis, altitude, vertical speed… These variables are declared as global so that the log subsystem can easily access them and give a proper feedback to the pilot.

The high number of global variables used for stabilization lead to never-ending and unreadable contracts when specifying data dependencies for SPARK. As a reminder, data dependencies are used to specify what global variables a subprogram can be read and/or written. A simple solution for this problem is to use state abstraction: state abstraction allows the developer to map a group of global variables to an ‘abstract state’, a symbol that can be accessed from the package where it was created but also by other packages.

Here is a procedure declaration specifying data dependencies without the use of state abstraction:

--  Update the Attitude PIDs
   procedure Stabilizer_Update_Attitude
     with
       Global => (Input  => (Euler_Roll_Desired,
                             Euler_Pitch_Desired,
                             Euler_Yaw_Desired,
                             Gyro,
                             Acc,
                             V_Acc_Deadband,
                             V_Speed_Limit),
                  Output => (Euler_Roll_Actual,
                             Euler_Pitch_Actual,
                             Euler_Yaw_Actual,
                             Roll_Rate_Desired,
                             Pitch_Rate_Desired,
                             Yaw_Rate_Desired,
                             Acc_WZ,
                             Acc_MAG),
                  In_Out => (V_Speed,
                             Attitude_PIDs));

We can see that these variables can be grouped. For instance, the 'Euler_Roll_Desired', 'Euler_Pitch_Desired' and 'Euler_Yaw_Desired' refer to the desired angles given by the pilot: we can create an abstract state Desired_Angles to refer them in our contracts in a more readable way. Applying the same reasoning for the other variables referenced in the contract, we can now have a much more concise specification for our data dependencies:

procedure Stabilizer_Update_Attitude
     with
       Global => (Input  => (Desired_Angles,
                             IMU_Outputs,
                             V_Speed_Parameters),
                  Output => (Actual_Angles,
                             Desired_Rates),
                  In_Out => (SensFusion6_State,
                             V_Speed_Variables,
                             Attitude_PIDs));

Combining generics and constrained types/subtypes

SPARK deals very well with Ada generics. Combining it with constrained types and subtypes can be very useful to prove absence of runtime errors on general algorithms that can have any kind of inputs.

Like many control systems, the stabilization system of the Crazyflie uses a cascaded PID control, involving two kinds of PID:

  • Attitude PIDs, using desired angles as inputs and outputting a desired rate
  • Rate PIDs, using attitude PIDs outputs as inputs

In the original C code, the C base float type was used to represent both angles and rates and the PID related code was also implemented using floats, allowing the developer to give angles or rates as inputs to the PID algorithm.

In SPARK, things are more complicated: PID functions do some calculations over the inputs, like calculating the error between the measured angle and the desired one. We’ve seen that, without any information about input ranges, it’s very difficult to prove the absence of runtime errors on calculations, even over a basic one like an error calculation (Error = Desired – Measured). In other words, we can’t implement a general PID controller using the Ada Float type if we intend to prove it with SPARK.

The good practice here is to use Ada generics: by creating a generic PID package using input, output and PID coefficient ranges as parameters, SPARK will analyze each instance of the package with all information needed to prove the calculations done inside the PID functions.

Conclusion

Thanks to SPARK 2014 and these tips and tricks, the Crazyflie stabilization system is proved to be free of runtime errors! SPARK also helped me to discover little bugs in the original firmware, one of which directly related with overflows. It has been corrected by the Bitcraze team with this commit since then.

All the source code can be found on my GitHub.

Since Ada allows an easy integration with C, the Crazyflie currently flies with its rewritten stabilization system in SPARK on top of FreeRTOS! The next step for my internship is to rewrite the whole firmware in Ada, by removing the FreeRTOS dependencies and use Ravenscar instead. All the drivers will be rewritten in Ada too.

I will certainly write a blog post about it so see you for the next episode :)

Posted in #UAVs    #crazyflie    #SPARK    #Drones   

About Anthony Leonardo Gracio

Anthony  Leonardo Gracio

Anthony Leonardo Gracio is a software engineer at AdaCore. He joined AdaCore in 2015 after he got an engineering degree at EPITA, an engineering school in Paris. He is currently working in the GPS (GNAT Programming Studio) team.