altos: Switch APRS to compressed position format

This provides lat/lon/alt in fewer bytes while improving precision.

Signed-off-by: Keith Packard <keithp@keithp.com>

9 files changed, 174 insertions, 14 deletions

diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c
index 6ab61e6a..96e90f00 100644
--- a/src/drivers/ao_aprs.c
+++ b/src/drivers/ao_aprs.c
@@ -144,6 +144,7 @@
 #endif
 
 #include <ao_aprs.h>
+#include <math.h>
 
 // Public methods, constants, and data structures for each class.
 
@@ -254,9 +255,9 @@ typedef enum
 /// AX.25 compliant packet header that contains destination, station call sign, and path.
 /// 0x76 for SSID-11, 0x78 for SSID-12
 static uint8_t TNC_AX25_HEADER[] = { 
-    'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60, \
-    'N' << 1, '0' << 1, 'C' << 1, 'A' << 1, 'L' << 1, 'L' << 1, 0x78, \
-    'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65, \
+    'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60,
+    'N' << 1, '0' << 1, 'C' << 1, 'A' << 1, 'L' << 1, 'L' << 1, 0x78,
+    'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65,
     0x03, 0xf0 };
 
 #define TNC_CALLSIGN_OFF	7
@@ -479,6 +480,14 @@ static void tnc1200TimerTick()
     } // END switch
 }
 
+static void tncCompressInt(uint8_t *dest, int32_t value, int len) {
+	int i;
+	for (i = len - 1; i >= 0; i--) {
+		dest[i] = value % 91 + 33;
+		value /= 91;
+	}
+}
+
 /**
  *   Generate the plain text position packet.
  */
@@ -487,7 +496,14 @@ static int tncPositionPacket(void)
     int32_t	latitude = ao_gps_data.latitude;
     int32_t	longitude = ao_gps_data.longitude;
     int32_t	altitude = ao_gps_data.altitude;
+    uint8_t	*buf;
 
+    if (altitude < 0)
+	altitude = 0;
+    altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048;
+    
+#if 0
+    char	lat_sign = 'N', lon_sign = 'E';
     uint16_t	lat_deg;
     uint16_t	lon_deg;
     uint16_t	lat_min;
@@ -495,8 +511,6 @@ static int tncPositionPacket(void)
     uint16_t	lon_min;
     uint16_t	lon_frac;
 
-    char	lat_sign = 'N', lon_sign = 'E';
-
     if (latitude < 0) {
 	lat_sign = 'S';
 	latitude = -latitude;
@@ -529,15 +543,56 @@ static int tncPositionPacket(void)
     longitude -= lon_min * 10000000;
     lon_frac = longitude / 100000;
 
-    if (altitude < 0)
-	altitude = 0;
-
-    altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048;
-    
+#if 0
     return sprintf ((char *) tncBuffer, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015",
 		    lat_deg, lat_min, lat_frac, lat_sign,
 		    lon_deg, lon_min, lon_frac, lon_sign,
 		    altitude);
+#endif
+
+    return sprintf ((char *) tncBuffer, "/%02u%02u%02uh%02u%02u.%02u%c/%03u%02u.%02u%c'/A=%06u\015",
+		    ao_gps_data.hour,
+		    ao_gps_data.minute,
+		    ao_gps_data.second,
+		    lat_deg, lat_min, lat_frac, lat_sign,
+		    lon_deg, lon_min, lon_frac, lon_sign,
+		    altitude);
+#endif
+    buf = tncBuffer;
+#if APRS_TIME
+    sprintf ((char *) buf, "/%02u%02u%02uh",
+	     ao_gps_data.hour,
+	     ao_gps_data.minute,
+	     ao_gps_data.second);
+    buf += 8;
+#else
+    *buf++ = '!';
+#endif
+
+    /* Symbol table ID */
+    *buf++ = '/';
+
+    latitude = ((uint64_t) 380926 * (900000000 - latitude)) / 10000000;
+    longitude = ((uint64_t) 190463 * (1800000000 + longitude)) / 10000000;
+
+#define ALTITUDE_LOG_BASE	0.001998002662673f	/* log(1.002) */
+
+    altitude = logf((float) altitude) * (1/ALTITUDE_LOG_BASE);
+
+    tncCompressInt(buf, latitude, 4);
+    buf += 4;
+    tncCompressInt(buf, longitude, 4);
+    buf += 4;
+
+    /* Symbol code */
+    *buf++ = '\'';
+
+    tncCompressInt(buf, altitude, 2);
+    buf += 2;
+
+    *buf++ = 33 + ((1 << 5) | (2 << 3));
+    *buf++ = '\0';
+    return buf - tncBuffer;
 }
 
 static int16_t
diff --git a/src/lpc/Makefile-lpc.defs b/src/lpc/Makefile-lpc.defs
index dd2bf0e6..fecb9135 100644
--- a/src/lpc/Makefile-lpc.defs
+++ b/src/lpc/Makefile-lpc.defs
@@ -4,7 +4,7 @@ endif
 
 include $(TOPDIR)/Makedefs
 
-vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR)
+vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR):$(TOPDIR)/math
 vpath make-altitude $(TOPDIR)/util
 vpath make-kalman $(TOPDIR)/util
 vpath kalman.5c $(TOPDIR)/kalman
@@ -28,7 +28,7 @@ CC=$(ARM_CC)
 
 WARN_FLAGS=-Wall -Wextra -Werror
 
-AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) 
+AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) -I$(TOPDIR)/math -I$(TOPDIR) $(PDCLIB_INCLUDES) 
 LPC_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m0 -mthumb\
 	-ffreestanding -nostdlib $(AO_CFLAGS) $(WARN_FLAGS)
 
diff --git a/src/math/ef_log.c b/src/math/ef_log.c
new file mode 100644
index 00000000..619fe909
--- /dev/null
+++ b/src/math/ef_log.c
@@ -0,0 +1,92 @@
+/* ef_log.c -- float version of e_log.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+ln2_hi =   6.9313812256e-01,	/* 0x3f317180 */
+ln2_lo =   9.0580006145e-06,	/* 0x3717f7d1 */
+two25 =    3.355443200e+07,	/* 0x4c000000 */
+Lg1 = 6.6666668653e-01,	/* 3F2AAAAB */
+Lg2 = 4.0000000596e-01,	/* 3ECCCCCD */
+Lg3 = 2.8571429849e-01, /* 3E924925 */
+Lg4 = 2.2222198546e-01, /* 3E638E29 */
+Lg5 = 1.8183572590e-01, /* 3E3A3325 */
+Lg6 = 1.5313838422e-01, /* 3E1CD04F */
+Lg7 = 1.4798198640e-01; /* 3E178897 */
+
+#ifdef __STDC__
+static const float zero   =  0.0;
+#else
+static float zero   =  0.0;
+#endif
+
+#ifdef __STDC__
+	float __ieee754_logf(float x)
+#else
+	float __ieee754_logf(x)
+	float x;
+#endif
+{
+	float hfsq,f,s,z,R,w,t1,t2,dk;
+	__int32_t k,ix,i,j;
+
+	GET_FLOAT_WORD(ix,x);
+
+	k=0;
+	if (FLT_UWORD_IS_ZERO(ix&0x7fffffff))
+	    return -two25/zero;		/* log(+-0)=-inf */
+        if (ix<0) return (x-x)/zero;	/* log(-#) = NaN */
+	if (!FLT_UWORD_IS_FINITE(ix)) return x+x;
+	if (FLT_UWORD_IS_SUBNORMAL(ix)) {
+	    k -= 25; x *= two25; /* subnormal number, scale up x */
+	    GET_FLOAT_WORD(ix,x);
+	} 
+	k += (ix>>23)-127;
+	ix &= 0x007fffff;
+	i = (ix+(0x95f64<<3))&0x800000;
+	SET_FLOAT_WORD(x,ix|(i^0x3f800000));	/* normalize x or x/2 */
+	k += (i>>23);
+	f = x-(float)1.0;
+	if((0x007fffff&(15+ix))<16) {	/* |f| < 2**-20 */
+           if(f==zero) { if(k==0) return zero;  else {dk=(float)k;
+                                return dk*ln2_hi+dk*ln2_lo;}}
+	    R = f*f*((float)0.5-(float)0.33333333333333333*f);
+	    if(k==0) return f-R; else {dk=(float)k;
+	    	     return dk*ln2_hi-((R-dk*ln2_lo)-f);}
+	}
+ 	s = f/((float)2.0+f); 
+	dk = (float)k;
+	z = s*s;
+	i = ix-(0x6147a<<3);
+	w = z*z;
+	j = (0x6b851<<3)-ix;
+	t1= w*(Lg2+w*(Lg4+w*Lg6)); 
+	t2= z*(Lg1+w*(Lg3+w*(Lg5+w*Lg7))); 
+	i |= j;
+	R = t2+t1;
+	if(i>0) {
+	    hfsq=(float)0.5*f*f;
+	    if(k==0) return f-(hfsq-s*(hfsq+R)); else
+		     return dk*ln2_hi-((hfsq-(s*(hfsq+R)+dk*ln2_lo))-f);
+	} else {
+	    if(k==0) return f-s*(f-R); else
+		     return dk*ln2_hi-((s*(f-R)-dk*ln2_lo)-f);
+	}
+}
diff --git a/src/math/fdlibm.h b/src/math/fdlibm.h
index 821619ad..ee9fcb22 100644
--- a/src/math/fdlibm.h
+++ b/src/math/fdlibm.h
@@ -19,6 +19,7 @@
 
 #define __ieee754_acosf acosf
 #define __ieee754_sqrtf sqrtf
+#define __ieee754_logf logf
 
 /* REDHAT LOCAL: Include files.  */
 #include <math.h>
diff --git a/src/math/math.h b/src/math/math.h
index fd543bc2..97dd74cf 100644
--- a/src/math/math.h
+++ b/src/math/math.h
@@ -34,4 +34,6 @@ float scalbnf(float x, int n);
 
 float copysignf(float x, float y);
 
+float logf(float x);
+
 #endif
diff --git a/src/telegps-v0.3/Makefile b/src/telegps-v0.3/Makefile
index bb9c8c64..5aad32b5 100644
--- a/src/telegps-v0.3/Makefile
+++ b/src/telegps-v0.3/Makefile
@@ -19,6 +19,9 @@ INC = \
 	Makefile
 
 
+MATH_SRC=\
+	ef_log.c
+
 ALTOS_SRC = \
 	ao_interrupt.c \
 	ao_boot_chain.c \
@@ -47,6 +50,7 @@ ALTOS_SRC = \
 	ao_log.c \
 	ao_log_mega.c \
 	ao_gps_report_mega.c \
+	$(MATH_SRC) \
 	$(SAMPLE_PROFILE)
 
 PRODUCT=TeleGPS-v0.3
diff --git a/src/telemega-v0.1/Makefile b/src/telemega-v0.1/Makefile
index 0145f49c..35f28b30 100644
--- a/src/telemega-v0.1/Makefile
+++ b/src/telemega-v0.1/Makefile
@@ -58,7 +58,8 @@ MATH_SRC=\
 	sf_sin.c \
 	sf_fabs.c \
 	sf_floor.c \
-	sf_scalbn.c
+	sf_scalbn.c \
+	ef_log.c
 
 ALTOS_SRC = \
 	ao_boot_chain.c \
diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile
index 543f7e74..b5c1f402 100644
--- a/src/telemega-v1.0/Makefile
+++ b/src/telemega-v1.0/Makefile
@@ -59,7 +59,8 @@ MATH_SRC=\
 	sf_fabs.c \
 	sf_floor.c \
 	sf_scalbn.c \
-	sf_sin.c
+	sf_sin.c \
+	ef_log.c
 
 ALTOS_SRC = \
 	ao_boot_chain.c \
diff --git a/src/telemetrum-v2.0/Makefile b/src/telemetrum-v2.0/Makefile
index cebc9cab..0b9e6914 100644
--- a/src/telemetrum-v2.0/Makefile
+++ b/src/telemetrum-v2.0/Makefile
@@ -37,6 +37,9 @@ INC = \
 #STACK_GUARD=ao_mpu_stm.c
 #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1
 
+MATH_SRC=\
+	ef_log.c
+
 ALTOS_SRC = \
 	ao_boot_chain.c \
 	ao_interrupt.c \
@@ -83,6 +86,7 @@ ALTOS_SRC = \
 	ao_packet.c \
 	ao_companion.c \
 	ao_aprs.c \
+	$(MATH_SRC) \
 	$(PROFILE) \
 	$(SAMPLE_PROFILE) \
 	$(STACK_GUARD)