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

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 . 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);
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 .

### 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);
if (adx == 0) c = 0;
//
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.