Compass calibration progress with geodesic sections in Ardupilot

Posted by Gustavo Sousa on Fri 27 May 2016

Ardupilot's compass calibration and the completion_mask

The Ardupilot flight stack has an embedded compass calibrator, which has the advantage of providing a consistent behavior across different GCSs. Another good point of the internal calibrator when comparing to the algorithms implemented in the popular GCSs is that it makes sure the samples collected are well distributed over a sphere in order to get good calibration parameters.

The calibrator can be triggered and stopped with MAVLink commands. MAVProxy is an example of a GCS that uses the internal calibration and the source code can be checked in the mavproxy_calibration.py module. During the calibration, Ardupilot keeps sending MAVLink messages informing the progress of the calibration. The calibration message is called MAG_CAL_PROGRESS 1 and two of its fields are completion_pct and completion_mask. The former is the overall percentage of the calibration completion and the latter is is a bitmask representing sections of a geodesic grid.

The goal of the completion_mask is to provide a picture on how the calibration is progressing with respect to the distribution of the samples collected and a way for the GCSs to somehow guide the user when she is performing the compass calibration, which would make the process faster than just letting the user rotate the vehicle randomly until the calibration is completed.

Geodesic grid implemented in Ardupilot

The internal calibration has a sample acceptance criteria that rejects samples that are too close to the ones already collected. So there's a higher probability of getting more samples accepted when the vehicle is rotated so that the magnetometer field hits geodesic sections not visited yet. That could be done with the help of the completion_mask and other parameters like the current magnetometer field and attitude.

While the completion_pct field was supported from the beginning, the completion_mask wasn't implemented until a few days ago, when a patch set of mine got applied into Ardupilot's master branch. Those patches introduce a new C++ class called AP_GeodesicGrid with only one static public function to return the section index for a given vector and uses that to implement the completion_mask field.

The completion_mask is an array of 10 uint8_t values, so that each bit represents one of 80 sections of the geodesic grid. Given the \(i\)-th uint8_t and its \(j\)-th bit2, the section index for that bit is \(8i + j\). During the calibration, as the sections get hit by the samples, their corresponding bit are set. The geodesic grid is abstracted as a tessellation of a regular icosahedron, centered at the origin, that divides each triangle into four and projects them to the circumscribed sphere, giving a total of 80 sections - one triangle on the sphere for each bit of the completion mask.

A calibration sample is a tridimensional vector representing a magnetometer reading, thus it comprises the \(x\), \(y\) and \(z\) components of the magnetic field. A way to think of a sample "hitting" a geodesic section is by imagining a ray3 that starts from the origin and has the same direction of the sample vector crossing the section's triangle. In other words, a calibration sample \(\vec{v}\) hits a section \(s\) if \(\exists \lambda > 0, \lambda \vec{v} \in A(s)\), where \(A(s)\) is the set of points of the area of the triangle of \(s\) and \(\lambda\) is a real number.

Details of the section search algorithm can be checked in AP_GeodesicGrid.cpp (or maybe in a possibly upcoming post :-)). There's a little Python script called geodesic_grid.py that was added with the patch to serve as a tool to aid development and understanding of the concepts used in the calibration. The image4 below is the result of calling libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py --icosahedron -b.

Tessellated Icosahedron

The tessellated icosahedron.

It's important to remember that the completion_mask is populated while the calibration is running, so some inaccuracy is expected in function of the calibration parameters (radius and offsets) that have an initial guess and get adjusted during the calibration. It's important to bear that in mind when implementing support for internal calibration in your GCS.

mag(i)cal_graph

Another applied patch set was one that added a local MAVProxy module in Ardupilot called magcal_graph.py. It basically plots the geodesic grid and highlights the sections that are hit while the calibration is running. The plot isn't very helpful for most of the end users - it was actually meant to serve as a reference on how to interpret the bit mask.

It's necessary to have Tools/mavproxy_modules/ in the PYTHONPATH environment variable in order to be able to use that module. With that done, using that module is as simple as using those commands in the MAVProxy shell:

module load magcal_graph
magcal_graph
magcal start

The command magcal start is a built-in MAVProxy command that sends to the vehicle the MAVLink command to begin the internal calibration.

Simulation

Since it would be cumbersome to have to use a vehicle for testing the calibration support development on the GCSs, the Ardupilot development team had the idea to provide a model in SITL to simulate calibration. That model allows the user to set a desired attitude or angular velocity to the vehicle in order to actuate on the vehicle while running the calibration. Together with that simulation model, I also added a local MAVProxy module called sitl_calibration.py to provide an interface to that model and also to automate the actuation necessary for the calibration processes (the currently supported ones are compass and accelerometer calibration).

The developer can either use the commands sitl_attitude and sitl_angvel to respectively set the vehicle at a desired attitude and to apply a desired angular velocity, or use the command sitl_magcal. The latter automates the actuation in order to complete the compass calibration. The command sitl_attitude receives three parameters, which are roll, pitch and yaw degree angles, respectively. The command sitl_angvel applies an angular velocity, which is defined by a rotation axis and a non-negative angular speed in degrees. The first three parameters are floating point numbers as the \(x\), \(y\) and \(z\) components of the rotation axis vector, respectively, and the last one is the angular speed.

There are also the commands sitl_accelcal, for the accelerometer calibration; and the sitl_stop, to stop any other of that module's commands.

Demo

I recorded a little demo on how to use the magcal_graph module together with sitl_calibration. There it is:

  1. The messages for the internal calibration are currently specific to Ardupilot, so they won't be found in the common message set specification. Refer to ardupilotmega.xml instead, which is the Ardupilot-specific message set specification.
  2. Both \(i\) and \(j\) start from zero and the first bit is the LSB.
  3. A ray can be thought of as a line segment with just one endpoint.
  4. Unfortunately, matplotlib has some problems rendering multiple objects in 3D and that's why the numbers seem messed up. Probably choosing another tool for rendering that would be a good idea.