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
   }
}