altos: Add conversion between Pa and meters

To be used with the MS5607 which generates data in calibrated units.

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

3 files changed, 160 insertions, 0 deletions

diff --git a/src/core/ao.h b/src/core/ao.h
index 27b9c5c4..a2092cfe 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -282,6 +282,18 @@ ao_altitude_to_pres(int16_t alt) __reentrant;
 int16_t
 ao_temp_to_dC(int16_t temp) __reentrant;
 
+/*
+ * ao_convert_pa.c
+ *
+ * Convert between pressure in Pa and altitude in meters
+ */
+
+int32_t
+ao_pa_to_altitude(int32_t pa);
+
+int32_t
+ao_altitude_to_pa(int32_t alt);
+
 #if HAS_DBG
 #include <ao_dbg.h>
 #endif
diff --git a/src/core/ao_convert_pa.c b/src/core/ao_convert_pa.c
new file mode 100644
index 00000000..0c93caea
--- /dev/null
+++ b/src/core/ao_convert_pa.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST)
+#include "ao.h"
+#endif
+
+static const int32_t altitude_table[] = {
+#include "altitude-pa.h"
+};
+
+#define ALT_SCALE	(1 << ALT_SHIFT)
+#define ALT_MASK	(ALT_SCALE - 1)
+
+int32_t
+ao_pa_to_altitude(int32_t pa)
+{
+	int16_t	o;
+	int16_t	part;
+	int32_t low, high;
+
+	if (pa < 0)
+		pa = 0;
+	if (pa > 120000)
+		pa = 120000;
+	o = pa >> ALT_SHIFT;
+	part = pa & ALT_MASK;
+
+	low = (int32_t) altitude_table[o] * (ALT_SCALE - part);
+	high = (int32_t) altitude_table[o+1] * part + (ALT_SCALE >> 1);
+	return (low + high) >> ALT_SHIFT;
+}
+
+int32_t
+ao_altitude_to_pa(int32_t alt)
+{
+	int32_t 	span, sub_span;
+	uint16_t	l, h, m;
+	int32_t 	pa;
+
+	l = 0;
+	h = NALT - 1;
+	while ((h - l) != 1) {
+		m = (l + h) >> 1;
+		if (altitude_table[m] < alt)
+			h = m;
+		else
+			l = m;
+	}
+	span = altitude_table[l] - altitude_table[h];
+	sub_span = altitude_table[l] - alt;
+	pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
+	if (pa > 120000)
+		pa = 120000;
+	if (pa < 0)
+		pa = 0;
+	return pa;
+}
diff --git a/src/core/ao_convert_pa_test.c b/src/core/ao_convert_pa_test.c
new file mode 100644
index 00000000..972a4d4c
--- /dev/null
+++ b/src/core/ao_convert_pa_test.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <stdint.h>
+#define AO_CONVERT_TEST
+#include "ao_host.h"
+#include "ao_convert_pa.c"
+
+#define STEP_P	1
+#define STEP_A	1
+
+static inline i_abs(int i) { return i < 0 ? -i : i; }
+
+main ()
+{
+	int	i;
+	int32_t p_to_a, p_to_a_to_p;
+	int32_t a_to_p, a_to_p_to_a;
+	int max_p_error = 0, max_p_error_p = -1;
+	int max_a_error = 0, max_a_error_a = -1;
+	int p_error;
+	int a_error;
+	int ret = 0;
+
+	for (i = 0; i < 120000 + STEP_P; i += STEP_P) {
+		if (i > 120000)
+			i = 120000;
+		p_to_a = ao_pa_to_altitude(i);
+		p_to_a_to_p = ao_altitude_to_pa(p_to_a);
+		p_error = i_abs(p_to_a_to_p - i);
+		if (p_error > max_p_error) {
+			max_p_error = p_error;
+			max_p_error_p = i;
+		}
+//		printf ("pa %d alt %d pa %d\n",
+//			i, p_to_a, p_to_a_to_p);
+	}
+	for (i = -1450; i < 74250 + STEP_A; i += STEP_A) {
+		if (i > 74250)
+			i = 74250;
+		a_to_p = ao_altitude_to_pa(i);
+		a_to_p_to_a = ao_pa_to_altitude(a_to_p);
+		a_error = i_abs(a_to_p_to_a - i);
+		if (a_error > max_a_error) {
+			max_a_error = a_error;
+			max_a_error_a = i;
+		}
+//		printf ("alt %d pa %d alt %d\n",
+//			i, a_to_p, a_to_p_to_a);
+	}
+	if (max_p_error > 2) {
+		printf ("max p error %d at %d\n", max_p_error,
+			max_p_error_p);
+		ret++;
+	}
+	if (max_a_error > 1) {
+		printf ("max a error %d at %d\n", max_a_error,
+			max_a_error_a);
+		ret++;
+	}
+	return ret;
+}