Monday, July 18, 2011

Interfacing the LS20031 GPS Receiver

This system currently gets GPS data from an LS20031 GPS receiver, interfaced to the Olimex I2C client. In order to configure the GPS, it is convenient to use the MiniGPS software available on the SparkFun site for this product.

Here is a configuration arrangement where a CP2102 USB converter is used for communication to the PC running MiniGPS, as well as a 3.3 V power supply to the GPS.





The MiniGPS software has been used to configure the $GPRMC NMEA 0183 sentence at a baud rate of 9600, with an update rate of 5 Hz, and WAAS enabled ( I am currently using an older 5 Hz LS20031 version, soon to be replaced by the 10 Hz one).

Here is the format of the output sentence:
               $GPRMC,180846.600,A,4659.9999,N,07059.9999,W,6.00,45.00,050210,,,D*77
where the 6 numbers to be extracted are highlighted. The latitude and longitude are each read as 2 integer values to avoid rounding errors here, followed by the SOG and COG floating point values.

WARNING: the onboard battery will keep the configuration up to several weeks if the GPS is not powered during this time, and will then revert to the default configuration with a 57600 baud rate. After a winter season on the dry, the configuration step has to be repeated.
 
Once configured, here is how the GPS is interfaced to the microcontroller, using a logic level converter to step up the NMEA output from 3.3 to 5 V. This is a good illustration of the kind of punishment you deserve when you mix 3.3 V sensors with 5 V microprocessors.


Here is the part of the code used by the Olimex microprocessor to parse the NMEA data from the GPS.
unsigned static char buf0[500];
volatile unsigned char temprec0;
volatile unsigned char idx0 = 0;
typedef union
{
   unsigned char messageBuf[36];
   struct
   {
      double heading;   // magnetic heading from gyrocompass
      double heel;   // heel angle from gryrocompass
      double pitch;   // pitch angle from gyrocompass
      double rot;    // rate of turn from gyrocompass
      double cog;    // COG from GPS
      double sog;    // SOG from GPS
      int long1;    // longitude (1st part) from GPS
      int long2;    // longitude (2nd part) from GPS
      int lat1;    // latitude (1st part) from GPS
      int lat2;    // latitude (2nd part) from GPS
      double speed2;    // boat speed from port transducer
   };
} package;

volatile package pack;

/* This interrupt routine is called each time a new character
   is received from the GPS NMEA stream. When the end of
   a NMEA sentence is detected (by the '\n' character), the
   complete sentence accumulated in the 'buf0[]' buffer is deciphered,
   the desired numbers are extracted and put in the 'pack' repository,
   that is used for the I2C transfer to the main controller.
*/
ISR(USART0_RX_vect)
{
   temprec0 = UDR0;
   if(temprec0 != '\n')
   {
      buf0[idx0++] = temprec0;
   }
   else
   {
     buf0[idx0] = '\0';
     idx0 = 0;
     if(buf0[0] == '$')
     {
        // $GPRMC,180846.600,A,4659.9999,N,07059.9999,W,6.00,45.00,050210,,,D*77
        sscanf(&(buf0[20]), "%d", &pack.lat1);
        sscanf(&(buf0[25]), "%d", &pack.lat2);
        sscanf(&(buf0[33]), "%d", &pack.long1);
        sscanf(&(buf0[38]), "%d", &pack.long2);
        sscanf(&(buf0[45]), "%lf,%lf", &pack.sog, &pack.cog);
     }
   }
}
int main(void)
{
   ...
   /* USART0 */
   /* Set baud rate : 9600 @ 16MHz */
   UBRR0L = (unsigned char)(103);
   /* Enable receiver and interrupt on received characters */
   UCSR0B = _BV(RXEN) | _BV(RXCIE);
   idx0 = 0;
 
   ...
}

Saturday, July 16, 2011

I2C Transfer between 2 microcontrollers

In this system, the Olimex board is programmed as an I2C client, and the main controller as an I2C master.


  I2C Client Side

The I2C client code is based on Atmel Application Note AVR311: “Using the TWI module as I2C slave”, and the corresponding Atmel files ‘TWI_Slave.h’ and ‘TWI_Slave.c’. These 2 files shall be included in the project and be part of the compilation. The following line of the ‘TWI_Slave.h’ file should however be changed from:
               #define TWI_BUFFER_SIZE 4
to:
                #define TWI_BUFFER_SIZE 36

Here, we chose a very minimalist implementation where the I2C client is always addressed for reading by the master, never for writing. Each time the I2C client is addressed for reading, it expects that the master will always request a fixed number of 36 bytes.

With this minimal implementation, the required I2C client code is deceptively simple. It requires only the following lines, where ‘ens.messageBuf’ is the address of the first byte of the 36-byte buffer to transmit. The buffer is continually updated by the interrupt service routines of the GPS, the gyrocompass and one of the speed transducers.


#include "TWI_Slave.h"


int main(void)
{
   unsigned char TWI_slaveAddress;

  
   // Own TWI slave address
   TWI_slaveAddress = 0x10;


   // Initialise TWI module for slave operation.
   // Include address and/or enable General Call.
   TWI_Slave_Initialise( (unsigned char)((TWI_slaveAddress<<TWI_ADR_BITS)
      | (TRUE<<TWI_GEN_BIT) ));

  
   …
   // Start the TWI transceiver to enable reseption of the first command

   // from the TWI Master.
   TWI_Start_Transceiver_With_Data((char*)(ens.messageBuf), 36);

   for(;;)
   {
      if (!TWI_Transceiver_Busy())
      {
         TWI_Start_Transceiver_With_Data((char*)(ens.messageBuf), 36);
      }
   }
}





I2C Master Side

Here the code used by the Mavric-IIB master controller to read the I2C client buffer.

typedef union  //  this is the same data structure used by the client
{
   unsigned char messageBuf[36];
   struct
   {
      double heading;
      double heel;
      double pitch;
      double rot;
      double cog;
      double sog;
      int long1;
      int long2;
      int lat1;
      int lat2;
      double speed2;
   };
} package;

volatile package pack;
...

int main(void)
{
   ...
   /* set the I2C bit rate generator to 100 kb/s */
   TWSR &= ~0x03;
   TWBR  = 28;
   TWCR |= _BV(TWEN);

   ...

   for(;;)  
   {
      // begin a new cycle
      ...
  
      // fetch the I2C client

     
      // I2C start
      TWCR = (_BV(TWINT) | _BV(TWEN) | _BV(TWSTA));
      while(!(TWCR & _BV(TWINT)));
     

      // select Olimex client (I2C address 0x10) for reading
      TWDR = (0x10 << 1) + 1;
      TWCR = (_BV(TWINT) | _BV(TWEN));
     
while(!(TWCR & _BV(TWINT)));
     

      // read the first 35 bytes, with acknowledge request
      for(i = 0; i < 35; i++)
      {
         TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA);
         while(!(TWCR & _BV(TWINT)));
         pack.messageBuf[i] = (unsigned char)TWDR;
      }
     

      // read the last byte without acknowledge request
      TWCR = _BV(TWINT) | _BV(TWEN);
      while(!(TWCR & _BV(TWINT)));
      pack.messageBuf[35] = (unsigned char)TWDR;
     

      // I2C stop
      TWCR = _BV(TWINT) | _BV(TWEN)| _BV(TWSTO);
      while(TWCR & _BV(TWSTO));
  
      ...
      // wait for a timer signal to begin a new cycle
      ...

   }
}




WARNING : this is a minimalist implementation of I2C for the ATMega128, without any error checking. If the I2C link between the master and the client is disconnected, the master will hang forever. This would not be acceptable in a commercial or distributable product, but the objective here was to keep the code as simple as possible.

Sunday, July 10, 2011

Interfacing the Airmar H2183 Gyrocompass (Part 2)

Here is the part of the code used by the Olimex microcontroller to parse the gyrocompass NMEA data. The I2C transfer code (using Atmel "TWI_Slave.h") will be further documented in a future post.

#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "TWI_Slave.h"
#include <string.h>
#include <stdlib.h>
#include <uart.h>
#include <math.h>


unsigned static char buf[500];
volatile unsigned char temprec;
volatile unsigned char idx = 0;
double theta_rad, dtheta_rad, deviation;


...

typedef union
{
  unsigned char messageBuf[36];

  struct
  {
    double heading;   // magnetic heading from gyrocompass
    double heel;   // heel angle from gryrocompass
    double pitch;   // pitch angle from gyrocompass
    double rot;    // rate of turn from gyrocompass
    double cog;    // COG from GPS
    double sog;    // SOG from GPS
    int long1;    // longitude (1st part) from GPS
    int long2;    // longitude (2nd part) from GPS
    int lat1;    // latitude (1st part) from GPS
    int lat2;    // latitude (2nd part) from GPS
    double speed2;    // boat speed from port transducer

  };
} package;


volatile package pack;

/* This interrupt routine is called each time a new character
   is received from the gyrocompass NMEA stream. When the end of
   a NMEA sentence is detected (by the '\n' character), the
   complete sentence accumulated in the 'buf[]' buffer is deciphered,
   the desired numbers are extracted and put in the 'pack' repository,
   that is used for the I2C transfer to the main controller.
*/

ISR(USART1_RX_vect)
{
  temprec = UDR1;
  if(temprec != '\n')
  {
    buf[idx++] = temprec;
  }
  else
  {
    buf[idx] = '\0';
    idx = 0;
    if(buf[0] == '$')
    {
      if(buf[1] == 'H')
      {
        sscanf(buf, "$HCHDG,%lf", &pack.heading);
 
        // calculate deviation

        theta_rad = pack.heading * 3.14159 / 180.0;
        dtheta_rad = theta_rad * 2.0;
        deviation = 4.103844 - 8.302381 * sin(theta_rad)
           + 15.92628 * cos(theta_rad)
           + 1.519511 * sin(dtheta_rad)
           + 2.346229 * cos(dtheta_rad);
 
        pack.heading -= deviation;
 
        if(pack.heading > 360.0)
          pack.heading -= 360.0;
        else if(pack.heading < 0.0)
          pack.heading += 360.0;
 
      }
      else if(buf[1] == 'P')
        sscanf(buf, "$PFEC,GPatt,,%lf,%lf", &pack.pitch, &pack.heel);
      else if(buf[1] == 'T')
        sscanf(buf, "$TIROT,%lf", &pack.rot);
    }
  }
}


int main(void)
{
  unsigned char TWI_slaveAddress;

  ...
 
  /* USART1 */
  /* Set baud rate : 4800 @ 16MHz */
  UBRR1L = (unsigned char)(207);
  /* Enable receiver and interrupt on received characters */
  UCSR1B = _BV(RXEN) | _BV(RXCIE);
  idx = 0;
 
  ...
 
  // Own TWI slave address
  TWI_slaveAddress = 0x10;

  // Initialise TWI module for slave operation.
  // Include address and/or enable General Call.
  TWI_Slave_Initialise((unsigned char)((TWI_slaveAddress << TWI_ADR_BITS)
                                         |(TRUE<<TWI_GEN_BIT)));  


  sei();
 
  ...
 
  /* Start the TWI(I2C) transceiver to enable reception of the first
     command from the TWI(I2C) Master. This will be further documented
     in a future post on I2C transfer.
  */
  TWI_Start_Transceiver_With_Data((char*)(pack.messageBuf), 36);
  
  for(;;)
  {
    if (!TWI_Transceiver_Busy())
      TWI_Start_Transceiver_With_Data((char*)(pack.messageBuf), 36);
  }
}

Interfacing the Airmar H2183 Gyrocompass (Part 1)

In this system, the Olimex microcontroller is used to read the NMEA output from the gyrocompass and from the GPS. It is also programmed as an I2C client that the main controller interrogates ten times per second.

The system actually uses an Airmar H2183 gyrocompass to get heading, heel and pitch angles, and rate of turn values. This compass can provide both NMEA 0183 and NMEA 2000 outputs. Here we use the NMEA 0183 output which is a standard RS232 serial bus.

In order to configure and calibrate the compass, it is necessary to get a specific Airmar cable and USB converter in the following temporary arrangement.



The Airmar WeatherCaster software has been used to configure the following NMEA 0183 sentences:
               $HCHDG (at 5 Hz) for the magnetic heading
               $PFEC,GPatt (at 5 Hz) for the heel and pitch angles
               $TIROT (at 2 Hz) for the rate of turn.

For this selection of outputs, we are limited to 5 Hz by the 4800 baud NMEA 0183 bandwidth.

Here is the format of the output sentences:
               $HCHDG,55.6,0.0,E,,*1F     (magnetic heading : 55.6 deg)
               $PFEC,GPatt,,-8.7,+4.8*63   (pitch angle : -8.7 deg,  heel angle: 4.8 deg)¸
               $TIROT,4.3,A*3C   (rate of turn:  4.3 deg/min).

The WeatherCaster software has also an autocalibration routine, but it is almost necessary to use a custom calibration, as explained in a previous post: http://sailboatinstruments.blogspot.com/2011/01/gyro-compass-calibration.html

Once configured, here is how the compass is interfaced to the microcontroller, using a Conxall CX-428-8-pin connector :
http://www.blueheronmarine.com/Conxall-CX-428-8-Pin-Panel-Mount-Socket-Male-6907



Here is the pin-out of the WS-C01 cable. On the microcontroller side of the cable, we need to provide +12 V to pin 2, ground pins 1 and 8, and connect pin 5 (A/+ OUT) to the receive pin of the DB9 connector of the Olimex board.

 
In part 2, I will post the microcontroller code used to parse the NMEA sentences.

Thursday, May 26, 2011

True wind, VMG and current calculations



Ten times per second (10 Hz rate), the master controller goes through all true wind, VMG and current calculations. Here the code used for these calculations.

/*
 [this_blog] == sailboatinstruments.blogspot.com
*/


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


// Inputs

/* The measured apparent wind angle is the
   calibrated wind vane reading:
   [this_blog]/2011/10/new-wind-vane-calibration.html
*/

double awa_measured;  // -180 to 180 degrees

/* The offset is positive is the masthead unit misalignment is
   clockwise from above, and negative if the misalignment is
   counterclockwise from above. Obtained from calibration test run:
   [this_blog]/2011/02/corrections-to-apparent-wind-angle.html
*/

double offset;  // degrees, positive or negative

/* The measured boat speed is the speed sensor reading,
 [this_blog]/2011/03/measuring-boat-speed-part-1.html
 corrected by a calibration factor from a trial run:
 [this_blog]/2011/01/boat-and-wind-speed-calibration.html
*/

double meas_boat_speed;  // knots


/* The apparent wind speed is the speed sensor reading,
 corrected by a calibration factor from a trial run:
 [this_blog]/2011/01/boat-and-wind-speed-calibration.html
*/

double aws;

/* Heel is positive when the mast leans to starboard,
   and negative when the mast leans to port. Obtained
   from gyro compass.
*/

double heel;   // degrees, positive or negative

/* The leeway calibration factor K is obtained
   from a trial calibration run:
   [this_blog]/2011/02/leeway-calibration.html
*/

double K;

/* The magnetic heading is obtained from the
   gyrocompass reading, corrected for deviation:
   [this_blog]/2011/01/gyro-compass-calibration.html
*/

double heading;  // 0 to 360 deg (magnetic)

/* The COG (course over ground) is the true
   heading from the GPS
*/

double cog;  // 0 to 360 deg (true)

/* The SOG (speed over ground) is the boat speed
   measured by the GPS
*/

double sog;  // knots

/*  Magnetic variation: difference between true and magnetic North
*/

double variation;

// Outputs

double awa_offset;  // AWA corrected for offset (-180 to 180)
double awa_heel;  //   AWA corrected for heel (-180 to 180)

double leeway;    // leeway angle (degrees, positive or negative)
double stw;  // speed through water (knots)
double vmg;  // velocity made good (knots)

double tws;  // true wind speed (knots)
double twa;  // true wind angle (-180 to 180)
double wdir; // wind direction (0 to 360 deg, magnetic)
double soc;  // speed of current (knots)
double doc;  // direction of current (0 to 360 deg, magnetic)


// Correct awa for alignment offset
awa_offset = awa_measured + offset;
if(awa_offset > 180.0)
   awa_offset -= 360.0;
else if(awa_offset < -180.0)
   awa_offset += 360.0;


// Correct awa for heel
double tan_awa = tan(awa_offset * DEG_TO_RAD);
if(isnan(tan_awa))
   awa_heel = awa_offset;
else
{
   double cos_heel = cos(heel * DEG_TO_RAD);
   awa_heel = atan(tan_awa / cos_heel) * RAD_TO_DEG;
 

   if(awa_offset >= 0.0)
   {
      if(awa_offset > 90.0)
         awa_heel += 180.0;
    }
    else
    {
       if(awa_offset < -90.0)
          awa_heel -= 180.0;
    }
}


// Calculate leeway angle
if(meas_boat_speed == 0.0
 || (awa_heel > 0.0 && heel > 0.0)
 || (awa_heel < 0.0 && heel < 0.0))
        leeway = 0.0;
else
{
   leeway = K * heel / (meas_boat_speed * meas_boat_speed);
   // limit leeway value for very low speeds
   if(leeway > 45.0)
      leeway = 45.0;
   else if(leeway < -45.0)
      leeway = -45.0;
}


// Calculate STW (speed through water)
stw = meas_boat_speed / cos(leeway * DEG_TO_RAD);


// Calculate component of stw perpendicular to boat axis
double lateral_speed = stw * sin(leeway * DEG_TO_RAD);

// Calculate TWS (true wind speed)
double cartesian_awa = (270.0 - awa_heel) * DEG_TO_RAD;
double aws_x = aws * cos(cartesian_awa);
double aws_y = aws * sin(cartesian_awa);
double tws_x = aws_x + lateral_speed;
double tws_y = aws_y + meas_boat_speed;
tws = sqrt(tws_x * tws_x + tws_y * tws_y);


// Calculat TWA (true wind angle)
double twa_cartesian = atan2(tws_y, tws_x);
if(isnan(twa_cartesian)) // singularity
{
   if(tws_y < 0.0)
      twa = 180.0;
    else twa = 0.0;
}
else
{
   twa = 270.0 - twa_cartesian * RAD_TO_DEG;
   if(awa_heel >= 0.0)
      twa = fmod(twa, 360.0);
   else
      twa -= 360.0;

   if(twa > 180.0)
      twa -= 360.0;
   else if(twa < -180.0)
      twa += 360.0;
}


// Calculate VMG (velocity made good)
vmg = stw * cos((-twa + leeway) * DEG_TO_RAD);


// Calculate WDIR (wind direction)
wdir = heading + twa;
if(wdir > 360.0)
   wdir -= 360.0;
else if(wdir < 0.0)
   wdir += 360.0;

// Calculate SOC (speed of current)
double cog_mag = cog + variation;
double alpha = (90.0 - (heading + leeway)) * DEG_TO_RAD;
double gamma = (90.0 - cog_mag) * DEG_TO_RAD;
double curr_x = sog * cos(gamma) - stw * cos(alpha);
double curr_y = sog * sin(gamma) - stw * sin(alpha);
soc = sqrt(curr_x * curr_x + curr_y * curr_y);

// Calculate DOC (direction of current)
double doc_cartesian = atan2(curr_y, curr_x);
if(isnan(doc_cartesian))
{
   if(curr_y < 0.0)
      doc = 180.0;
    else doc = 0.0;
}
else
{
   doc = 90.0 - doc_cartesian * RAD_TO_DEG;
   if(doc > 360.0)
      doc -= 360.0;
   else if(doc < 0.0)
     doc += 360.0;
}


Wednesday, April 27, 2011

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

Tuesday, April 26, 2011

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 (ax, ay, az) is used in the calculations, along with the current magnetometer result (mx, my, mz).

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.