Oh, time flies. It’s already week 8 and we have zero weekly driver posts out there – don’t worry
though because there’s plenty of drivers and `embedded-hal`

implementations in the works.

To play catch up in this post I’ll cover two `embedded-hal`

drivers: the `l3gd20`

and the
`lsm303dlhc`

. The L3GD20 is an IC that contains a gyroscope and exposes I2C and SPI interfaces;
the LSM303DLHC is an IC that contains an accelerometer and a magnetometer, and exposes an I2C
interface. You can find these two ICs on the STM32F3DISCOVERY board.

Gyroscope, accelerometer and magnetometer – all these are motion sensors. On their own they aren’t
*that* useful because each one has some sort of weakness but when you put them together you can
build some nifty stuff.

# L3GD20

Even though the L3GD20 has I2C and SPI interfaces the `l3gd20`

driver only lets you interface the
L3GD20 using SPI. Coincidentally, on the STM32F3DISCOVERY board the L3GD20 is connected to the SPI
bus of the STM32F303 microcontroller.

The L3GD20 contains a gyroscope, but what’s a gyroscope useful for?

## Gyroscope

A gyroscope is a sensor that measures the angular rate exerted on it. Angular rate is basically the speed at which something is rotating, and it’s measured in degrees per second or in radians per second. Gyroscopes like the L3GD20 measure a 3D angular rate but report it as angular rates across three orthogonal axes. See picture below:

So the L3GD20 will report an angular rate across its X axis, another one across its Y axis and yet another across its Z axis.

Going back to the `l3gd20`

driver. Once you create an instance of the driver you can read the sensor
using the blocking `gyro()`

method. The method returns an `I16x3`

value which contains the readings
of the sensor along its X, Y and Z axes.

```
let mut l3gd20 = L3gd20::new(spi, nss)?;
let I16x3 { x, y, z } = l3gd20.gyro()?;
```

Each reading is a 16 bit integer that represents an angular rate. To map this integer to degrees
per second you need to multiply it by the sensitivity of the sensor. The L3GD20 defaults to a
sensitivity of `8.75e-3 dps / LSB`

(dps = degrees per second; LSB = Least Significant Bit); this
maps the 16 bit integer to a range of about `[-250, 250]`

degrees per second.

If we collect data from the gyroscope while keeping the F3 board still we’ll see something like this:

`AR_x`

stands for Angular Rate across the X axis; `|AR|`

is the magnitude of the 3D angular rate.
The blue line is sensor data collected during the span of one second and the green line is the mean
of the data.

### Calibration

There’s a problem with this data though: it says that the mean angular rate is not zero, which
implies that the L3GD20 and the F3 board to which is attached are rotating. We know that was not the
case: the board was kept still while measuring so the all the readings *should* be centered around
zero but instead they are *offset* by some value. This offset is known as *bias* and it’s commonly
found on gyroscopes and other kind of sensors.

To correct this bias error we have to calibrate the gyroscope. But turns out we kind of already did: the mean values we computed in the previous graph are the biases of the gyroscope. The only thing that’s left is to subtract the bias from each axis measurements.

Now these are more correct measurements!

We can calibrate the gyroscope for bias while the gyroscope / board is not moving so we can do that each time the system is initialized if we know that the board is still during initialization. The problem is that bias doesn’t remain constant in time; it tends to drift with both the passage of time and with changes in temperature so the calibration will eventually become invalid. There are methods to compensate for bias drift but I won’t cover them here.

Let’s move on to the next IC.

# LSM303DLHC

This IC contains an accelerometer and a magnetometer. Let’s look at each of them in detail.

## Accelerometer

Accelerometers are sensors that measure *proper* acceleration. In simple terms, the difference
between proper acceleration and the *coordinate* acceleration you are familiar with is that proper
acceleration *includes* the acceleration of gravity. This means that even if an accelerometer is not
moving it will sense the acceleration of the gravity.

Like the gyroscope in the L3GD20, the accelerometer in the LSM303DLHC measures a 3D acceleration vector but it reports the decomposition of the 3D vector along three orthogonal axes.

Using the `lsm303dlhc`

driver is similar to using the `l3gd20`

driver. Once you instantiate the
driver you can read the accelerometer data using the blocking `accel()`

method.

```
let mut lsm303dlhc = Lsm303dlhc::new(i2c)?;
let I16x3 { x, y, z } = lsm303dlhc.accel()?;
```

Again, you get a 16 bit integer for the reading along each axis. The default sensitivity of the
accelerometer is around `6.1e-5 g / LSB`

(g is the acceleration of gravity); this maps the integer
to a range of `[-2, 2]`

g.

If we collect data from the accelerometer while keeping the F3 board still on a horizontal surface we’ll see something like this:

`G_x`

stands for acceleration of Gravity across the X axis; `|G|`

is the magnitude of the
acceleration of Gravity. The blue line is sensor data collected during the span of one second and
the green line is the mean of the data.

This accelerometer could use some calibration because the X and Y components should have a mean of zero and the Z (down) component should have a mean of 1g but that’s not that critical for the demo so I’ll skip it.

## Magnetometer

Magnetometers are sensors that measure magnetic fields. In the absence of nearby magnets a magnetometer will measure Earth’s magnetic field, which points to the geographic north (unless you are too close to the poles, I suppose), so you can use the magnetometer as digital compass.

Earth’s magnetic field is a 3D vector but the magnetometer in the LSM303DLHC decomposes it along three orthogonal axes.

The `lsm303dlhc`

driver provides a blocking `mag()`

method to read the magnetometer data.

```
let I16x3 { x, y, z } = lsm303dlhc.mag()?;
```

As with the other methods, you’ll get a 16 bit integer for each axis.

Below is shown a magnetometer reading obtained while the F3 board was sitting still on a horizontal surface. No scaling (multiply by the sensitivity) was performed.

```
I16x3 { x: 26, y: -167, z: -553 }
```

From the X and Y components you can compute where north is in the horizontal XY plane. For instance, if the X component is zero and the Y component is non zero then north is aligned to the Y axis of the magnetometer. The Z component, the vertical component of Earth’s magnetic field, varies with latitude; it should be close to zero when the measurement is done on the equator.

### Calibration

Like the gyroscope, the magnetometer also suffers from bias. Furthermore, the metal components on
the board itself can strengthen / weaken nearby magnetic fields; this results in different *per
axis* sensitivities so a magnetic field of constant magnitude can be perceived as having different
magnitudes when measured along the different axes of the magnetometer.

As Earth’s magnetic field magnitude is roughly constant, ideally we should measure it as having the same magnitude regardless of the orientation of the magnetometer. We’ll use this fact to calibrate the magnetometer and compensate for the ferromagnetic properties of the board.

Assuming the only magnetic field the magnetometer is sensing is the Earth’s we should observe that all the magnetometer readings satisfy the following equation:

```
mx * mx + my * my + mz * mz == K // Eq. 1
```

Where `mx`

is the reading along the X axis, `my`

is the reading along the Y axis, `mz`

the reading
along the Z axis, and `K`

is some constant.

This also happens to be the equation of a sphere centered at coordinate `(0, 0, 0)`

so if we plot
the readings in 3D space we should see that all of them lie on the surface of a sphere.

To calibrate the magnetometer we’ll collect measurements from the magnetometer in different orientations. We’ll do that by logging data while the board in being rotated in different ways. See video below for a demo of what I mean.

**IMPORTANT** While logging calibration data keep the magnetometer *away* from sources of
electromagnetic fields (EMF). The above video is actually a *bad* example in that regard because
the EMF that the laptop emits heavily affect the magnetometer readings – the laptop EMF can easily
double the magnetometer readings.

Here’s the data that was collected over the lapse of 32 seconds of motion.

Plot `M_XY`

is a scatter plot of the magnetometer readings along the X axis vs the readings along
the Y axis. Plots `M_XZ`

and `M_YZ`

are similar but involve a different pair of axes.

Remember that I said that the readings plotted in 3D space should look like a sphere? These first
three plots are like the projections of that sphere onto the XY, YZ and XZ planes so they *should*
look like circles centered at coordinate `(0, 0)`

but they are a bit off both in position and shape.

Plot `|M|`

is the magnitude of the Earth’s magnetic field as sensed by the magnetometer. The
Earth’s magnetic field is constant so the plot should be a straight line parallel to the X axis.
Instead we see that the sensed magnitude is all over the place.

The calibration process more or less boils down to finding a transformation of the calibration data
that makes it fulfill equation `1`

, the sphere equation. A proper solution involves matrices but a
simplified solution is to independently transform the readings along each axis according to this
equation:

```
// calibrate the magnetometer readings along the X axis
mx = (mx - bias_x) / range_x
```

Where `mx`

is an array of readings along the X axis, and `bias_x`

and `range_x`

are scalars that can
be found using these equations:

```
bias_x = (mx.max() + mx.min()) / 2;
range_x = (mx.max() - mx.min()) / 2;
```

Applying this transformation to the calibration data yields the following calibrated data:

Now the plots `M_XY`

, `M_YZ`

and `M_XZ`

do look like circles centered at `(0, 0)`

, and the magnitude
plot `|M|`

looks much more like a constant value with some noise sprinkled on it.

# Madgwick’s orientation filter

(This is the thing that you saw in the intro video.)

Madgwick’s orientation filter is a sensor fusion algorithm that computes the absolute orientation of
an object from MARG sensor data. MARG stands for Magnetic, Angular Rate and Gravity, and basically
refers to a system that can measure Earth’s **M**agnetic field, **A**ngular **R**ate and the
acceleration of **G**ravity. The magnetometer (M), gyroscope (AR) and accelerometer (G) on the F3
board form a MARG sensor array.

To give you an idea of how this filter works: the Gravity vector tells you where *down* is and the
Magnetic vector tells you where *north* is. These two vectors kind of form an absolute coordinate
system as their directions are pretty much constant.

Thus, the Gravity and Magnetic data give information about how the MARG system is oriented with
respect to this *down*-*north* coordinate system. Whereas the Angular Rate data gives information
about how that orientation is changing.

None of this data can’t be fully trusted though: if the MARG system moves then the accelerometer will measure not only the acceleration of gravity but also its own acceleration; the Magnetic data can be easily affected by EM radiation, external magnetic sources and nearby ferromagnetic materials; and the Angular Rate data suffers from bias that drifts over time and that changes with the ambient temperature.

The way the filter deals with all this uncertainty is to treat the data not as ground truth but as the input of an optimization problem. The rest is the magic of mathematics.

And it holds quite well, I must say. In the video at the beginning of this post the magnetometer is subject to the EMF that my laptop generates and at some point in the video I shake the board along the different accelerometer axes. All these are disturbances introduced in the input data but the filter handles them without much trouble.

For more details (i.e. the actual math) you can read Madgwick’s internal report. There’s also a conference paper but it’s behind a paywall – its contents are not that different from what’s written in the internal report though.

## API

I have published Madgwick’s orientation filter as the `madgwick`

crate. This crate is compatible
with `no_std`

programs and requires no memory allocation so it can be run on bare metal systems.

The API is straightforward to use: you create an instance of the filter; you feed MARG sensor data into it and you get back the 3D orientation as a unit quaternion.

```
let mut filter = madgwick::Marg::new(BETA, SAMPLING_PERIOD);
// e.g. `Quaternion(0.9999, 0.0017, -0.0046, 0.0006)`
let quat = filter.update(m, ar, g);
```

The parameter `BETA`

is the *gain* of the filter and its value should be in the same order of
magnitude as the measurement noise in the gyroscope (in `rad / s`

). You can measure the gyroscope
noise by logging gyroscope data while the board is still and then computing the standard deviation
of the readings along each axis.

The parameter `SAMPLING_PERIOD`

is the sampling period in seconds. You should run the filter
periodically and on each periodic run you should feed the latest sensor data to the filter. That
period is the sampling period of the filter.

## Visualizations

You can find the visualization software used for the Madgwick demo here. It uses `kiss3d`

to do
the rendering – kudos to the authors; this was my first time doing anything graphics related with
Rust and it was pretty straightforward 👍.

All the plots in this blog post where done using this Python script.

## Firmware

You can find the F3 firmware used for the demo as the `madgwick`

example in the `f3`

crate. That
crate also contains a `log-sensors`

example that was used to log the data used to make the plots
in this blog post and to calibrate the magnetometer.

# Conclusion

That’s it! Two `embedded-hal`

drivers are out; 50 more to go before the year ends 😅. And
you also got a reusable implementation of Madgwick’s orientation filter.

Finally, don’t forget to calibrate your sensors before you use them for anything serious!

**Thank you patrons! ❤️**

I want to wholeheartedly thank:

Iban Eguia, Aaron Turon, Geoff Cant, Harrison Chin, Brandon Edens, whitequark, James Munns, Fredrik Lundström, Kjetil Kjeka, Kor Nielsen, Alexander Payne, Dietrich Ayala, Kenneth Keiter, Hadrien Grasland, vitiral and 48 more people for supporting my work on Patreon.

Let’s discuss on reddit.