Tilt compensation code

This is the microcontroller code for the basic tilt-compensated compass described in the previous post. This will send the following results through the serial port at a baud rate of 9600: uncorrected heading, corrected heading, heel angle, pitch angle.

/* Basic tilt-compensated compass
 * Micromag3 magnetometer and SCA3000 accelerometer
 * using the WaveShare STK128+ Standard development
 * board with voltage level jumper set to 3.3 V

 * SCA3000 MOSI -> PB2(MOSI)
 * SCA3000 MISO -> PB3(MISO)
 * SCA3000 SCK -> PB1(SCK)
 * SCA3000 CSB -> PB4
 * SCA3000 RST -> PB5
 * SCA3000 INT (not connected)
 * SCA3000 VIN -> 5 V from USB port
 * SCA3000 GND -> common ground

 * Micromag3 MOSI -> PB2(MOSI)
 * Micromag3 MISO -> PB3(MISO)
 * Micromag3 SCLK -> PB1(SCK)
 * Micromag3 SSNOT -> PE2
 * Micromag3 RESET -> PB6
 * Micromag3 DRDY -> PE5
 * Micromag3 VDD -> 3.3 V
 * Micromag3 GND -> common ground

 * CP2102 USB Converter RX -> PD3 (TXD1)
 * Onboard LED -> PB0
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <util/delay.h>
#include <string.h>
#include <math.h>

#define PI 3.14159265
#define DEG_TO_RAD ((double)(PI/180.0))
#define RAD_TO_DEG ((double) (180.0/PI))

// _delay_loop_2(18432) = 0.01 s

double ax, ay, az;
double mx, my, mz;
volatile uint8_t iflag;
uint8_t istate;
uint8_t acount;
int32_t axreads;
int32_t ayreads;
int32_t azreads;
double xc, yc, y_ax_ay;

// Magnetometer calibration data
double m_xBias, m_sens_x;
double m_yBias, m_sens_y;
double m_zBias, m_sens_z;

// Accelerometer calibration data
double a_xBias, sens_x;
double a_yBias, sens_y;
double a_zBias, sens_z;

// Heel and pitch angles
double rho, phi;

// Buffer for accelerometer readings.
int16_t reads[3];

/*
 * This interrupt routine is called when the DRDY line
 * of the Micromag3 goes high
 */
ISR(INT5_vect)
{
   iflag = 1;
   EIMSK &= ~(_BV(INT5));  // disable interrupt on DRDY line
}

static int uart_putchar1(char c)
{
   loop_until_bit_is_set(UCSR1A, UDRE1);
   UDR1 = c;
   return 0;
}
void SPI_MasterInit(void)
{
   uint8_t i;

   for(i = 1; i < 100; i++)  // 1 s delay
      _delay_loop_2(18432);

   // Enable pull-up on PB4(CS0) and PE2(SSNOT)
   PORTB = _BV(PB4);
   PORTE = _BV(PE2);
  
   // Set PB0(LED), PB1(SCK), PB2(MOSI), PB5(RST0) and PB6(RST1) as output low
   // Set PB4(CS0) as output high
   DDRB = _BV(DDB0) | _BV(DDB1) | _BV(DDB2) | _BV(DDB4) | _BV(DDB5) | _BV(DDB6);
   // Set PE2(SSNOT) as output high
   DDRE = _BV(DDE2);

   // Enable SPI, Master, set clock rate fck/16
   SPCR = _BV(SPE) | _BV(MSTR) | _BV(SPR0); // 460.8 kHz

   for(i = 1; i < 100;i++)  // 1 s delay
      _delay_loop_2(18432);
 
   // release SCA3000 reset (PB5)
   PORTB |= _BV(PB5);

   for(i = 1; i < 25; i++)  // 0.25 s delay
      _delay_loop_2(18432);
}

/*
 * Start magnetometer measurement on axis 0(x), 1(y) or 2(z)
 */
void mag_start(uint8_t axis)
{
   PORTE &= ~(_BV(PE2)); // select Micromag3
   
   PORTB |= _BV(PB6); // pulse the reset (minimum 100 nanoseconds)
   asm volatile("nop\n\t"
                "nop\n\t"
                ::);
   PORTB &= ~(_BV(PB6));
  
   SPDR = 0x70 + axis + 1;
   while(!(SPSR & _BV(SPIF)));
  
   PORTE |= _BV(PE2); // deselect Micromag3
   istate = axis;  // save current active axis
   iflag = 0; // reset end-of-measurement flag
   EIMSK |= _BV(INT5);  // enable interrupt on DRDY line
}

int main(void)
{
   char buffer[64];
   uint8_t i;

   acount = 0;
   istate = 0;
   iflag = 0;

   for(i = 1; i < 100; i++)  // 1 s delay
      _delay_loop_2(18432);

   /* enable serial port UART */ 
   /* Set baud rate : 9600 bps @ 7.3728 MHz */
   UBRR1L = (unsigned char)(47);
   /* Enable transmitter */
   UCSR1B = _BV(TXEN1);

   SPI_MasterInit();

   // enable external interrupt on DRDY line
   EICRB = _BV(ISC50) | _BV(ISC51);

   sei();

   a_xBias = 278.0;
   a_yBias = 114.0;
   a_zBias = -131.5;

   sens_x = 1331.0;
   sens_y = 1334.0;
   sens_z = 1331.5;

   m_xBias = -49.5;
   m_yBias = 24.5;
   m_zBias = 26.0;

   m_sens_x = 3530.5;
   m_sens_y = 3498.5;
   m_sens_z = 3236.0;

   mag_start(0);  // start x-axis measurement

   while(1)
   {
      if(iflag)  // we have a magnetometer measurement
      {
         uint8_t mh, ml;
         int16_t mag;
  
         // read the result
         PORTE &= ~(_BV(PE2)); // select Micromag3
        
         SPDR = 0x00;
         while(!(SPSR & _BV(SPIF)));
         mh = SPDR;
         SPDR = 0x00;
         while(!(SPSR & _BV(SPIF)));
         ml = SPDR;
        
         PORTE |= _BV(PE2); // deselect Micromag3
         mag = (((int16_t)mh) << 8) + ml;
  
         switch(istate)  // which axis did we measure?
         {
         case 0:    // we just read magnetometer x-axis
            mx = (double)mag;
            mag_start(1);  // start y-axis measurement
            break;
         case 1:    // we just read magnetometer y-axis
            my = (double)mag;
            mag_start(2);  // start z-axis measurement
            break;
         case 2:    // we just read magnetometer z-axis
            mz = (double)mag;
   
            mx = (mx - m_xBias) / m_sens_x;
            my = (my - m_yBias) / m_sens_y;
            mz = (mz - m_zBias) / m_sens_z;
   
            // calculate and print the uncorrected heading
            double headbrut = 360.0 - atan2(my, mx) * RAD_TO_DEG;
            if(headbrut > 360.0)
               headbrut -= 360.0;
            sprintf(buffer, "%5.1f", headbrut);
            for(i = 0; i < strlen(buffer); i++)
               uart_putchar1((unsigned char)(buffer[i]));
   
            // calculate accelerometer average
            reads[0] = axreads / acount;
            reads[1] = ayreads / acount;
            reads[2] = azreads / acount;
   
            ax = (reads[0] - a_xBias) / sens_x;
            ay = (reads[1] - a_yBias) / sens_y;
            az = (reads[2] - a_zBias) / sens_z;
   
            // calculate heel(rho) and pitch(phi)
            rho = -atan(ay / sqrt(ax * ax + az * az)) * RAD_TO_DEG;
            phi = -atan(ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;
   
            // normalize accelerometer readings
            double norm = sqrt(ax * ax + ay * ay + az * az);
            ax /= norm;
            ay /= norm;
            az /= norm;
   
            // tilt compensation
            ay = -ay;
            double one_ax2 = 1.0 - ax * ax;
            y_ax_ay = my * ax * ay;
            xc = mx * one_ax2 - y_ax_ay - mz * ax * az;
            yc = my * az - mz * ay;
   
            // calculate and print corrected heading, heel, pitch
            double head_corr = 360.0 - atan2(yc, xc) * RAD_TO_DEG;
            if(head_corr > 360.0)
               head_corr -= 360.0;
            sprintf(buffer, "  %5.1f  %5.1f  %5.1f", head_corr, rho, phi);
            for(i = 0; i < strlen(buffer); i++)
               uart_putchar1((unsigned char)(buffer[i]));
            uart_putchar1('\r');
            uart_putchar1('\n');
   
            PORTB ^= 0x01;  // toggle LED
   
            axreads = 0;
            ayreads = 0;
            azreads = 0;
            acount = 0;
   
            mag_start(0);  // start x-axis measurement
         }
      }
 
      uint8_t azh, azl, ayh, ayl, axh, axl;  
      // read accelerometer
      // Select SCA3000
      PORTB &= ~(_BV(PB4));

      SPDR = 0x09 << 2;
      while(!(SPSR & _BV(SPIF)));
      
      SPDR = 0x00;
      while(!(SPSR & _BV(SPIF)));
      azh = SPDR;
      SPDR = 0x00;
      while(!(SPSR & _BV(SPIF)));
      azl = SPDR;
      SPDR = 0x00;
      while(!(SPSR & _BV(SPIF)));
      ayh = SPDR;
      SPDR = 0x00;
      while(!(SPSR & _BV(SPIF)));
      ayl = SPDR;
      SPDR = 0x00;
      while(!(SPSR & _BV(SPIF)));
      axh = SPDR;
      SPDR = 0x00;
      while(!(SPSR & _BV(SPIF)));
      axl = SPDR;
 
      // Deselect SCA3000
      PORTB |= _BV(PB4);
 
      reads[2] = ((((int16_t)azh) << 8) + azl) >> 3;
      reads[1] = ((((int16_t)ayh) << 8) + ayl) >> 3;
      reads[0] = ((((int16_t)axh) << 8) + axl) >> 3;
  
      axreads += reads[0];
      ayreads += reads[1];
      azreads += reads[2];
  
      acount++;
  
      _delay_loop_2(7089);    // 1/260 s delay
   }
}  

A custom gyro compass (Phase 2)

Here are some results of Phase 2 of this project, whose objective was to test a tilt-compensated compass in static conditions.

The compass actually consists of a single Micromag3 magnetometer and an SCA3000 accelerometer, arranged on a breadboard in the following configuration. The N+ means that the magnetometer reading is positive when the arrow points to magnetic North. Note the opposite directions of the Y-axis: this will be corrected in code by inverting the sign of the Y-axis accelerometer reading. The Z-axis points down in both cases.

The magnetometer has been calibrated using the same technique previously described for the accelerometer, with the following results:

The magnetometer reads continuously and consecutively the X, Y and Z axis. A complete measurement of the 3 axis takes about 105 ms. The microprocessor can take 31 complete accelerometer readings while waiting for the magnetometer to complete its measurements. The normalized average of these 31 measurements (a_x, a_y, a_z) is used in the calculations, along with the current magnetometer result (m_x, m_y, m_z).

The tilt-compensation equations are: 

In this first example, the breadboard is tilted on its side to produce a heel angle of around 35 degrees, and then slowly comes back to horizontal, as shown by the green curve. This produces a huge variation in the uncorrected heading (the blue curve). The tilt compensation does a decent job, but not good enough, as the red curve should ideally be horizontal. This means that the calibration should be improved.

The next example illustrates the dynamic response, as the breadboard is tilted from one side to the other. What is interesting here is that the tilt compensation is able to follow the moving compass without problem.

 The next step will be to implement more robust calibration procedures for the magnetometer and the accelerometer, adding required corrections for the lack of perpendicularity of the axes and linearity of the responses.

Tilt compensation: what to expect

In order to help in the calibration of a tilt-compensated compass, I have pre-calculated what are the uncorrected compass readings, for different heading and heel angles.

At my northern location, the horizontal and vertical components of the magnetic field are respectively 17.686 and 51.676 microteslas, meaning that the inclination of the field in the ground is 71.107°.

The results of these calculations are summarized in the following two figures, for positive and negative heel angles (with a zero pitch angle). Heel angle is considered positive when the mast leans to starboard.

It is interesting to note that there is one heading where the non-compensated compass will give a correct result, independantly of the heel angle: 270° for positive heel, and 90° for negative heel.

To reproduce these results for other locations, here are the equations to use.