Topics

► Games

► Sound & Music

► Watches & Clocks

► GPS

► Power Supplies

► Computers

► Graphics

► Thermometers

► Tools

► Tutorials

By processor

► ATtiny10

► ATtiny2313

► ATtiny3216

► ATtiny402

► ATtiny84

► ATtiny841

► ATtiny85

► ATtiny861

► ATtiny88

► ATmega328

► ATmega1284

► ATSAMD21

About me

  • About me

Feeds

RSS feed

A Simple GPS Library

This article describes some GPS routines designed for small size and efficient execution on small processors, such as the ATtiny85, when working with distances of up to a few hundred km. They use long integer arithmetic, to avoid the need for floating-point, and they use approximations to atan, cos, and sqrt, to avoid the need to load these functions.

The routines are used in my project TinyNav Simple GPS Navigator [Updated].

Units

These routines work in units of 1e-4 arc minutes. Thus one degree is represented as 600,000 units:

const long DEGREE = 600000;

These units allow easy and exact conversion from the NMEA sentences provided by GPS modules, and fit conveniently into a long integer. The largest number we need to represent is 360° represented as 216000000, which fits in a long interger. The resolution is 1e-4 arc minutes, equivalent to about 0.2m at the equator.

Difference

The first routine provides the difference between two angular measures, the shortest way round the earth; the result is -180 * DEGREE ≤ diff ≤ 180 * DEGREE.

long Diff (long deg1, long deg2) {
  long result = deg2 - deg1;
  if (result > 180 * DEGREE) return result - 360 * DEGREE;
  else if (result < -180 * DEGREE) return result + 360 * DEGREE;
  else return result;
}

Approximation to cos

The following routines use the following approximate formula for cos [1]:

cos(π/2 ⋅ x) ≈ 0.962 ⋅ (1 – x2), valid for abs(angle) ≤ 90°.

This is implemented in C as:

unsigned int CosFix (long angle) {
  unsigned long u = labs(angle)>>16;
  u = (u * u * 6086)>>24;
  return 246 - u;
}

The result is scaled by a factor 28 (range: 0..246). The constant 6086 in the function is:

0.962 ⋅  264/(90 ⋅ DEGREE)2.

Distance between two points

The standard method of calculating the shortest distance over the earth’s surface between two points is the haversine formula [2]. However, for small distances we can assume that the surface of the earth is flat and use Pythagoras’s theorem. The formula is:

x = Δλ ⋅ cos (φ1 + φ2)/2

y = Δφ

d = R ⋅ √(x2 + y2)

where φ is the latitude in radians, λ is the longitude in radians, and R is the earth's mean radius, 6371km.

We can use the above approximation to cos. We can also avoid sqrt by using an approximate formula for h = √(x2 + y2):

h ≈ y + 0.428 ⋅ x2 / y , assuming  0 ≤ x ≤ y.

unsigned int DistanceBetween (long lat1, long long1, long lat2, long long2) {
  long dx = (Diff(long2, long1) * CosFix((lat1 + lat2)/2)) / 256;
  long dy = Diff(lat2, lat1);
  unsigned long adx = labs(dx);
  unsigned long ady = labs(dy);
  unsigned long b = max(adx, ady);
  unsigned long a = min(adx, ady);
  if (b == 0) return 0;
  return 95 * (b + (110 * a / b * a + 128) / 256) / 512;
}

Here 110/256 is an approximation to 0.428, and 95/512 is the conversion factor between the angular units of latitude and meters, an approximation to (R ⋅ 2π)/(360 ⋅ DEGREE). The divisors are chosen as powers of 2 so the compiler can optimise them to right shifts [3].

Initial bearing between two points

The initial bearing from one point to another is given by the formula:

θ = atan2( sin Δλ ⋅ cos φ2 , cos φ1 ⋅ sin φ2 − sin φ1 ⋅ cos φ2 ⋅ cos Δλ )

where φ is the latitude in radians, and λ is the longitude in radians, and Δλ is λ2 – λ1.

For small angles we can assume that the surface of the earth is flat, to get:

θ = atan2( Δλ ⋅ cos φ2 , Δφ )

We can use the above approximation to cos. We can also use the following approximation to atan:

atan(x) ≈ x ⋅ (45 + 16 (1 − x)), for 0 < x < 1, result in degrees.

The resulting routine is:

unsigned int CourseTo (long lat1, long long1, long lat2, long long2) {
  int c;
  long dx = (Diff(long2, long1) * CosFix((lat1 + lat2)/2)) / 256;
  long dy = Diff(lat2, lat1);
  long adx = labs(dx);
  long ady = labs(dy);
  if (adx == 0) c = 0;
  else if (adx < ady) c = (adx * (45 + (16 * (ady - adx))/ady))/ady;
  else c = 90 - (ady * (45 + (16 * (adx - ady))/adx))/adx;
  //
  if (dx <= 0 && dy < 0) return c;
  else if (dx < 0 && dy >= 0) return 180 - c;
  else if (dx >= 0 && dy >= 0) return 180 + c;
  else return 360 - c;
}

 This returns the bearing as an integer from 0 to 359.


  1. ^ I am grateful to Edgar Bonet for helping me improve my original routines, and providing the approximations to cos, sqrt, and atan.
  2. ^ Haversine formula on Wikipedia.
  3. ^ Edgar Bonet has confirmed that for unsigned numbers the compiler optimises division by a power of two to a right shift, but for signed numbers it leaves it as a division.

blog comments powered by Disqus