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

No comments:

Post a Comment