Pages: [1]   Go Down
Author Topic: Calculation of GPS and Compass  (Read 1532 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Full Member
***
Karma: 0
Posts: 196
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

Global Moderator
Netherlands
Online Online
Shannon Member
*****
Karma: 168
Posts: 12427
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


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:
//
//    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:
//
//    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

Logged

Rob Tillaart

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

Global Moderator
Netherlands
Online Online
Shannon Member
*****
Karma: 168
Posts: 12427
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

.cpp file

Code:
//
//    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;
}
Logged

Rob Tillaart

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

Offline Offline
Full Member
***
Karma: 0
Posts: 196
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 238
Posts: 24317
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code?
Logged

"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.

Offline Offline
Full Member
***
Karma: 0
Posts: 196
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 238
Posts: 24317
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

"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.

UK
Offline Offline
Shannon Member
****
Karma: 183
Posts: 11148
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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"?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Offline Offline
Full Member
***
Karma: 0
Posts: 196
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

by GPS heading i mean to say the Bearings from GPS
Logged

UK
Offline Offline
Shannon Member
****
Karma: 183
Posts: 11148
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Offline Offline
Full Member
***
Karma: 0
Posts: 196
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

i mean to say the calculated bearing through lat\long 
Logged

UK
Offline Offline
Shannon Member
****
Karma: 183
Posts: 11148
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Pages: [1]   Go Up
Jump to: