2.5/boards/ek-lm3s1968/examples/gps/compass.c

   1 #include <cfg/compiler.h>
   2 #include <cfg/debug.h>
   3 #include <math.h>
   4 #include "compass.h"
   5
   6 static const char *_compass_heading[] =
   7 {
   8         "N", "NNE", "NE", "ENE",
   9         "E", "ESE", "SE", "SSE",
  10         "S", "SSW", "SW", "WSW",
  11         "W", "WNW", "NW", "NNW",
  12 };
  13
  14 /**
  15  * Use the Haversine formula to calculate great-circle distances between the
  16  * two points.
  17  *
  18  * The Haversine formula remains particularly well-conditioned for numerical
  19  * computation even at small distances, unlike calculations based on the
  20  * spherical law of cosines.
  21  */
  22 float distance(float lat1, float lon1, float lat2, float lon2)
  23 {
  24         const float PLANET_RADIUS = 6371000;
  25         float d_lat = deg2rad(lat2 - lat1);
  26         float d_lon = deg2rad(lon2 - lon1);
  27
  28         float a = sin(d_lat / 2) * sin(d_lat / 2) +
  29                         cos(deg2rad(lat1)) * cos(deg2rad(lat2)) *
  30                         sin(d_lon / 2) * sin(d_lon / 2);
  31         float c = 2 * atan2(sqrt(a), sqrt(1 - a));
  32
  33         return PLANET_RADIUS * c;
  34 }
  35
  36 /**
  37  * Evaluate the bearing (also known as forward azimuth) using spherical law
  38  * coordinates.
  39  *
  40  * The bearing is a straight line along a great-circle arc from the start point
  41  * to the destination point.
  42  */
  43 int bearing(float lat1, float lon1, float lat2, float lon2)
  44 {
  45         float res;
  46
  47         res = rad2deg(atan2(sin(deg2rad(lon2 - lon1)) *
  48                 cos(deg2rad(lat2)), cos(deg2rad(lat1)) *
  49                 sin(deg2rad(lat2)) - sin(deg2rad(lat1)) *
  50                 cos(deg2rad(lat2)) * cos(deg2rad(lon2) -
  51                 deg2rad(lon1))));
  52         return ((int)res + 360) % 360;
  53 }
  54
  55 const char *compass_heading(int bearing)
  56 {
  57         ASSERT(bearing >= 0 && bearing < 360);
  58         /*
  59          * bearing / 22.5
  60          */
  61         return _compass_heading[(bearing << 4) / 360];
  62 }