Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

3 files changed, 159 insertions, 4 deletions

diff --git a/src/test/Makefile b/src/test/Makefile
index 0dcdc949..092bf360 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,7 +1,8 @@
 vpath % ..:../core:../drivers:../util
 
 PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \
-	ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test
+	ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \
+	ao_aprs_test
 
 INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
 
@@ -9,7 +10,7 @@ KALMAN=make-kalman
 
 CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall
 
-all: $(PROGS)
+all: $(PROGS) ao_aprs_data.wav
 
 clean:
 	rm -f $(PROGS) run-out.baro run-out.full
@@ -49,5 +50,14 @@ ao_kalman.h: $(KALMAN)
 ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_fec_rx.c
 	cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_fec_rx.c -lm
 
+ao_aprs_test: ao_aprs_test.c ao_aprs.c
+	cc $(CFLAGS) -o $@ ao_aprs_test.c
+
+SOX_INPUT_ARGS=--type raw --encoding unsigned-integer -b 8 -c 1 -r 9600
+SOX_OUTPUT_ARGS=--type wav
+
+ao_aprs_data.wav: ao_aprs_test
+	./ao_aprs_test | sox $(SOX_INPUT_ARGS) - $(SOX_OUTPUT_ARGS) $@
+
 check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests
 	./ao_fec_test && ./run-tests
\ No newline at end of file
diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c
new file mode 100644
index 00000000..3b31f2d3
--- /dev/null
+++ b/src/test/ao_aprs_test.c
@@ -0,0 +1,132 @@
+/*
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdarg.h>
+
+#include <ao_telemetry.h>
+
+struct ao_telemetry_location ao_gps_data;
+
+#define AO_APRS_TEST
+
+typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len);
+
+#define DEBUG 0
+#if DEBUG
+void
+ao_aprs_bit(uint8_t bit)
+{
+	static int	seq = 0;
+	printf ("%6d %d\n", seq++, bit ? 1 : 0);
+}
+#else
+void
+ao_aprs_bit(uint8_t bit)
+{
+	putchar (bit ? 0xc0 : 0x40);
+}
+#endif
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill);
+
+#include <ao_aprs.c>
+
+/*
+ * @section copyright_sec Copyright
+ *
+ * Copyright (c) 2001-2009 Michael Gray, KD7LMO
+
+
+ *
+ *
+ * @section gpl_sec GNU General Public License
+ *
+ *  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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
+ *  
+
+ */
+
+static void
+audio_gap(int secs)
+{
+#if !DEBUG
+	int	samples = secs * 9600;
+
+	while (samples--)
+		ao_aprs_bit(0);
+#endif
+}
+
+// This is where we go after reset.
+int main(int argc, char **argv)
+{
+    audio_gap(1);
+
+    ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000;
+    ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000;
+    ao_gps_data.altitude = 84;
+
+    /* Transmit one packet */
+    ao_aprs_send();
+
+    tncBuffer[strlen((char *) tncBuffer) - 2] = '\0';
+    fprintf(stderr, "packet: %s\n", tncBuffer);
+
+    exit(0);
+}
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill)
+{
+	int16_t	len;
+	uint8_t	done = 0;
+	uint8_t	buf[16], *b, c;
+	uint8_t bit;
+
+	while (!done) {
+		len = (*fill)(buf, sizeof (buf));
+		if (len < 0) {
+			done = 1;
+			len = -len;
+		}
+		b = buf;
+		while (len--) {
+			c = *b++;
+			for (bit = 0; bit < 8; bit++) {
+				ao_aprs_bit(c & 0x80);
+				c <<= 1;
+			}
+		}
+	}
+}
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index acdf4d92..cdd1f236 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -236,10 +236,14 @@ extern int32_t	ao_accel_scale;
 extern alt_t	ao_ground_height;
 extern alt_t	ao_sample_alt;
 
+double ao_sample_qangle;
+
 int ao_sample_prev_tick;
 uint16_t	prev_tick;
 
+
 #include "ao_kalman.c"
+#include "ao_sqrt.c"
 #include "ao_sample.c"
 #include "ao_flight.c"
 
@@ -309,7 +313,7 @@ ao_mpu6000_accel(int16_t sensor)
 }
 
 static double
-ao_mpu6000_gyro(int16_t sensor)
+ao_mpu6000_gyro(int32_t sensor)
 {
 	return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
 }
@@ -370,6 +374,7 @@ ao_insert(void)
 		if (!ao_summary) {
 			printf("%7.2f height %8.2f accel %8.3f "
 #if MEGAMETRUM
+			       "roll %8.3f angle %8.3f qangle %8.3f "
 			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
 #endif
 			       "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
@@ -377,6 +382,9 @@ ao_insert(void)
 			       height,
 			       accel,
 #if MEGAMETRUM
+			       ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
+			       ao_mpu6000_gyro(ao_sample_angle) / 100.0,
+			       ao_sample_qangle,
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
@@ -715,12 +723,14 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 	q_dot.q2 =  0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
 	q_dot.q3 =  0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
 
+#if 0
 	printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
 	printf ("q_dot (%g) %g %g %g\n",
 		q_dot.q0,
 		q_dot.q1,
 		q_dot.q2,
 		q_dot.q3);
+#endif
 
 	ao_orient.q0 += q_dot.q0 * dt;
 	ao_orient.q1 += q_dot.q1 * dt;
@@ -731,6 +741,8 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 
 	ao_quat_rot(&ao_current, &ao_up, &ao_orient);
 
+	ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2));
+#if 0
 	printf ("orient (%g) %g %g %g current (%g) %g %g %g\n",
 		ao_orient.q0,
 		ao_orient.q1,
@@ -740,6 +752,7 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 		ao_current.q1,
 		ao_current.q2,
 		ao_current.q3);
+#endif
 }
 #endif
 
@@ -845,7 +858,7 @@ ao_sleep(void *wchan)
 						double		rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
 						double		rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
 
-						update_orientation(rate_x, rate_z, rate_y, tick);
+						update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
 					}
 					ao_records_read++;
 					ao_insert();