  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.

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.

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

Join DIY Drones

Top Discussions

1 Most important features for a power module ?

2 Wall Printing Drone

3 IS "AIRBORNE PROJECTS" STILL IN BUSINESs?

4 ESC Setting Issue

5 How big is the impact of 5G on drones?

6 Chinese drones are doing something impossible

7 OpenUAV open source fixed wing platform

8 The drone turned into a fire extinguisher and staged "Motorish Rescue"

9 10 Groups

631 members

259 members

281 members

1470 members

• ArduCopter User Group

3164 members

A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here