Developer

New ArduPilot Pocket Navigation Algorithm

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... =)
E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones

Comments

  • 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));
  • Developer
    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.. .
  • 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
  • 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
  • Developer
    Yeap, for about 8 Kilometers the error is about 10 meters.. You can check it in the log file included...
  • congrats!
    but that means at more than 8 km the precision to reach a waypoint will be not accurate? im a bit confused,
  • It looks very nice Jordi! I'm going to test these.
This reply was deleted.