diff options
Diffstat (limited to 'src/test')
| -rw-r--r-- | src/test/Makefile | 16 | ||||
| -rw-r--r-- | src/test/ao_aprs_test.c | 132 | ||||
| -rw-r--r-- | src/test/ao_flight_test.c | 228 | ||||
| -rw-r--r-- | src/test/ao_gps_test.c | 8 | ||||
| -rw-r--r-- | src/test/ao_gps_test_skytraq.c | 10 |
5 files changed, 388 insertions, 6 deletions
diff --git a/src/test/Makefile b/src/test/Makefile index 44cee904..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 @@ -29,7 +30,7 @@ ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kal cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) - cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< + cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< -lm ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< @@ -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 7180f02d..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), @@ -547,6 +555,207 @@ int32(uint8_t *bytes, int off) static int log_format; +#if MEGAMETRUM + +static double +ao_vec_norm(double x, double y, double z) +{ + return x*x + y*y + z*z; +} + +static void +ao_vec_normalize(double *x, double *y, double *z) +{ + double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); + + *x *= scale; + *y *= scale; + *z *= scale; +} + +struct ao_quat { + double q0, q1, q2, q3; +}; + +static void +ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) +{ + r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; + r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; + r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; + r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; +} + +#if 0 +static void +ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) +{ + r->q0 = a->q0 * s; + r->q1 = a->q1 * s; + r->q2 = a->q2 * s; + r->q3 = a->q3 * s; +} +#endif + +static void +ao_quat_conj(struct ao_quat *r, struct ao_quat *a) +{ + r->q0 = a->q0; + r->q1 = -a->q1; + r->q2 = -a->q2; + r->q3 = -a->q3; +} + +static void +ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) +{ + struct ao_quat t; + struct ao_quat c; + ao_quat_mul(&t, q, a); + ao_quat_conj(&c, q); + ao_quat_mul(r, &t, &c); +} + +static void +ao_quat_from_angle(struct ao_quat *r, + double x_rad, + double y_rad, + double z_rad) +{ + double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); + double s = sin(angle/2); + double c = cos(angle/2); + + r->q0 = c; + r->q1 = x_rad * s / angle; + r->q2 = y_rad * s / angle; + r->q3 = z_rad * s / angle; +} + +static void +ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) +{ + ao_vec_normalize(&x, &y, &z); + double x_rad = atan2(z, y); + double y_rad = atan2(x, z); + double z_rad = atan2(y, x); + + ao_quat_from_angle(r, x_rad, y_rad, z_rad); +} + +static double +ao_quat_norm(struct ao_quat *a) +{ + return (a->q0 * a->q0 + + a->q1 * a->q1 + + a->q2 * a->q2 + + a->q3 * a->q3); +} + +static void +ao_quat_normalize(struct ao_quat *a) +{ + double norm = ao_quat_norm(a); + + if (norm) { + double m = 1/sqrt(norm); + + a->q0 *= m; + a->q1 *= m; + a->q2 *= m; + a->q3 *= m; + } +} + +static struct ao_quat ao_up, ao_current; +static struct ao_quat ao_orient; +static int ao_orient_tick; + +void +set_orientation(double x, double y, double z, int tick) +{ + struct ao_quat t; + + printf ("set_orientation %g %g %g\n", x, y, z); + ao_quat_from_vector(&ao_orient, x, y, z); + ao_up.q1 = ao_up.q2 = 0; + ao_up.q0 = ao_up.q3 = sqrt(2)/2; + ao_orient_tick = tick; + + ao_orient.q0 = 1; + ao_orient.q1 = 0; + ao_orient.q2 = 0; + ao_orient.q3 = 0; + + printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", + ao_orient.q0, + ao_orient.q1, + ao_orient.q2, + ao_orient.q3, + ao_up.q0, + ao_up.q1, + ao_up.q2, + ao_up.q3); + + ao_quat_rot(&t, &ao_up, &ao_orient); + printf ("pad orient (%g) %g %g %g\n", + t.q0, + t.q1, + t.q2, + t.q3); + +} + +void +update_orientation (double rate_x, double rate_y, double rate_z, int tick) +{ + struct ao_quat q_dot; + double lambda; + double dt = (tick - ao_orient_tick) / 100.0; + + ao_orient_tick = tick; + +// lambda = 1 - ao_quat_norm(&ao_orient); + lambda = 0; + + q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; + q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; + 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; + ao_orient.q2 += q_dot.q2 * dt; + ao_orient.q3 += q_dot.q3 * dt; + + ao_quat_normalize(&ao_orient); + + 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, + ao_orient.q2, + ao_orient.q3, + ao_current.q0, + ao_current.q1, + ao_current.q2, + ao_current.q3); +#endif +} +#endif + void ao_sleep(void *wchan) { @@ -635,6 +844,21 @@ ao_sleep(void *wchan) f(gyro_x); f(gyro_y); f(gyro_z); + + double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); + double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); + double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); + + /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes + * Z is normal to the ground, the MPU y axis + */ + set_orientation(accel_x, accel_z, accel_y, tick); + } else { + double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); + 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 * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); } ao_records_read++; ao_insert(); @@ -779,6 +1003,8 @@ ao_sleep(void *wchan) continue; #if MEGAMETRUM + (void) a; + (void) b; #else switch (type) { case 'F': diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index d75a12ec..3844a326 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -88,6 +88,7 @@ ao_mutex_put(uint8_t *mutex) static int ao_gps_fd; +#if 0 static void ao_dbg_char(char c) { @@ -103,6 +104,7 @@ ao_dbg_char(char c) } write(1, line, strlen(line)); } +#endif #define QUEUE_LEN 4096 @@ -391,6 +393,7 @@ ao_serial1_putchar(char c) #define AO_SERIAL_SPEED_4800 0 #define AO_SERIAL_SPEED_57600 1 +#define AO_SERIAL_SPEED_115200 2 static void ao_serial1_set_speed(uint8_t speed) @@ -407,6 +410,9 @@ ao_serial1_set_speed(uint8_t speed) case AO_SERIAL_SPEED_57600: cfsetspeed(&termios, B57600); break; + case AO_SERIAL_SPEED_115200: + cfsetspeed(&termios, B115200); + break; } tcsetattr(fd, TCSAFLUSH, &termios); tcflush(fd, TCIFLUSH); @@ -420,7 +426,6 @@ ao_serial1_set_speed(uint8_t speed) void ao_dump_state(void *wchan) { - double lat, lon; int i; if (wchan == &ao_gps_data) ao_gps_print(&ao_gps_data); @@ -510,4 +515,5 @@ main (int argc, char **argv) } ao_gps_setup(); ao_gps(); + return 0; } diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 846daa94..81008b39 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -397,6 +397,7 @@ ao_serial1_putchar(char c) #define AO_SERIAL_SPEED_4800 0 #define AO_SERIAL_SPEED_9600 1 #define AO_SERIAL_SPEED_57600 2 +#define AO_SERIAL_SPEED_115200 3 static void ao_serial1_set_speed(uint8_t speed) @@ -411,11 +412,14 @@ ao_serial1_set_speed(uint8_t speed) cfsetspeed(&termios, B4800); break; case AO_SERIAL_SPEED_9600: - cfsetspeed(&termios, B38400); + cfsetspeed(&termios, B9600); break; case AO_SERIAL_SPEED_57600: cfsetspeed(&termios, B57600); break; + case AO_SERIAL_SPEED_115200: + cfsetspeed(&termios, B115200); + break; } tcsetattr(fd, TCSAFLUSH, &termios); tcflush(fd, TCIFLUSH); @@ -423,6 +427,10 @@ ao_serial1_set_speed(uint8_t speed) #define ao_time() 0 +uint8_t ao_task_minimize_latency; + +#define ao_usb_getchar() 0 + #include "ao_gps_print.c" #include "ao_gps_skytraq.c" |