Helicopter SAS: AHRS module

This post will describe the AHRS (Attitude and Heading Reference System) module of my helicopter stability augmentation system.

Summary

The AHRS provides the sensing component of the stability augmentation system (SAS). It estimates the helicopter's orientation using measurements from the onboard accelerometer and gyro. Ever 20 ms, it reports an updated estimate to the SAS main board over an SPI serial interface.

Hardware

This section describes the hardware and communications connectivity of the AHRS module.

The AHRS uses an unmodified SparkFun 9 degree of freedom Razor IMU (rev 14, with the 8Mhz ATmega328P). The board comprises:

1. ATmega328P 8-bit microcontroller with 8 MHz clock
2. 3-axis accelerometer
3. 2-axis (pitch/roll) and 1-axis (yaw) gyros, with 48 Hz lowpass filter
4. 3-axis magnetometer
5. 6-pin header for Sparkfun's FTDI USB-UART breakout board
6. 6-pin header for ICSP
7. 2-pin header for unregulated power supply

The microcontroller interfaces with the accelerometer and magnetometer via I²C and with the gyros via its ADC channels. The UART header brings out the RX and TX pins. The ICSP header brings out the the SPI MISO, MOSI, and clock pins, but unfortunately not the slave select pin, so we can only operate the SPI module in master mode.

Under normal operation, the AHRS communicates with the SAS main board over SPI. The UART module is disabled and the RX pin used as a digital output to drive the SAS main board's SPI slave select pin. That is the purpose of the white wire in the photo at the top of this post. 

The AHRS board is mounted inside a polycarbonate enclosure wrapped in aluminum foil. When the main board is installed on top of the box, its ground plane makes contact with the foil, creating a shielded environment for the sensors.

Estimator Algorithm

This section describes the quaternion-based kinematic estimator implemented as a first-order linear complementary filter between gyro measurement of rotation rate and accelerometer measurement of gravity vector.

The purpose of the AHRS is to provide estimates of rotation rate, orientation, and translational acceleration to the SAS. The gyros directly measure rotation rate, but the accelerometers measure a combination of gravitational and translational acceleration, so we need an estimator to separate the two. For now, we are not using the magnetometer because the local magnetic fields from the helicopter's motors disrupt the measurements.

First, the sensor measurements are filtered with an 8 Hz (50 rad/s) first-order digital lowpass filter. This helps mitigate the noise (mechanical vibration) produced by the main rotor. Combined with oversampling (estimator loop runs at 50 Hz, but gyros are sampled at 500 Hz and accelerometer at 200 Hz), this also increases measurement resolution, but that's a moot point given the low signal-to-noise ratio in flight conditions.

Unlike a dynamic estimator, which uses the dynamic equations of motion (F = ma), a kinematic estimator uses only kinematic equations (v = dx/dt, a = dv/dt). In this case, we estimate orientation (rotation angles) from gyro measurements (rotation rates) and gravity vector measurements (rotation angle error). Implemented in discrete time, the estimator looks like:

1. At time t₁, we have an estimated orientation q₁, a quaternion representing the rotation of the helicopter from its reference position to its current orientation. Therefore q₁ also represents the transformation from body-fixed axes to inertial axes, and q₁^-1 the transformation from inertial to body-fixed.
2. We can predict that the gravity vector, measured in body-fixed axes, will be g_pred = q₁^-1g₀q₁, where g₀ is the gravity vector in inertial axes, i.e. [0;  0;  1].
3. The actual accelerometer measurement a_meas gives us two things:
1. First, the translational acceleration is simply the residual acceleration: a_trans = a_meas - g_pred
2. Second, assuming a_trans averages out to zero, our measured gravity vector is simply our measured acceleration, normalized to one gee: g_meas = a_meas/||a_meas||
4. Thus, step 3.2 gives us one estimate of orientation error: for small angles, Δθ_grav = g_meas × g_pred, where Δθ_grav is the additional rotation we need to apply to q₁ in order to bring the predicted gravity vector in line with the measured gravity vector.
5. However, our rotation rate measurements give us another estimate of orientation error: Δθ_gyro = ωΔt where ω is the gyro rotation rate measurement.
6. This is where the complementary filter steps in. For a crossover frequency f_C << 1/Δt, the update law Δθ = Δθ_gyro + (2πf_CΔt)Δθ_grav is equivalent to low-passing Δθ_grav at f_C, high-passing Δθ_gyro at the same frequency, and summing the two filtered values, an operation that preserves unity gain and zero phase over all frequencies.
7. For small angles, we can update the quaternion using the operation:
   q₂ = [1;  Δθ_x/2;  Δθ_y/2;  Δθ_z/2] × q₁
8. We can then report rotation rate (ω, measured), orientation (Euler angles derived from q), and translational acceleration (a_trans, from step 3.1) to the SAS main board.

I chose a crossover frequency f_C = 0.1 Hz (0.6 rad/s) as a balance between the evils of offset drift in the rotation rate measurements (mitigated by high crossover frequency) and translational acceleration transients in the gravity vector measurements (mitigated by low crossover frequency).

Software Implementation

The AHRS code is divided into several modules: a main module that calls the necessary functions from the other modules, a kinematic estimator module that implements the algorithm described above, and low-level input, ouput, and math modules.

Main module

One of the funny things the Arduino IDE does is it takes your .pde source files and puts them all into one file and auto-generates a header file before compilation. On the one hand, you don't need to write your own header files, but it also means you can't use keywords like "static" to manage access control (because everything ends up in the same file anyways). So I decided to be lazy and just make all the vector and matrix variables global. They are declared in the main module.

The main module also contains:

setup(): Calls the other modules' initialization functions

loop(): Called whenever the processor is free; it polls the Arduino library millis() function to run a 50 Hz update. The loop body consists of calling the 3 sensor update functions (gyro, accelerometer, and magnetometer), the estimator update function, and finally the data output function.

Estimator module

estimator_init(): Takes an average of a bunch of gravity vector measurements to create a reference vector for the "zero" orientation.

estimator_update(): Calls the low-level math routines to carry out the algorithm described above.

Input modules

I2C module: This is a C library I wrote as an alternative to the Arduino library's supplied I2C module. In order to write data, you supply the module with an I2C address, a pointer to a flag byte, and the data you wish to write. The module copies the data into an internal buffer, begins the writing process (done through hardware), and returns. The ISR makes sure your data gets sent and sets your flag when it's done. To read data, you pass it a pointer to a read buffer. The ISR populates the read buffer as data comes in, and sets your flag when it's done. This way, multiple sources can all access the I2C module at the same time, and nobody has to wait on the hardware.

Sensors module: The sensors are all read and filtered on their own loop (a 1 kHz timer ISR), separate from the main loop. The gyro (an analog input) is read at 500 Hz and the accelerometer (I2C interface) at 200 Hz; both are filtered with an 8 Hz digital lowpass filter. When the user asks for sensor data, the module returns the most recent filtered values (within a global interrupt disable/enable block).

Output modules

SPI Master module: This is a pretty straightforward module implementing blocking code to send a 38-byte data frame from the AHRS to the SAS. Unlike the I2C module, which is called from within an interrupt, it is okay for this code to hold up execution. The data frame consists of a start-of-frame byte, 9 floating point values (Euler angles, gyro rates, and translational accelerations), and a checksum byte.

Serial output module: This is only active in debug mode; it calls the Arduino library Serial.print() functions to send AHRS results and raw sensor readings as ASCII text through the UART module.

Math modules

Vector module: Contains some very straightforward functions for vector addition, scaling, and dot and cross products. Also the fast inverse square root routine, for vector normalization.

Matrix module: Matrix multiplication, and a function for turning a rotation matrix into Euler angles.

Quaternion module: Functions for quaternion scaling and normalization, rotating a quaternion by body-fixed Euler angles, and converting a quaternion into a rotation matrix.