Home

Background

This project started in an effort to write a native CircuitPython library for AHRS (Attitude and Heading Reference System) after getting a Adafruit LSM9DS1 to help orientate a robot I was building. Adafruit has an excellent tutorial on sensor fusion but I realized all the examples were written for Arduino.

This library is my attempt to rewrite the C Arduino AHRS libraries to run directly in CircuitPython.

Caution

The AHRS algorithms can be processor intensive. I have tested on an SAMD51 and had acceptable results with the Mahony algorithm but less with Madgwick. Future faster processors may have better results. I have not attempted to port NXP Sensor Fusion yet.

Setup

All included example code was based on the LSM9DS1 as that is what I had access to. If you have another 9DOF sensor (or a combination of multiple sensors that give you 9DOF) you will have to configure your code to read from that sensor properly.

Calibration

Any sensor will need calibration to work properly. There are more complete guides available (such as in the link above) but I have included a basic routine that has given acceptable results.

There are two steps involved. For both start by: import calibrate Located in the examples folder.

Calibrate Magnetometer

1. Run calibrate.calibrate()
2. Move the device in all directions, figure 8s, and rotate it in all orientations
3. As it moves you will see the MAG_MIN and MAG_MAX values change
4. After these values seem to stabilize write these two values down

You will see in the mahony_simpletest.py file where these values go.

Calibrate Gyroscope

1. Run calibrate.calibrate()
2. Let the device sit perfectly still. Avoid any vibrations of the surface
3. Note the average gx/gy/gz values (the line that says Gyro: not the uncalibrated raw value above it)

You will see in the mahony_simpletest.py file where these values go.

Fuse!

The following is written for Mahony only. Madgwick is a similar process.

The first step is to create the AHRS filter.

ahrs = mahony.Mahony(Kp, Ki, frequency)

You will have to experiment with Kp and Ki to find values that work best for you. Search for information on PID controllers for more information on these values.

Frequency will be approximately how often you feed the filter new information in samples/second.

Next you will feed the filter information every time you read it (pseudo code follows):

while True:
     # read sensor values
     gx, gy, gz = sensor.gyro
     ax, ay, az = sensor.accelerometer
     mx, my, mz = sensor.magnometer

     ahrs.update(gx, gy, gz, ax, ay, az, mx, my, mz)

The values to be read from the system are in radians/sec. Multiply by 57.20578 to get degrees/second.

The following values are available from the filter:

1. Heading - Based on the z-axis the compass direction the sensor is facing (see the example to see how to convert to 0-360 degrees)
2. Pitch - Based on the y-axis how far up/down the sensor is tilted
3. Roll - Based on the x-axis how far left/right the sensor is tilted

You can also get the quaternion that the filter is holding internally by referencing the members q0, q1, q2, q3

Seeing Results

The Adafruit Sensor Fusion tutorial has a section on web serialization. The sample code will output values that can be read by this visualizer to see your results in action. Full credit to Adafruit (sorry I do not know who personally wrote it!) for this awesome tool to test with.