Arduino follow me with GPS

i'm making project purpose build robot follow me. in order this, began basics, made app android 4 buttons control direction of robot via wifi using esp8266-01 connected arduino, connected 2 motors, , worked.
then have added app location listener , compass reading, placing 1 smartphone on robot , in hand, send latitude, longitude , compass angle arduino, use them determinate how long has go , direction has turn. calculation of distance between 2 works approximately well, whith error of 6 meters, have trouble direction. @ beginning stupidly used formula:

code: [select]

float y = latphone - latard;
  float x = lonphone - lonard;
  dirphone = 2 * atan( y / ( sqrt(x * x + y * y) + x)); //value in radians
  dirphone = dirphone * 180 / m_pi; //to degree


but have thought "hey, lat , lon not catetes angles!" searched , found formula hackerhouse project, there cooler follow person, want:

code: [select]


  float y = sin(lonphone-lonard) * cos(latphone);
  float x = cos(latard)*sin(latphone) - sin(latard)*cos(latphone)*cos(lonphone-lonard);
  difdir = atan2(y, x) * 180 / m_pi;

but not worked...! has tryied similar , can enlight me mistake? thanks. (in attachment arduino sketch use)

quote

but not worked...!

post actual input , output numbers.

over short distances, use pythagoras theorem (the "equirectangular approximation") calculate distances. on avr-based arduinos, double same float. error in single precision arduino floating point calculations large use spherical trigonometry.

excellent reference on navigation formulas here.

Arduino Forum > Topics > Robotics (Moderator: fabioc84) > Arduino follow me with GPS

arduino