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.
- ^ I am grateful to Edgar Bonet for helping me improve my original routines, and providing the approximations to cos, sqrt, and atan.
- ^ Haversine formula on Wikipedia.
- ^ 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