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... =)
You need to be a member of DIY Drones to add comments!
Join DIY Drones