For months i have been slowly investigating and "developing" an accurate, efficient nav code (for slow power 8-bit uControllers), finally i just finish my new navigation algorithm, is super faster and smaller.... =).

Before the functions were like this:

int get_gps_course(float flat1, float flon1, float flat2, float flon2)
{
float calc;
float calc2;
float bear_calc;
float diflon;
//I've to spplit all the calculation in several steps. If i try it to do it in a single line the arduino will explode.
flat1=radians(flat1);
flat2=radians(flat2);

diflon=radians((flon2)-(flon1));

calc=sin(diflon)*cos(flat2);
calc2=cos(flat1)*sin(flat2)-sin(flat1)*cos(flat2)*cos(diflon);

calc=atan2(calc,calc2);

bear_calc= degrees(calc);

if(bear_calc<=1){
bear_calc=360+bear_calc;
}
return bear_calc;
/********************************************************************************/
}
unsigned int get_gps_dist(float flat1, float flon1, float flat2, float flon2)
{
float dist_calc=0;
float dist_calc2=0;
float diflat=0;
float diflon=0;

//I've to spplit all the calculation in several steps. If i try to do it in a single line the arduino will explode.
diflat=radians(flat2-flat1);
flat1=radians(flat1);
flat2=radians(flat2);
diflon=radians((flon2)-(flon1));

dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
dist_calc2= cos(flat1);
dist_calc2*=cos(flat2);
dist_calc2*=sin(diflon/2.0);
dist_calc2*=sin(diflon/2.0);
dist_calc +=dist_calc2;

dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

dist_calc*=6371000.0; //Converting to meters, i love the metric system.. =)
return dist_calc;
}


Now are like this:

int get_gps_course(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;
}
/********************************************************************************/
unsigned int get_gps_dist(float flat1, float flon1, float flat2, float flon2)
{
float x = 69.1 * (flat2 - flat1);
float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);
return (float)sqrt((float)(x*x) + (float)(y*y))*1609.344;
}


And if you wonder the difference i have compared them side by side and recorded it.

The results are here:

The precision is exactly the same, only if you go more than 8 km away, the distance starts to get dirty... But good enough for our purposes... =)

Views: 826

Comment by Michal B on February 2, 2009 at 12:09am
It looks very nice Jordi! I'm going to test these.
Comment by Jhon on February 2, 2009 at 10:59am
congrats!
but that means at more than 8 km the precision to reach a waypoint will be not accurate? im a bit confused,

Developer
Comment by Jordi Muñoz on February 2, 2009 at 11:12am
Yeap, for about 8 Kilometers the error is about 10 meters.. You can check it in the log file included...
Comment by Magnus on February 2, 2009 at 12:17pm
Jhon,
What it means is that if you have a leg between two waypoints that is longer than a couple of km their will be an error in the initial angle the planes fly to and the calcluated distance to next waypoint, but the closer the waypoint it gets the smaller the error. So as long as each leg isn't to long you can still have a a route that has unlimited length.

Could someone please correct me if I'm wrong!

/Magnus
Comment by Jhon on February 2, 2009 at 2:26pm
still confused, in the log i see a difference of 1~2 meters when looking at 8000 meters, where is the diference of 10 meters at 8000 meters?

82 82 8010 8012
82 82 8101 8103
82 82 8193 8194
82 82 8284 8286

Developer
Comment by Jordi Muñoz on February 3, 2009 at 4:27pm
Jhon, i remember i saw an error of meter, maybe i got confuse. Even better!

Jay, Yes the cos is to compensate the longitude. The latitude is always the same.. .
Comment by JimEli on February 4, 2009 at 11:25am
Would this be more efficient?

//calculate rhumbline distance in nautical miles
if (cos(true_course) == 0.0)
distance = 60.0 * (point2.longitude - point1.longitude) * cos(point1.latitude);
else
distance = 60.0 * ((point2.latitude - point1.latitude) / cos(true_course));

Comment

You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service