Calculation of GPS and Compass

How to make GPS and Compass calculation or programming for making the autonomous robot to drive straight in programming i am using GPS EM406a

and Compass HMC6352

The GPS will produce strings which include the latitude and longitude. You need to parse those strings to know your position and height.
IIRC the search word is NMEA

The Compass 6352 is a more difficult one to talk to. I have worked with one and it took several days to get reasonable readings from it. I started with a library for it but other issues came in between so it is not finished. This is the latest version I could find and it might be a starting point for you.

I recall that setting the different modes is not easy and at a certain moment the whole compass blocked. Took me long to get it running again, so the usage of the code below is at YOUR OWN risk. That means that all disclaimers apply, and that I won’t help you fix the code or the blocking compass.

The query mode is the simplest.

// 
//    FILE: querymode.pde
//  AUTHOR: Rob Tillaart
// VERSION: 0.1.00
// PURPOSE: test app HMC6352 library for Arduino
//
// Released to the public domain
//

#include <Wire.h>
#include <hmc6352.h>

hmc6352 Compass(0x21);

void setup()
{
  Serial.begin(115200);
  Serial.println("demo HMC6352: continuous mode");
  Serial.println(HMC_LIB_VERSION);
  
  Compass.setOperationalModus(CONT, 20, true);  // 20 Hz
  
  Serial.println(Compass.getOperationalModus());
  Compass.direction();  // first time
}

void loop()
{
  int x = Compass.qry();
  Serial.print("DEGREE: ");
  Serial.println(x);

  // Serial.println(Compass.getOperationalModus(), BIN);
  delay(50);  // 20 Hz
}

and the .h file

// 
//    FILE: hmc6352.h
//  AUTHOR: Rob Tillaart
// VERSION: 0.1.03
// PURPOSE: HMC6352 library for Arduino
//
// DETAILS: see cpp file
// 
// Released to the public domain
//

//#ifndef hmc6352_h
//#define hmc6352_h

#include "Wprogram.h"

#define HMC_LIB_VERSION 	"0.1.01"

#define HMC_GET_DATA		0x41
#define HMC_WAKE			0x57
#define HMC_SLEEP			0x53
#define HMC_SAVE_OP_MODE	0x4C
#define HMC_CALLIBRATE_ON	0x43
#define HMC_CALLIBRATE_OFF	0x45
#define HMC_UPDATE_OFFSETS	0x4F
#define HMC_WRITE_RAM		0x47
#define HMC_READ_RAM		0x67
#define HMC_WRITE_EEPROM	0x77
#define HMC_READ_EEPROM		0x72

enum hmcMode { STANDBY=0, QUERY=1, CONT=2, ERROR};

class hmc6352
{
	public:
    hmc6352(uint8_t device);

	// BASIC CALLS FOR STANDBY MODE
	int getHeading(void); // just a merge of ask & read
    int askHeading(void);
    int readHeading(void);

    int wakeUp(void);
    int sleep(void);

	// EXPERT CALLS
	int factoryReset();

	int setOperationalModus(hmcMode m, uint8_t freq, bool periodicReset);
	int getOperationalModus();

	int setOutputModus(uint8_t om);
	int getOutputModus();

	int callibrationOn(void);
	int callibrationOff(void);

	int setI2CAddress(uint8_t address);
	int getI2CAddress();

	int writeEEPROM(uint8_t address, uint8_t data);
	int readEEPROM(uint8_t address);
	
	int writeRAM(uint8_t address, uint8_t data);
	int readRAM(uint8_t address);


	// NOT TESTED / UNKNOWN
	int setTimeDelay(uint8_t msec);
	int getTimeDelay();
	int setMeasurementSumming(uint8_t ms);
	int getMeasurementSumming();
	int saveOpMode(void);  
	int updateOffsets(void);



  private:
	int cmd(uint8_t c);
	int readCmd(uint8_t c, uint8_t address);
	int writeCmd(uint8_t c, uint8_t address, uint8_t data);

	uint8_t _device;
};

//#endif

.cpp file

// 
//    FILE: hmc6352.cpp
//  AUTHOR: Rob Tillaart
// VERSION: 0.1.02
// PURPOSE: HMC6352 library for Arduino
//
// HISTORY: 
// 0.1.00 - 2011-04-07 initial version
// 0.1.01 - 2011-04-09 quite a complete redo 
// 0.1.02 - 2011-04-12 added timing, fixed a bug
// 0.1.03 - 2011-04-13 fixed small things; added getHeading()
// 
// Released to the public domain
//

#include <hmc6352.h>
#include <Wire.h>
#include "WProgram.h"

/* ERROR CODES ALL FUNCTIONS
//
// * twi_writeTo codes (== endTransmission  commands)
//   0 .. OK
//  -1 .. length to long for buffer
//  -2 .. address send, NACK received
//  -3 .. data send, NACK received
//  -4 .. other twi error (lost bus arbitration, bus error, ..)
//
// * requestFrom 
// -10 .. not enough values returned
//
// * function calls
//   0 .. OK
// -20 .. error param1
// -21 .. error param2
// -22 .. error param3
//
// 
*/

hmc6352::hmc6352(uint8_t device)
{
	Wire.begin();
	_device = constrain(device, 0x10, 0xF6);
}

int hmc6352::getHeading()
{
	int rv = askHeading();
	if (rv != 0) return rv; 
	return readHeading();
}

// Ask the device to make a new reading
int hmc6352::askHeading()
{
	int rv = cmd(HMC_GET_DATA);
	if (rv != 0) return -rv;   // problem with handshake
	delay(6); // see datasheet, p8
	return rv;
}

// read the last value from the
int hmc6352::readHeading()
{
	int rv = Wire.requestFrom(_device, (uint8_t)2);  // remove ambiguity
	if (rv != 2) return -10;
	rv = Wire.receive() * 256;  // MSB
	rv += Wire.receive();       // LSB
	return rv;
}

// wake up from energy saving modus
int hmc6352::wakeUp()
{
	int rv =  cmd(HMC_WAKE);
	delayMicroseconds(100);
	return rv;
}

// go into energy saving modus
int hmc6352::sleep()
{
	int rv = cmd(HMC_SLEEP);
	delayMicroseconds(10);
	return rv;
}

// values obtained from dump
int hmc6352::factoryReset()
{
	writeRAM(0x74, 0x50); // is needed !!
	writeCmd(HMC_WRITE_EEPROM, 0, 66);
	writeCmd(HMC_WRITE_EEPROM, 1, 0);
	writeCmd(HMC_WRITE_EEPROM, 2, 0);
	writeCmd(HMC_WRITE_EEPROM, 3, 0);
	writeCmd(HMC_WRITE_EEPROM, 4, 0);
	writeCmd(HMC_WRITE_EEPROM, 5, 1);
	writeCmd(HMC_WRITE_EEPROM, 6, 4);
	writeCmd(HMC_WRITE_EEPROM, 7, 6);
	writeCmd(HMC_WRITE_EEPROM, 8, 0x50);
	cmd(HMC_SAVE_OP_MODE);
	delayMicroseconds(125);
	return 0;
}

// HANDLE WITH CARE - RESTART NECESSARY
// Returns Operational Mode Config Byte
int hmc6352::setOperationalModus(hmcMode m, uint8_t freq, bool periodicReset)
{
	byte omcb = 0;  // Operational Mode Control Byte
	switch(freq)
	{
	case 1: break;
	case 5: omcb |= 0x20; break;
	case 10: omcb |= 0x40; break;
	case 20: omcb |= 0x60; break;
	default: return -21;
	}

	if (periodicReset) omcb |= 0x10;

	switch(m)
	{
	case STANDBY: break;  // omcb |= 0x00;
	case QUERY: omcb |= 0x01; break;
	case CONT: omcb |= 0x02; break;
	default: return -20;
	}

	writeCmd(HMC_WRITE_RAM, 0x74, omcb);
	cmd(HMC_SAVE_OP_MODE);
	delayMicroseconds(125);
	return omcb;
} 

// read the Operational Modus as byte from EEPROM
// TODO: split into 3 items
//
int hmc6352::getOperationalModus()
{
	// datasheet state that at startup the OM is copied from EEPROM
	// and that after writing to RAM a reboot is needed to enable new settings
	// my interpretation ==> EEPROM is leading
	return readCmd(HMC_READ_RAM, 0x74);
	//return readCmd(HMC_READ_EEPROM, 0x08);
}

// Switch between normal heading and raw modes
// default = 0 ==> normal headings
// Note: after a reboot the output modus will be 0 again.
int hmc6352::setOutputModus(uint8_t om)
{
	if (om > 4) return -20;
	return writeCmd(HMC_WRITE_RAM, 0x4E, om);
}

// Read the output modus from RAM
int hmc6352::getOutputModus()
{
	return readCmd(HMC_READ_RAM, 0x4E);
}

// NOT TESTED
int hmc6352::callibrationOn()
{
	int rv = cmd(HMC_CALLIBRATE_ON);
	delayMicroseconds(10);
	return rv;
}

// NOT TESTED
int hmc6352::callibrationOff()
{
	int rv = cmd(HMC_CALLIBRATE_OFF);
	delay(15);
	return rv;
}

// NOT TESTED
int hmc6352::setI2CAddress(uint8_t address)
{
	if (address < 0x10 || address > 0xF6 ) return -20;
	return writeCmd(HMC_WRITE_EEPROM, 0, address);
}

// returns current I2C address
int hmc6352::getI2CAddress()
{
	return readCmd(HMC_READ_EEPROM, 0);
}

// NOT TESTED
// meaning time delay unknown
// therefore removed from lib for now
int hmc6352::setTimeDelay(uint8_t msec)
{
	return writeCmd(HMC_WRITE_EEPROM, 5, msec);
}

int hmc6352::getTimeDelay()
{
	return readCmd(HMC_READ_EEPROM, 5);
}

// NOT TESTED
// meaning measurement summing unknown
// therefore removed from lib for now
int hmc6352::setMeasurementSumming(uint8_t ms)
{
	if (ms > 16 ) ms = 16;
	return writeCmd(HMC_WRITE_EEPROM, 6, ms);
}

int hmc6352::getMeasurementSumming()
{
	return readCmd(HMC_READ_EEPROM, 6);
}

// Makes only sense in setOperationalModus()
// therefore removed from lib for now
int hmc6352::saveOpMode()
{
	int rv = cmd(HMC_SAVE_OP_MODE);
	delayMicroseconds(125);
	return rv;
}


// NOT TESTED
// meaning UpdateOffsets unknown
// therefore removed from lib for now
int hmc6352::updateOffsets()
{
	int rv = cmd(HMC_UPDATE_OFFSETS);
	delay(6);
	return rv;
}

// idem
// use at own risk ...
int hmc6352::writeEEPROM(uint8_t address, uint8_t data)
{
	return writeCmd(HMC_WRITE_EEPROM, address, data);
}

// idem
int hmc6352::readEEPROM(uint8_t address)
{
	return readCmd(HMC_READ_EEPROM, address);
}


// idem
// Most RAM locations have an unknown meaning
// use at own risk ...
int hmc6352::writeRAM(uint8_t address, uint8_t data)
{
	return writeCmd(HMC_WRITE_RAM, address, data);
}
// idem
int hmc6352::readRAM(uint8_t address)
{
	return readCmd(HMC_READ_RAM, address);
}

/* PRIVATE FUNCTIONS
   mostly to remove redundancy in functions above
*/
int hmc6352::cmd(uint8_t c)
{
	Wire.beginTransmission(_device);
	Wire.send(c);
	int rv = Wire.endTransmission();
	delay(10);
	return rv;
}

int hmc6352::readCmd(uint8_t c, uint8_t address)
{
	Wire.beginTransmission(_device);
	Wire.send(c);
	Wire.send(address);
	int rv = Wire.endTransmission();
	if (rv != 0) return -rv;
	
	delayMicroseconds(70);
	
	rv = Wire.requestFrom(_device, (uint8_t)1);
	if (rv != 1) return -10; 
	rv = Wire.receive();
	return rv;
}

int hmc6352::writeCmd(uint8_t c, uint8_t address, uint8_t data)
{
	Wire.beginTransmission(_device);
	Wire.send(c); 
	Wire.send(address); 
	Wire.send(data); 
	int rv = Wire.endTransmission();
	delayMicroseconds(70);
	return rv;
}

My Robot works fine on Compass heading Problem is that i want to utilize the GPS heading so that it can move on Accurate direction

Code?

its my simple request kindly tell me the place where i am wrong

i am facing Problem in calculating the Heading of the GPS and then combining it with my compass so that my Robot moves straight towards the destination

#include <math.h>
#include <nmea.h>
#include <Wire.h>
#include <LiquidCrystal.h>
LiquidCrystal myLCD(42, 40, 38, 36, 34, 32);
float lat; 
float lon;

NMEA myGPS(GPRMC);

float dest_latitude = 33.693792;
float dest_longitude = 73.053170;

int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41;
const int potPin = A8;
const int leftMotor = 27; //PWM pin to the L293
const int rightMotor = 26; //PWM pin to the L293
int     motorA1=22;
int     motorA2=23;

char Dir;
bool isLeft()          {
  return (analogRead(potPin)/4) <85;
}
bool isLeftOfCenter()  {
  return (analogRead(potPin)/4) < 115;
}
bool isRightOfCenter() {
  return (analogRead(potPin)/4) > 147;
}
bool isRight()         {
  return (analogRead(potPin)/4) > 157;
}
int beeperpin=44;
void setup() {
  Serial.begin(9600);
  Serial1.begin(4800);
  myLCD.begin(16,2);
  Wire.begin();
  HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;
  pinMode(leftMotor,OUTPUT);
  pinMode(rightMotor,OUTPUT);
  pinMode(motorA1,OUTPUT);
  pinMode(motorA2,OUTPUT);
  myLCD.print("GPS BASED");
  delay(3000);
  myLCD.clear();
  myLCD.home();
  pinMode(13, OUTPUT); 
  pinMode(8, OUTPUT); 
  pinMode(6, OUTPUT); 
}
void loop() {
  if (Serial1.available() > 0 ) {
    if (myGPS.decode(Serial1.read())) {
      if (myGPS.gprmc_status() == 'A') {
        lat=myGPS.gprmc_latitude();
        lon = myGPS.gprmc_longitude();
        Serial.print("D:");
        float d=(round(myGPS.gprmc_distance_to(dest_latitude, dest_longitude, MTR)));
        Serial.print(d);
        Serial.print("m");
        Serial.print('\n');
        myLCD.setCursor(5, 0);
        myLCD.print(d);
        myLCD.print("m");
        if (d < 18){
          myLCD.clear();
          myLCD.home();
          myLCD.setCursor(5,0);
          myLCD.print("Destinat");
          digitalWrite(13, HIGH);
          digitalWrite(beeperpin, HIGH);
          carbreak();
          steering('C');
        }
        else if (d >= 18){
          digitalWrite(13, LOW);

          myLCD.clear();
          myLCD.home();
          myLCD.setCursor(5, 0);
          myLCD.print(d);
          myLCD.print("m");


          float currentdir = compassvalue();
          //myLCD.setCursor(4,2);
          //myLCD.print(currentdir);
          if (currentdir < 358 && currentdir > 200)
          {
            myLCD.setCursor(4,1);
            Serial.print ('<');
            // digitalWrite(8, HIGH);
            // digitalWrite(6, LOW);
            //myLCD.clear(); 
            myLCD.print ("LEFT");
            steering('L');
            delay(500);
            motorForward();
          }
          if (currentdir < 160 && currentdir > 2)
          {
            Serial.print ('>');    
            myLCD.setCursor(4,1);
            //digitalWrite(6, HIGH);
            //digitalWrite(8, LOW);
            //myLCD.clear(); 
            myLCD.print ("RIGHT"); 
            steering('R');
            delay(500);
            motorForward();
          }
          if (currentdir > 160 && currentdir < 200)
          {
            Serial.print ('=');
            myLCD.setCursor(4,1);
            //myLCD.clear();
            myLCD.print("center");
            //digitalWrite(6, LOW);
            //digitalWrite(8, LOW);
            steering('C');
            delay(300);
            motorForward();
          }
        }
      }
    }
  }
}
//  if (currentdir < 20 && currentdir > 2)
//  {
//    Serial.print ('=');
//    myLCD.setCursor(1,1);
//    myLCD.clear();
//    myLCD.print("center");
//    //digitalWrite(6, LOW);
//    //digitalWrite(8, LOW);
//    steering('C');
//  }


int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
  float calc;
  float bear_calc;

  float x = 69.1 * (flat2 - flat1); 
  float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

  calc=atan2(y,x);

  bear_calc= degrees(calc);

  if(bear_calc<=1){
    bear_calc=360+bear_calc; 
  }
  return bear_calc;
}
int compassvalue(){
  //"Get Data. Compensate and Calculate New Heading"
  Wire.beginTransmission(HMC6352SlaveAddress);
  Wire.send(HMC6352ReadAddress);              // The "Get Data" command
  Wire.endTransmission();

  //time delays required by HMC6352 upon receipt of the command
  //Get Data. Compensate and Calculate New Heading : 6ms
  delay(6);

  Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB

  //"The heading output data will be the value in tenths of degrees
  //from zero to 3599 and provided in binary format over the two bytes."
  byte MSB = Wire.receive();
  byte LSB = Wire.receive();

  float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
  float headingInt = headingSum / 10; 

  //Serial.print(headingInt);
  //Serial.println(" degrees");

  return headingInt;
}



void motorStop() {
  digitalWrite(leftMotor, LOW); 
  digitalWrite(rightMotor, LOW);
}
void motorLeft() {
  digitalWrite(leftMotor, HIGH);
}
void motorRight() {
  digitalWrite(rightMotor, HIGH);
}

//back wheels
void motorForward()
{
  digitalWrite(motorA1,HIGH);
  digitalWrite(motorA2,LOW);
}

void motorBackward()
{
  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,HIGH);
}


void carbreak()
{

  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,LOW);
}
void steering(char Dir)
{
  //if (Serial.available() == 0)
  //return;
  //Dir=toupper(Serial.read());
  switch (Dir)
  {
  case 'L':
    //Serial.println("New command: L");
    if (isLeft())
    {
    }//Serial.println("Already Left");
    else
    {
      //Serial.println("Heading Left");
      motorLeft();
      while (!isLeft()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Left");
    }
    break;

  case 'C':
    //Serial.println("New command: C");
    if (isRightOfCenter())
    {
      //Serial.println("Heading Left toward Center");
      motorLeft();
      while (isRightOfCenter()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Centered");
    }
    else
      if (isLeftOfCenter())
      {
        //Serial.println("Heading Right toward Center");
        motorRight();
        while (isLeftOfCenter()) /* JUST WAITING */;
        motorStop();
        //Serial.println("Is now Centered");
      }
      else
      {
      }
    //Serial.println("Already Centered");
    break;
  case 'R':
    //Serial.println("New command: R");
    if (isRight())
    {
    }//Serial.println("Already Right");
    else
    {
      //Serial.println("Heading Right");
      motorRight();
      while (!isRight()) /* JUST WAITING */;
      motorStop();
      //Serial.println("Is now Right");
    }
    break;
  }  // end of switch on direction
}

You seem to think we have some magic debug wand and mystical psychic powers that enable us to see what your debug output looks like, and how your system behaves.

For the very last time - we do not.

That is where you are going wrong.

cutebuddy6: My Robot works fine on Compass heading Problem is that i want to utilize the GPS heading so that it can move on Accurate direction

I don't understand what this means, please clarify: what do you mean by "GPS heading"?

by GPS heading i mean to say the Bearings from GPS

cutebuddy6: by GPS heading i mean to say the Bearings from GPS

I understand how you get heading information from a compass, but I don't understand how you get it from a GPS receiver - unless the receiver also has a compass built in which you are using to get the heading. Can you clarify exactly what you mean by GPS heading?

i mean to say the calculated bearing through lat\long

cutebuddy6: i mean to say the calculated bearing through lat\long

That makes more sense. So you use GPS to find your current lat/long, and have somehow got the lat/long of the place you want to go towards. Is that the idea?

In order to get there you really need to know which way you're pointing as well as where you are. The algorithm would be something like:

Find current position from GPS. Calculate desired direction of travel to the waypoint. Find orientation of vehicle from compass. Calculate angular offset between vehicle orientation and desired direction of travel. Calculate steering position to steer towards the desired orientation. Steering position should be based on the angular offset so that the amount of steering reduces as the angular offset reduces, until eventually the vehicle is travelling in a straight line towards the waypoint. If the vehicle is going to be travelling at high speed, you should scale the steering angle according to the speed to prevent the system from being oversensitive at high speed.