# 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_WRITE_EEPROM	0x77

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

class hmc6352
{
public:
hmc6352(uint8_t device);

// BASIC CALLS FOR STANDBY MODE

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

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

{
if (rv != 0) return rv;
}

{
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 rv = Wire.requestFrom(_device, (uint8_t)2);  // remove ambiguity
if (rv != 2) return -10;
rv = Wire.receive() * 256;  // MSB
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
}

// 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()
{
}

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

{
}

// 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()
{
}

// 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()
{
}

// 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 ...
{
}

// idem
{
}

// idem
// Most RAM locations have an unknown meaning
// use at own risk ...
{
}
// idem
{
}

/* 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;
}

{
Wire.beginTransmission(_device);
Wire.send(c);
int rv = Wire.endTransmission();
if (rv != 0) return -rv;

delayMicroseconds(70);

rv = Wire.requestFrom(_device, (uint8_t)1);
if (rv != 1) return -10;
return rv;
}

int hmc6352::writeCmd(uint8_t c, uint8_t address, uint8_t data)
{
Wire.beginTransmission(_device);
Wire.send(c);
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;

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()          {
}
bool isLeftOfCenter()  {
}
bool isRightOfCenter() {
}
bool isRight()         {
}
int beeperpin=44;
void setup() {
Serial.begin(9600);
Serial1.begin(4800);
myLCD.begin(16,2);
Wire.begin();
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.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.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."

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

//Serial.println(" degrees");

}

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;
switch (Dir)
{
case 'L':
//Serial.println("New command: L");
if (isLeft())
{
else
{
motorLeft();
while (!isLeft()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Left");
}
break;

case 'C':
//Serial.println("New command: C");
if (isRightOfCenter())
{
motorLeft();
while (isRightOfCenter()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Centered");
}
else
if (isLeftOfCenter())
{
motorRight();
while (isLeftOfCenter()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Centered");
}
else
{
}
break;
case 'R':
//Serial.println("New command: R");
if (isRight())
{
else
{
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.