Go Down

### Topic: Calculation of GPS and Compass (Read 3552 times)previous topic - next topic

#### cutebuddy6

##### Apr 20, 2012, 04:07 pm
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

#### robtillaart

#1
##### Apr 21, 2012, 06:51 pm

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.
Code: [Select]
`// //    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

Code: [Select]
`// //    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 0x72enum 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`

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### robtillaart

#2
##### Apr 21, 2012, 06:52 pm
.cpp file

Code: [Select]
`// //    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 readingint 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 theint 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 modusint hmc6352::wakeUp(){ int rv =  cmd(HMC_WAKE); delayMicroseconds(100); return rv;}// go into energy saving modusint hmc6352::sleep(){ int rv = cmd(HMC_SLEEP); delayMicroseconds(10); return rv;}// values obtained from dumpint 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 Byteint 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 RAMint hmc6352::getOutputModus(){ return readCmd(HMC_READ_RAM, 0x4E);}// NOT TESTEDint hmc6352::callibrationOn(){ int rv = cmd(HMC_CALLIBRATE_ON); delayMicroseconds(10); return rv;}// NOT TESTEDint hmc6352::callibrationOff(){ int rv = cmd(HMC_CALLIBRATE_OFF); delay(15); return rv;}// NOT TESTEDint hmc6352::setI2CAddress(uint8_t address){ if (address < 0x10 || address > 0xF6 ) return -20; return writeCmd(HMC_WRITE_EEPROM, 0, address);}// returns current I2C addressint hmc6352::getI2CAddress(){ return readCmd(HMC_READ_EEPROM, 0);}// NOT TESTED// meaning time delay unknown// therefore removed from lib for nowint 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 nowint 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 nowint hmc6352::saveOpMode(){ int rv = cmd(HMC_SAVE_OP_MODE); delayMicroseconds(125); return rv;}// NOT TESTED// meaning UpdateOffsets unknown// therefore removed from lib for nowint 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);}// idemint 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);}// idemint 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;}`
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### cutebuddy6

#3
##### Apr 29, 2012, 07:47 pm
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

#### AWOL

#4
##### Apr 29, 2012, 10:38 pm
Code?
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

#### cutebuddy6

#5
##### Apr 30, 2012, 06:06 pm
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

Code: [Select]
`#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 L293const int rightMotor = 26; //PWM pin to the L293int     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 wheelsvoid 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}`

#### AWOL

#6
##### Apr 30, 2012, 06:18 pm
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.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

#### PeterH

#7
##### Apr 30, 2012, 07:28 pm

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"?
I only provide help via the forum - please do not contact me for private consultancy.

#### cutebuddy6

#8
##### Apr 30, 2012, 10:09 pm
by GPS heading i mean to say the Bearings from GPS

#### PeterH

#9
##### May 01, 2012, 01:54 am

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 only provide help via the forum - please do not contact me for private consultancy.

#### cutebuddy6

#10
##### May 01, 2012, 10:35 am
i mean to say the calculated bearing through lat\long

#### PeterH

#11
##### May 01, 2012, 01:38 pm

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.
I only provide help via the forum - please do not contact me for private consultancy.

Go Up

Please enter a valid email to subscribe