Calibration Procedure of the PX4 Firmware - Part I

In my previous article what_is_going_on_behind_the_calibration_procedure_of_qgroundcontrol, I found out that the QGroundControl software only provides an interface of the calibration of PIXHAWK sensors. The actual calibration procedures are carried out on board. In this article, we'll continue to find out what the calibration procedures are.

Find Out the Directory of the Calibration Code in the PX4 Firmware

This time we'll turn to the source code of the PX4 firmware. First clone the git repository at https://github.com/PX4/Firmware. We need to find where the mavlink message MAV_CMD_PREFLIGHT_CALIBRATION goes in the firmware (Note this is the message sent by the startCalibration function of the QGroundControl software, as described in my previous article). I am not using an IDE this time, so in order to find the message, the following bash command was executed.

Note I used the number 241, instead of the name of the constant MAV_CMD_PREFLIGHT_CALIBRATION, because that name is defined in the QGroundControl source code, not in the PX4 firmware. I actually tried that name as the search keyword, but found no luck. In the generated file abc, I searched for 241 and found the following definition in ./src/modules/uORB/topics/vehicle_command.h as

#define VEHICLE_CMD_PREFLIGHT_CALIBRATION 241

and I searched the constant VEHICLE_CMD_PREFLIGHT_CALIBRATION again with the above bash command. In the file ./src/modules/commander/commander.cpp, I found the following code

Several functions emerged here, e.g. do_gyro_calibration(mavlink_fd), do_mag_calibration(mavlink_fd), do_accel_calibration(mavlink_fd), do_level_calibration(mavlink_fd). As the names suggest, they might contain the code for the calibration. Those functions were found in folder ./src/modules/commander/. For the accelerometer, the file accelerometer_calibration.cpp contains its calibration-related functions.

The Calibration Procedure of the Accelerometer

The following is the comment from the beginning of the file accelerometer_calibration.cpp

Now, let's take a closer look at the detailed calibration procedure. By the way, calibration of the accelerometer aims to obtain a transformation from the acceleration vector measured in the sensor's coordinate system to the acceleration vector in the vehicle's body coordinate system. The sensor's bias is also calibrated. The following is from the comment of this file, which gives a fairly detailed description of this procedure.

The calibration procedure above is clearly a standard textbook one. The following is a description of the procedure in mathematical equations.

apart from the final Rotation part. Since the calibration procedure will itself obtain a rotation matrix to account for the misalignment of the sensor's axes and the vehicle's body axes, so why there is another rotation procedure stated here? We'll solve this problem first.

The Rotation Myth

I searched the keyword “rotate” in the main calibration function do_accel_calibration() and found the following two lines

A constant SENS_BOARD_ROT comes out, what is this? It turned out to be a constant defined in ./src/modules/sensors/sensor_params.c as

PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);

Apparently it does not refer to the misalignment of the sensor's coordinate system and the vehicle's since the misalignment is the result of the calibration. Anyone who has done the calibration procedure in QGroundControl will remember there is a drop-down listbox asking the user to choose the orientation of the vehicle, as in Fig. 2.

Figure 2. The dropdown listbox which asks for the orientation of the vehicle in the sensors config panel of QGroundControl.

So could this drop-down listbox relate to the SENS_BOARD_ROT constant? We'll have to go back to the QGroundControl's code to check. The string in the box “ROTATION_NONE” could be a clue.

It is a property defined in src/AutoPilotPlugins/PX4/SensorsComponent.qml as

seems to get this parameter from somewhere. We know that PIXHAWK stored a bunch of variables in the flash memory or EEPROM. Could the above line be getting the values from the flash memory? I found the following parameters stored in the flash memory of PIXHAWK

It can be seen that there is a direct correspondence between the definition of SENS_BOARD_ROT and the rotations property, so we can confidently say that the SENS_BOARD_ROT variable in the PX4 program corresponds to this bit of the settings in QGroundControl.

Note: src/AutoPilotPlugins/PX4/ParameterFactMetaData.xml file is a pretty good one, since it gives a clear description to all the variables stored in the flash memory.

Now, we come back to the PX4 code, two problems remain: first, how the calibration is actually done; second, how are the calibration results stored?

Basically, what the comments say is that the sensor model is assumed to be linear, as in the two models above. The two models are equivalent. The rotational matrix rot is known from the predetermined SENS_BOARD_ROT variable, and the remaining unknowns are accel_T and accel_offs, which are exactly the same as the ones in the unrotated calibration version.
it accounts for the rotations defined in the constant “SENS_BOARD_ROT”.
Now, let's have a look at the code implementation of the algorithm.
The calibration is carried out in this function

which I guess carries out the data collection task and the task of calculating the averages of the data, which is done by function accel_calibration_worker(), this function is passed as a handle to the previous function.

In the last part of this function, the core calibration algorithm is done through the following function

function param_get() stores an integer corresponding to the rotation configuration into board_rotation_int, then the corresponding matrix is obtained by function get_rot_matrix() and stored in board_rotation, which is a matrix object.
The rotated offset vector and misalignment matrix are computed by the following operations.