From 321d0f68c04a5a9c6ea7874081e6245d44c48bb4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 13 Oct 2012 13:39:03 -0700 Subject: altos/test: Add ao_flight_test_mm This reads mega metrum eeprom files and runs the flight code over it Signed-off-by: Keith Packard --- src/test/Makefile | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) (limited to 'src/test/Makefile') diff --git a/src/test/Makefile b/src/test/Makefile index db3cc04b..37949e86 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,10 +1,11 @@ vpath % ..:../core:../drivers:../util -PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test +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 KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -O3 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall all: $(PROGS) @@ -16,7 +17,7 @@ install: ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h cc $(CFLAGS) -o $@ $< -ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h +ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude-pa.h ao_kalman.h cc -DNOISY_ACCEL=1 $(CFLAGS) -o $@ $< ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h @@ -25,6 +26,9 @@ ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalm ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h 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 altitude.h ao_kalman.h + cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< + ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< -- cgit v1.2.3 From 91b8c8b20cead2836ec835f44b4ca0cf06cbf518 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 13 Oct 2012 15:04:46 -0700 Subject: altos/test: Display MPU6000 values in ao_flight_test_mm output No computation yet, just making the values visible in the output Signed-off-by: Keith Packard --- src/test/Makefile | 12 ++++++----- src/test/ao_flight_test.c | 52 ++++++++++++++++++++++++++++++++++++++++++++--- 2 files changed, 56 insertions(+), 8 deletions(-) (limited to 'src/test/Makefile') diff --git a/src/test/Makefile b/src/test/Makefile index 37949e86..44cee904 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -3,6 +3,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 +INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h + KALMAN=make-kalman CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall @@ -14,19 +16,19 @@ clean: install: -ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h +ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h $(INCS) cc $(CFLAGS) -o $@ $< -ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude-pa.h ao_kalman.h +ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) cc -DNOISY_ACCEL=1 $(CFLAGS) -o $@ $< -ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h +ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) cc $(CFLAGS) -o $@ -DHAS_ACCEL=0 ao_flight_test.c -ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h +ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) 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 altitude.h ao_kalman.h +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 $@ $< ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 6c0bb4da..0df9a5d7 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -260,6 +260,10 @@ static int landed_set; static double landed_time; static double landed_height; +#if HAS_MPU6000 +static struct ao_mpu6000_sample ao_ground_mpu6000; +#endif + void ao_test_exit(void) { @@ -297,6 +301,20 @@ ao_test_exit(void) exit(0); } +#if HAS_MPU6000 +static double +ao_mpu6000_accel(int16_t sensor) +{ + return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; +} + +static double +ao_mpu6000_gyro(int16_t sensor) +{ + return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; +} +#endif + void ao_insert(void) { @@ -350,10 +368,22 @@ ao_insert(void) } if (!ao_summary) { - printf("%7.2f height %8.2f accel %8.3f 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", + printf("%7.2f height %8.2f accel %8.3f " +#if MEGAMETRUM + "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", time, height, accel, +#if MEGAMETRUM + 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), + ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), + ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), + ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), +#endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, ao_k_speed / 65536.0 / 16.0, @@ -536,6 +566,7 @@ ao_sleep(void *wchan) if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { #if MEGAMETRUM + ao_data_static.mpu6000 = ao_ground_mpu6000; #else ao_data_static.adc.accel = ao_flight_ground_accel; #endif @@ -585,8 +616,23 @@ ao_sleep(void *wchan) ao_data_static.ms5607_raw.pres = int32(bytes, 0); ao_data_static.ms5607_raw.temp = int32(bytes, 4); ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); - ao_data_set_accel(&ao_data_static, -int16(bytes, 10)); -// printf ("accel %d pres %d\n", ao_data_accel_cook(&ao_data_static), value.pres); + ao_data_static.mpu6000.accel_x = int16(bytes, 8); + ao_data_static.mpu6000.accel_y = -int16(bytes, 10); + ao_data_static.mpu6000.accel_z = int16(bytes, 12); + ao_data_static.mpu6000.gyro_x = int16(bytes, 14); + ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); + ao_data_static.mpu6000.gyro_z = int16(bytes, 18); + if (ao_records_read == 0) + ao_ground_mpu6000 = ao_data_static.mpu6000; + else if (ao_records_read < 10) { +#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) + f(accel_x); + f(accel_y); + f(accel_z); + f(gyro_x); + f(gyro_y); + f(gyro_z); + } ao_records_read++; ao_insert(); return; -- cgit v1.2.3 From 5f6b3790667d9b92370b4fe0dad5626929fea2ba Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 30 Nov 2012 20:51:47 -0800 Subject: altos: Make skytraq reflashing code try both 9600 and 4800 baud This lets it communicate with the ROM code which boots at 4800 baud instead of 9600 baud. Signed-off-by: Keith Packard --- ao-tools/Makefile.am | 2 +- ao-tools/lib/Makefile.am | 1 + ao-tools/lib/cc.h | 116 +++++++++++++++++++++++ configure.ac | 1 + src/drivers/ao_gps_skytraq.c | 6 ++ src/test/Makefile | 2 +- src/test/ao_flight_test.c | 213 +++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 339 insertions(+), 2 deletions(-) (limited to 'src/test/Makefile') diff --git a/ao-tools/Makefile.am b/ao-tools/Makefile.am index 871b8205..4cf1f382 100644 --- a/ao-tools/Makefile.am +++ b/ao-tools/Makefile.am @@ -1 +1 @@ -SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash +SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash ao-mega diff --git a/ao-tools/lib/Makefile.am b/ao-tools/lib/Makefile.am index 1f8f2e42..a664d083 100644 --- a/ao-tools/lib/Makefile.am +++ b/ao-tools/lib/Makefile.am @@ -34,6 +34,7 @@ libao_tools_a_SOURCES = \ cc-telem.c \ cc-telemetry.c \ cc-telemetry.h \ + cc-mega.c \ cp-usb-async.c \ cp-usb-async.h \ i0.c \ diff --git a/ao-tools/lib/cc.h b/ao-tools/lib/cc.h index 6257ee44..625540bb 100644 --- a/ao-tools/lib/cc.h +++ b/ao-tools/lib/cc.h @@ -269,6 +269,122 @@ struct cc_telem { int cc_telem_parse(const char *input_line, struct cc_telem *telem); +struct ao_log_mega { + char type; /* 0 */ + uint8_t is_config; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + } flight; /* 12 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; + uint16_t reason; + } state; + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel_x; /* 12 */ + int16_t accel_y; /* 14 */ + int16_t accel_z; /* 16 */ + int16_t gyro_x; /* 18 */ + int16_t gyro_y; /* 20 */ + int16_t gyro_z; /* 22 */ + int16_t mag_x; /* 24 */ + int16_t mag_y; /* 26 */ + int16_t mag_z; /* 28 */ + int16_t accel; /* 30 */ + } sensor; /* 32 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t v_pbatt; /* 6 */ + int16_t n_sense; /* 8 */ + int16_t sense[10]; /* 10 */ + } volt; /* 30 */ + /* AO_LOG_GPS_TIME */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + uint8_t hour; /* 14 */ + uint8_t minute; /* 15 */ + uint8_t second; /* 16 */ + uint8_t flags; /* 17 */ + uint8_t year; /* 18 */ + uint8_t month; /* 19 */ + uint8_t day; /* 20 */ + uint8_t pad; /* 21 */ + } gps; /* 22 */ + /* AO_LOG_GPS_SAT */ + struct { + uint16_t channels; /* 4 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[12]; /* 6 */ + } gps_sat; /* 30 */ + + struct { + uint32_t kind; + int32_t data[6]; + } config_int; + + struct { + uint32_t kind; + char string[24]; + } config_str; + + /* Raw bytes */ + uint8_t bytes[28]; + } u; +}; + +#define AO_CONFIG_CONFIG 1 +#define AO_CONFIG_MAIN 2 +#define AO_CONFIG_APOGEE 3 +#define AO_CONFIG_LOCKOUT 4 +#define AO_CONFIG_FREQUENCY 5 +#define AO_CONFIG_RADIO_ENABLE 6 +#define AO_CONFIG_ACCEL_CAL 7 +#define AO_CONFIG_RADIO_CAL 8 +#define AO_CONFIG_MAX_LOG 9 +#define AO_CONFIG_IGNITE_MODE 10 +#define AO_CONFIG_PAD_ORIENTATION 11 +#define AO_CONFIG_SERIAL_NUMBER 12 +#define AO_CONFIG_LOG_FORMAT 13 +#define AO_CONFIG_MS5607_RESERVED 14 +#define AO_CONFIG_MS5607_SENS 15 +#define AO_CONFIG_MS5607_OFF 16 +#define AO_CONFIG_MS5607_TCS 17 +#define AO_CONFIG_MS5607_TCO 18 +#define AO_CONFIG_MS5607_TREF 19 +#define AO_CONFIG_MS5607_TEMPSENS 20 +#define AO_CONFIG_MS5607_CRC 21 + + +#define AO_LOG_FLIGHT 'F' +#define AO_LOG_SENSOR 'A' +#define AO_LOG_TEMP_VOLT 'T' +#define AO_LOG_DEPLOY 'D' +#define AO_LOG_STATE 'S' +#define AO_LOG_GPS_TIME 'G' +#define AO_LOG_GPS_LAT 'N' +#define AO_LOG_GPS_LON 'W' +#define AO_LOG_GPS_ALT 'H' +#define AO_LOG_GPS_SAT 'V' +#define AO_LOG_GPS_DATE 'Y' + +#define AO_LOG_CONFIG 'c' + +int +cc_mega_parse(const char *input_line, struct ao_log_mega *l); + #ifndef TRUE #define TRUE 1 #define FALSE 0 diff --git a/configure.ac b/configure.ac index 0fcd97e2..ad10ac3c 100644 --- a/configure.ac +++ b/configure.ac @@ -164,6 +164,7 @@ ao-tools/ao-telem/Makefile ao-tools/ao-stmload/Makefile ao-tools/ao-send-telem/Makefile ao-tools/ao-sky-flash/Makefile +ao-tools/ao-mega/Makefile ao-utils/Makefile src/Version ]) diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index b7dc0a86..d637a602 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -515,7 +515,13 @@ gps_update(void) __reentrant ao_timer_set_adc_interval(0); #endif ao_skytraq_sendstruct(ao_gps_115200); + ao_delay(AO_MS_TO_TICKS(500)); + ao_gps_set_speed(AO_SERIAL_SPEED_4800); + ao_delay(AO_MS_TO_TICKS(500)); + ao_skytraq_sendstruct(ao_gps_115200); + ao_delay(AO_MS_TO_TICKS(500)); ao_gps_set_speed(AO_SERIAL_SPEED_115200); + ao_delay(AO_MS_TO_TICKS(500)); /* It's a binary protocol; abandon attempts to escape */ for (;;) diff --git a/src/test/Makefile b/src/test/Makefile index 44cee904..0dcdc949 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -29,7 +29,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 $@ $< diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 7180f02d..acdf4d92 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -547,6 +547,202 @@ 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; + + 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); + + 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); + + 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 + void ao_sleep(void *wchan) { @@ -635,6 +831,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, rate_z, rate_y, tick); } ao_records_read++; ao_insert(); @@ -779,6 +990,8 @@ ao_sleep(void *wchan) continue; #if MEGAMETRUM + (void) a; + (void) b; #else switch (type) { case 'F': -- cgit v1.2.3 From 0c2c47dd7af2fc95de852178c4244daba02f44ed Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 19:44:09 -0800 Subject: altos: Add test scaffolding for APRS This moves some test code out of ao_aprs.c and into ao_aprs_test.c, and then adds Makefile fragments to compile and run the resulting program, creating a wav file as output Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 163 +++++------------------------------------------- src/test/Makefile | 14 ++++- src/test/ao_aprs_test.c | 145 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 174 insertions(+), 148 deletions(-) create mode 100644 src/test/ao_aprs_test.c (limited to 'src/test/Makefile') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index be7abaf5..df68278c 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -139,11 +139,10 @@ * */ -#include -#include -#include -#include -#include +#ifndef AO_APRS_TEST +#include +#endif + #include typedef int bool_t; @@ -395,10 +394,8 @@ const uint32_t freqTable[256] = */ void ddsSetFTW (uint32_t ftw) { - static int id; int x = ftw - freqTable[0]; putchar (x > 0 ? 0xff : 0x0); -// printf ("%d %d\n", id++, x > 0 ? 1 : 0); } /** @@ -1003,6 +1000,8 @@ void tncSetMode(TNC_DATA_MODE dataMode) // FSK tones at 445.947 and 445.953 MHz ddsSetFSKFreq (955382980, 955453621); break; + case TNC_MODE_STANDBY: + break; } // END switch tncDataMode = dataMode; @@ -1061,11 +1060,12 @@ void tnc1200TimerTick() case TNC_TX_SYNC: // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // When the flag is done, determine if we need to send more or data. if (++tncBitCount == 8) @@ -1101,11 +1101,12 @@ void tnc1200TimerTick() // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // Save the data stream so we can determine if bit stuffing is // required on the next bit time. @@ -1145,11 +1146,12 @@ void tnc1200TimerTick() // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // Save the data stream so we can determine if bit stuffing is // required on the next bit time. @@ -1177,11 +1179,12 @@ void tnc1200TimerTick() case TNC_TX_END: // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // If all the bits were shifted, get the next one. if (++tncBitCount == 8) @@ -1239,7 +1242,7 @@ tncPrintf(char *fmt, ...) int c; va_start(ap, fmt); - c = vsprintf(tncBufferPnt, fmt, ap); + c = vsprintf((char *) tncBufferPnt, fmt, ap); va_end(ap); tncBufferPnt += c; tncLength += c; @@ -1369,7 +1372,7 @@ void tncGPRMCPacket() */ void tncStatusPacket(int16_t temperature) { - uint16_t voltage; +// uint16_t voltage; // Plain text telemetry. tncPrintf (">ANSR "); @@ -1425,7 +1428,7 @@ void tncStatusPacket(int16_t temperature) */ void tncTxPacket(TNC_DATA_MODE dataMode) { - int16_t temperature; + int16_t temperature = 20; uint16_t crc; // Only transmit if there is not another message in progress. @@ -1510,135 +1513,3 @@ void tncTxPacket(TNC_DATA_MODE dataMode) } /** @} */ - -#if 0 -uint32_t counter; - -uint8_t bitIndex; -uint8_t streamIndex; -uint8_t value; - -uint8_t bitStream[] = { 0x10, 0x20, 0x30 }; - -void init() -{ - counter = 0; - bitIndex = 0; - streamIndex = 0; - value = bitStream[0]; -} - -void test() -{ - counter += 0x10622d; - -// CCP_1 = (uint16_t) ((counter >> 16) & 0xffff); - - if ((value & 0x80) == 0x80) - setup_ccp1 (CCP_COMPARE_SET_ON_MATCH); - else - setup_ccp1 (CCP_COMPARE_CLR_ON_MATCH); - - if (++bitIndex == 8) - { - bitIndex = 0; - - if (++streamIndex == sizeof(bitStream)) - { - streamIndex = 0; - } - - value = bitStream[streamIndex]; - } else - value = value << 1; -} -#endif - -// This is where we go after reset. -int main(int argc, char **argv) -{ - uint8_t i, utcSeconds, lockLostCounter; - -//test(); - - // Configure the basic systems. -// sysInit(); - - // Wait for the power converter chains to stabilize. -// delay_ms (100); - - // Setup the subsystems. -// adcInit(); -// flashInit(); - gpsInit(); -// logInit(); -// timeInit(); -// serialInit(); - tncInit(); - - // Program the DDS. -// ddsInit(); - - // Transmit software version packet on start up. - tncTxPacket(TNC_MODE_1200_AFSK); - - exit(0); - // Counters to send packets if the GPS time stamp is not available. - lockLostCounter = 5; - utcSeconds = 55; - - // This is the main loop that process GPS data and waits for the once per second timer tick. - for (;;) - { - // Read the GPS engine serial port FIFO and process the GPS data. -// gpsUpdate(); - - if (gpsIsReady()) - { - // Start the flight timer when we get a valid 3D fix. - if (gpsGetFixType() == GPS_3D_FIX) - timeSetRunFlag(); - - // Generate our packets based on the GPS time. - if (tncIsTimeSlot(gpsPosition.seconds)) - tncTxPacket(TNC_MODE_1200_AFSK); - - // Sync the internal clock to GPS UTC time. - utcSeconds = gpsPosition.seconds; - - // This counter is reset every time we receive the GPS message. - lockLostCounter = 0; - - // Log the data to flash. -// sysLogGPSData(); - } // END if gpsIsReady - - // Processing that occurs once a second. - if (timeIsUpdate()) - { - // We maintain the UTC time in seconds if we shut off the GPS engine or it fails. - if (++utcSeconds == 60) - utcSeconds = 0; - - // If we loose information for more than 5 seconds, - // we will determine when to send a packet based on internal time. - if (lockLostCounter == 5) - { - if (tncIsTimeSlot(utcSeconds)) - tncTxPacket(TNC_MODE_1200_AFSK); - } else - ++lockLostCounter; - - // Update the ADC filters. -// adcUpdate(); - - if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) - gpsPowerOff(); - - } // END if timeIsUpdate - - } // END for -} - - - 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..d791e930 --- /dev/null +++ b/src/test/ao_aprs_test.c @@ -0,0 +1,145 @@ +/* + * Copyright © 2012 Keith Packard + * + * 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 +#include +#include +#include +#include + +#include + +#define AO_APRS_TEST + +#include + +/* + * @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 + * + + */ + +// This is where we go after reset. +int main(int argc, char **argv) +{ + uint8_t utcSeconds, lockLostCounter; + +//test(); + + // Configure the basic systems. +// sysInit(); + + // Wait for the power converter chains to stabilize. +// delay_ms (100); + + // Setup the subsystems. +// adcInit(); +// flashInit(); + gpsInit(); +// logInit(); +// timeInit(); +// serialInit(); + tncInit(); + + // Program the DDS. +// ddsInit(); + + // Transmit software version packet on start up. + tncTxPacket(TNC_MODE_1200_AFSK); + + exit(0); + // Counters to send packets if the GPS time stamp is not available. + lockLostCounter = 5; + utcSeconds = 55; + + // This is the main loop that process GPS data and waits for the once per second timer tick. + for (;;) + { + // Read the GPS engine serial port FIFO and process the GPS data. +// gpsUpdate(); + + if (gpsIsReady()) + { + // Start the flight timer when we get a valid 3D fix. + if (gpsGetFixType() == GPS_3D_FIX) + timeSetRunFlag(); + + // Generate our packets based on the GPS time. + if (tncIsTimeSlot(gpsPosition.seconds)) + tncTxPacket(TNC_MODE_1200_AFSK); + + // Sync the internal clock to GPS UTC time. + utcSeconds = gpsPosition.seconds; + + // This counter is reset every time we receive the GPS message. + lockLostCounter = 0; + + // Log the data to flash. +// sysLogGPSData(); + } // END if gpsIsReady + + // Processing that occurs once a second. + if (timeIsUpdate()) + { + // We maintain the UTC time in seconds if we shut off the GPS engine or it fails. + if (++utcSeconds == 60) + utcSeconds = 0; + + // If we loose information for more than 5 seconds, + // we will determine when to send a packet based on internal time. + if (lockLostCounter == 5) + { + if (tncIsTimeSlot(utcSeconds)) + tncTxPacket(TNC_MODE_1200_AFSK); + } else + ++lockLostCounter; + + // Update the ADC filters. +// adcUpdate(); + + if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) + gpsPowerOff(); + + } // END if timeIsUpdate + + } // END for +} + + + + -- cgit v1.2.3 From 540309240a8515116120dbd4403902282ed8c27b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 Jan 2013 15:15:49 -0800 Subject: altos: Add Kalman filter to MicroPeak This filters altitudes more accurately and also allows tracking of acceleration, which is used to discard height data generated by ejection charge noise Signed-off-by: Keith Packard --- src/micropeak/Makefile | 4 +- src/micropeak/ao_microflight.c | 143 ++++++++++++++++++++++++++++++ src/micropeak/ao_microkalman.c | 74 ++++++++++++++++ src/micropeak/ao_micropeak.c | 106 +---------------------- src/micropeak/ao_micropeak.h | 32 +++++-- src/test/Makefile | 11 ++- src/test/ao_micropeak_test.c | 192 +++++++++++++++++++++++++++++++++++++++++ src/test/plotmicro | 14 +++ 8 files changed, 462 insertions(+), 114 deletions(-) create mode 100644 src/micropeak/ao_microflight.c create mode 100644 src/micropeak/ao_microkalman.c create mode 100644 src/test/ao_micropeak_test.c create mode 100755 src/test/plotmicro (limited to 'src/test/Makefile') diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index ff0a4499..315b93f6 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -33,7 +33,9 @@ ALTOS_SRC = \ ao_eeprom_tiny.c \ ao_panic.c \ ao_log_micro.c \ - ao_async.c + ao_async.c \ + ao_microflight.c \ + ao_microkalman.c INC=\ ao.h \ diff --git a/src/micropeak/ao_microflight.c b/src/micropeak/ao_microflight.c new file mode 100644 index 00000000..714bb90a --- /dev/null +++ b/src/micropeak/ao_microflight.c @@ -0,0 +1,143 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include +#include + +uint32_t pa; +uint32_t pa_ground; +uint32_t pa_min; + +static void +ao_microsample(void) +{ + ao_pa_get(); + ao_microkalman_predict(); + ao_microkalman_correct(); +} + +#define NUM_PA_HIST 16 + +#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) + +static uint32_t pa_hist[NUM_PA_HIST]; + +void +ao_microflight(void) +{ + int16_t sample_count; + uint16_t time; + uint32_t pa_interval_min, pa_interval_max; + int32_t pa_diff; + uint8_t h, i; + uint8_t accel_lock = 0; + uint32_t pa_sum = 0; + + /* Wait for motion, averaging values to get ground pressure */ + + time = ao_time(); + ao_pa_get(); + ao_microkalman_init(); + pa_ground = pa; + sample_count = 0; + h = 0; + for (;;) { + time += SAMPLE_SLEEP; + if (sample_count == 0) + ao_led_on(AO_LED_REPORT); + ao_delay_until(time); + ao_microsample(); + if (sample_count == 0) + ao_led_off(AO_LED_REPORT); + pa_hist[h] = pa; + h = SKIP_PA_HIST(h,1); + pa_diff = pa_ground - ao_pa; + + /* Check for a significant pressure change */ + if (pa_diff > BOOST_DETECT) + break; + + if (sample_count < GROUND_AVG * 2) { + if (sample_count < GROUND_AVG) + pa_sum += pa; + ++sample_count; + } else { + pa_ground = pa_sum >> GROUND_AVG_SHIFT; + pa_sum = 0; + sample_count = 0; + } + } + + /* Go back and find the first sample a decent interval above the ground */ + pa_min = pa_ground - LAND_DETECT; + for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) { + if (pa_hist[i] < pa_min) + break; + } + + /* Log the remaining samples so we get a complete history since leaving the ground */ + for (; i != h; i = SKIP_PA_HIST(i,2)) { + pa = pa_hist[i]; + ao_log_micro_data(); + } + + /* Now sit around until the pressure is stable again and record the max */ + + sample_count = 0; + pa_min = ao_pa; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + for (;;) { + time += SAMPLE_SLEEP; + ao_delay_until(time); + if ((sample_count & 3) == 0) + ao_led_on(AO_LED_REPORT); + ao_microsample(); + if ((sample_count & 3) == 0) + ao_led_off(AO_LED_REPORT); + if (sample_count & 1) + ao_log_micro_data(); + + /* If accelerating upwards, don't look for min pressure */ + if (ao_pa_accel < ACCEL_LOCK_PA) + accel_lock = ACCEL_LOCK_TIME; + else if (accel_lock) + --accel_lock; + else if (ao_pa < pa_min) + pa_min = ao_pa; + + if (sample_count == (GROUND_AVG - 1)) { + pa_diff = pa_interval_max - pa_interval_min; + + /* Check to see if the pressure is now stable */ + if (pa_diff < LAND_DETECT) + break; + sample_count = 0; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + } else { + if (ao_pa < pa_interval_min) + pa_interval_min = ao_pa; + if (ao_pa > pa_interval_max) + pa_interval_max = ao_pa; + ++sample_count; + } + } +} diff --git a/src/micropeak/ao_microkalman.c b/src/micropeak/ao_microkalman.c new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/micropeak/ao_microkalman.c @@ -0,0 +1,74 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include + +#define FIX_BITS 16 + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix8(x) ((x) >> 8) +#define from_fix(x) ((x) >> 16) +#define fix8_to_fix16(x) ((x) << 8) +#define fix16_to_fix8(x) ((x) >> 8) + +#include + +/* Basic time step (96ms) */ +#define AO_MK_STEP to_fix16(0.096) +/* step ** 2 / 2 */ +#define AO_MK_STEP_2_2 to_fix16(0.004608) + +uint32_t ao_k_pa; /* 24.8 fixed point */ +int32_t ao_k_pa_speed; /* 16.16 fixed point */ +int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +uint32_t ao_pa; /* integer portion */ +int16_t ao_pa_speed; /* integer portion */ +int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void) +{ + ao_pa = pa; + ao_k_pa = pa << 8; +} + +void +ao_microkalman_predict(void) +{ + ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); + ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; +} + +void +ao_microkalman_correct(void) +{ + int16_t e; /* Height error in Pa */ + + e = pa - from_fix8(ao_k_pa); + + ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); + ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; + ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; + ao_pa = from_fix8(ao_k_pa); + ao_pa_speed = from_fix(ao_k_pa_speed); + ao_pa_accel = from_fix(ao_k_pa_accel); +} diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c index f361aa26..10f0d192 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/micropeak/ao_micropeak.c @@ -24,16 +24,10 @@ static struct ao_ms5607_sample sample; static struct ao_ms5607_value value; -uint32_t pa; -uint32_t pa_avg; -uint32_t pa_ground; -uint32_t pa_min; alt_t ground_alt, max_alt; alt_t ao_max_height; -static uint32_t pa_sum; - -static void +void ao_pa_get(void) { ao_ms5607_sample(&sample); @@ -60,21 +54,9 @@ ao_pips(void) ao_delay(AO_MS_TO_TICKS(200)); } -#define NUM_PA_HIST 16 - -#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) - -static uint32_t pa_hist[NUM_PA_HIST]; - int main(void) { - int16_t sample_count; - uint16_t time; - uint32_t pa_interval_min, pa_interval_max; - int32_t pa_diff; - uint8_t h, i; - ao_led_init(LEDS_AVAILABLE); ao_timer_init(); @@ -93,93 +75,9 @@ main(void) ao_log_micro_dump(); ao_delay(BOOST_DELAY); - /* Wait for motion, averaging values to get ground pressure */ - time = ao_time(); - ao_pa_get(); - pa_avg = pa_ground = pa << FILTER_SHIFT; - sample_count = 0; - h = 0; - for (;;) { - time += SAMPLE_SLEEP; - if (sample_count == 0) - ao_led_on(AO_LED_REPORT); - ao_delay_until(time); - ao_pa_get(); - if (sample_count == 0) - ao_led_off(AO_LED_REPORT); - pa_hist[h] = pa; - h = SKIP_PA_HIST(h,1); - pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; - pa_diff = pa_ground - pa_avg; - - /* Check for a significant pressure change */ - if (pa_diff > (BOOST_DETECT << FILTER_SHIFT)) - break; - - if (sample_count < GROUND_AVG * 2) { - if (sample_count < GROUND_AVG) - pa_sum += pa; - ++sample_count; - } else { - pa_ground = pa_sum >> (GROUND_AVG_SHIFT - FILTER_SHIFT); - pa_sum = 0; - sample_count = 0; - } - } - pa_ground >>= FILTER_SHIFT; + ao_microflight(); - /* Go back and find the first sample a decent interval above the ground */ - pa_min = pa_ground - LAND_DETECT; - for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) { - if (pa_hist[i] < pa_min) - break; - } - - /* Log the remaining samples so we get a complete history since leaving the ground */ - for (; i != h; i = SKIP_PA_HIST(i,2)) { - pa = pa_hist[i]; - ao_log_micro_data(); - } - - /* Now sit around until the pressure is stable again and record the max */ - - sample_count = 0; - pa_min = pa_avg; - pa_interval_min = pa_avg; - pa_interval_max = pa_avg; - for (;;) { - time += SAMPLE_SLEEP; - ao_delay_until(time); - if ((sample_count & 3) == 0) - ao_led_on(AO_LED_REPORT); - ao_pa_get(); - if ((sample_count & 3) == 0) - ao_led_off(AO_LED_REPORT); - if (sample_count & 1) - ao_log_micro_data(); - pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; - if (pa_avg < pa_min) - pa_min = pa_avg; - - if (sample_count == (GROUND_AVG - 1)) { - pa_diff = pa_interval_max - pa_interval_min; - - /* Check to see if the pressure is now stable */ - if (pa_diff < (LAND_DETECT << FILTER_SHIFT)) - break; - sample_count = 0; - pa_interval_min = pa_avg; - pa_interval_max = pa_avg; - } else { - if (pa_avg < pa_interval_min) - pa_interval_min = pa_avg; - if (pa_avg > pa_interval_max) - pa_interval_max = pa_avg; - ++sample_count; - } - } - pa_min >>= FILTER_SHIFT; ao_log_micro_save(); ao_compute_height(); ao_report_altitude(); diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h index e408d7c5..382b98d9 100644 --- a/src/micropeak/ao_micropeak.h +++ b/src/micropeak/ao_micropeak.h @@ -18,7 +18,6 @@ #ifndef _AO_MICROPEAK_H_ #define _AO_MICROPEAK_H_ -#define FILTER_SHIFT 3 #define SAMPLE_SLEEP AO_MS_TO_TICKS(96) /* 16 sample, or about two seconds worth */ @@ -32,14 +31,11 @@ #define BOOST_DELAY AO_SEC_TO_TICKS(30) /* Pressure change (in Pa) to detect landing */ -#define LAND_DETECT 12 /* 1m at sea level, 1.2m at 2000m */ +#define LAND_DETECT 24 /* 2m at sea level, 2.4m at 2000m */ /* Current sensor pressure value */ extern uint32_t pa; -/* IIR filtered pressure value */ -extern uint32_t pa_avg; - /* Average pressure value on ground */ extern uint32_t pa_ground; @@ -52,5 +48,31 @@ extern alt_t ground_alt, max_alt; /* max_alt - ground_alt */ extern alt_t ao_max_height; +void +ao_pa_get(void); + +void +ao_microflight(void); + +#define ACCEL_LOCK_PA -20 +#define ACCEL_LOCK_TIME 10 + +extern uint32_t ao_k_pa; /* 24.8 fixed point */ +extern int32_t ao_k_pa_speed; /* 16.16 fixed point */ +extern int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +extern uint32_t ao_pa; /* integer portion */ +extern int16_t ao_pa_speed; /* integer portion */ +extern int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void); + +void +ao_microkalman_predict(void); + +void +ao_microkalman_correct(void); + #endif diff --git a/src/test/Makefile b/src/test/Makefile index 092bf360..87bd70fe 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,14 +1,14 @@ -vpath % ..:../core:../drivers:../util +vpath % ..:../core:../drivers:../util:../micropeak 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_aprs_test + ao_aprs_test ao_micropeak_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -60,4 +60,7 @@ 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 + ./ao_fec_test && ./run-tests + +ao_micropeak_test: ao_micropeak_test.c ao_microflight.c + cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm \ No newline at end of file diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c new file mode 100644 index 00000000..04686402 --- /dev/null +++ b/src/test/ao_micropeak_test.c @@ -0,0 +1,192 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include + +FILE *emulator_in; +char *emulator_app; +char *emulator_name; +char *emulator_info; +uint8_t ao_flight_debug; + +#define AO_FLIGHT_TEST + +typedef int32_t alt_t; + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) + +#define AO_LED_REPORT 0 + +static void ao_led_on(uint8_t led) { +} + +static void ao_led_off(uint8_t led) { +} + +static void ao_delay_until(uint16_t target) { +} + +static uint16_t ao_time(void) { + return 0; +} + +#include "ao_microflight.c" +#include "ao_microkalman.c" +#include "ao_convert_pa.c" + +uint16_t now; +uint8_t running; + +void ao_log_micro_data() { + running = 1; +} + +void +ao_micro_report(void) +{ + if (running) { + alt_t ground = ao_pa_to_altitude(pa_ground); + printf ("%6.2f %10d %10d %10d\n", now / 100.0, + ao_pa_to_altitude(pa) - ground, + ao_pa_to_altitude(ao_pa) - ground, + ao_pa_to_altitude(pa_min) - ground); + } +} + +void +ao_micro_finish(void) +{ + ao_micro_report(); +} + +void +ao_pa_get(void) +{ + char line[4096]; + char *toks[128]; + char *saveptr; + int t, ntok; + static int time_id; + static int pa_id; + double time; + double pressure; + static double last_time; + static int been_here; + static int start_samples; + + if (been_here && start_samples < 100) { + start_samples++; + return; + } + ao_micro_report(); + for (;;) { + if (!fgets(line, sizeof (line), emulator_in)) + exit(0); + for (t = 0; t < 128; t++) { + toks[t] = strtok_r(t ? NULL : line, ", ", &saveptr); + if (!toks[t]) + break; + } + ntok = t; + if (toks[0][0] == '#') { + if (strcmp(toks[0],"#version") == 0) { + for (t = 1; t < ntok; t++) { + if (!strcmp(toks[t], "time")) + time_id = t; + if (!strcmp(toks[t],"pressure")) + pa_id = t; + } + } + continue; + } + time = strtod(toks[time_id],NULL); + pressure = strtod(toks[pa_id],NULL); + if (been_here && time - last_time < 0.1) + continue; + been_here = 1; + last_time = time; + now = floor (time * 100 + 0.5); + pa = pressure; + break; + } +} + +void +ao_dump_state(void) +{ +} + +static const struct option options[] = { + { .name = "summary", .has_arg = 0, .val = 's' }, + { .name = "debug", .has_arg = 0, .val = 'd' }, + { .name = "info", .has_arg = 1, .val = 'i' }, + { 0, 0, 0, 0}, +}; + +void run_flight_fixed(char *name, FILE *f, int summary, char *info) +{ + emulator_name = name; + emulator_in = f; + emulator_info = info; + ao_microflight(); + ao_micro_finish(); +} + +int +main (int argc, char **argv) +{ + int summary = 0; + int c; + int i; + char *info = NULL; + + emulator_app="baro"; + while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) { + switch (c) { + case 's': + summary = 1; + break; + case 'd': + ao_flight_debug = 1; + break; + case 'i': + info = optarg; + break; + } + } + + if (optind == argc) + run_flight_fixed("", stdin, summary, info); + else + for (i = optind; i < argc; i++) { + FILE *f = fopen(argv[i], "r"); + if (!f) { + perror(argv[i]); + continue; + } + run_flight_fixed(argv[i], f, summary, info); + fclose(f); + } + exit(0); +} diff --git a/src/test/plotmicro b/src/test/plotmicro new file mode 100755 index 00000000..cdfcc581 --- /dev/null +++ b/src/test/plotmicro @@ -0,0 +1,14 @@ +#!/bin/sh +for i in "$@"; do +gnuplot -p << EOF & +set title "$i" +set ylabel "height (m)" +set xlabel "time (s)" +set xtics border out nomirror +set ytics border out nomirror +set y2tics border out nomirror +plot "$i" using 1:2 with lines lt 2 axes x1y1 title "raw height",\ + "$i" using 1:3 with lines lt 4 axes x1y1 title "kalman height",\ + "$i" using 1:4 with lines lt 1 axes x1y1 title "max height" +EOF +done -- cgit v1.2.3 From 0e982294961205bef525ecad7172a1f3ab66677f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 4 Feb 2013 09:56:18 -0800 Subject: test: Accept micropeak CSV files for micropeak testing This interpolates the missing values to provide a reasonable testing environment for the Micropeak flight firmware. Signed-off-by: Keith Packard --- src/test/Makefile | 4 ++-- src/test/ao_micropeak_test.c | 40 ++++++++++++++++++++++++++++++++++------ src/test/plotmicro | 4 +++- 3 files changed, 39 insertions(+), 9 deletions(-) (limited to 'src/test/Makefile') diff --git a/src/test/Makefile b/src/test/Makefile index 87bd70fe..1c2d771e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -62,5 +62,5 @@ ao_aprs_data.wav: ao_aprs_test check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests ./ao_fec_test && ./run-tests -ao_micropeak_test: ao_micropeak_test.c ao_microflight.c - cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm \ No newline at end of file +ao_micropeak_test: ao_micropeak_test.c ao_microflight.c ao_kalman.h + cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c index 04686402..5961bd93 100644 --- a/src/test/ao_micropeak_test.c +++ b/src/test/ao_micropeak_test.c @@ -67,10 +67,11 @@ ao_micro_report(void) { if (running) { alt_t ground = ao_pa_to_altitude(pa_ground); - printf ("%6.2f %10d %10d %10d\n", now / 100.0, + printf ("%6.3f %10d %10d %10d %10d %10d\n", now / 100.0, ao_pa_to_altitude(pa) - ground, ao_pa_to_altitude(ao_pa) - ground, - ao_pa_to_altitude(pa_min) - ground); + ao_pa_to_altitude(pa_min) - ground, + ao_pa_speed, ao_pa_accel); } } @@ -92,14 +93,24 @@ ao_pa_get(void) double time; double pressure; static double last_time; + static double last_pressure; static int been_here; static int start_samples; + static int is_mp; + static int use_saved; if (been_here && start_samples < 100) { start_samples++; return; } ao_micro_report(); + if (use_saved) { + pa = last_pressure; + now = last_time; + use_saved = 0; +// printf ("use saved %d %d\n", now, pa); + return; + } for (;;) { if (!fgets(line, sizeof (line), emulator_in)) exit(0); @@ -119,15 +130,32 @@ ao_pa_get(void) } } continue; + } else if (!strcmp(toks[0], "Time")) { + time_id = 0; + pa_id = 1; + is_mp = 1; + continue; } time = strtod(toks[time_id],NULL); pressure = strtod(toks[pa_id],NULL); - if (been_here && time - last_time < 0.1) + time *= 100; + if (been_here && time - last_time < 0.096 * 100) continue; - been_here = 1; + if (is_mp && been_here) { + double avg_pressure = (pressure + last_pressure) / 2.0; + double avg_time = (time + last_time) / 2.0; + + now = avg_time; + pa = avg_pressure; +// printf ("new %d %d\n", now, pa); + use_saved = 1; + } else { + now = floor (time + 0.5); + pa = pressure; + } + last_pressure = pressure; last_time = time; - now = floor (time * 100 + 0.5); - pa = pressure; + been_here = 1; break; } } diff --git a/src/test/plotmicro b/src/test/plotmicro index cdfcc581..bb8f4d1d 100755 --- a/src/test/plotmicro +++ b/src/test/plotmicro @@ -3,12 +3,14 @@ for i in "$@"; do gnuplot -p << EOF & set title "$i" set ylabel "height (m)" +set y2label "accel (m/s²)" set xlabel "time (s)" set xtics border out nomirror set ytics border out nomirror set y2tics border out nomirror plot "$i" using 1:2 with lines lt 2 axes x1y1 title "raw height",\ "$i" using 1:3 with lines lt 4 axes x1y1 title "kalman height",\ - "$i" using 1:4 with lines lt 1 axes x1y1 title "max height" + "$i" using 1:4 with lines lt 1 axes x1y1 title "max height",\ + "$i" using 1:6 with lines lt 3 axes x1y2 title "pa accel" EOF done -- cgit v1.2.3 From 5e53a485310cc11e6add077fb4bd0b0267734ff0 Mon Sep 17 00:00:00 2001 From: Mike Beattie Date: Fri, 15 Feb 2013 21:59:08 +1300 Subject: all: clean up .gitignore files and Makefile clean targets Signed-off-by: Mike Beattie --- .gitignore | 1 + altosdroid/.gitignore | 1 + altoslib/.gitignore | 4 ++-- altosui/.gitignore | 1 + altosui/Makefile.am | 2 +- altosuilib/.gitignore | 4 ++++ micropeak/.gitignore | 2 ++ micropeak/Makefile.am | 3 ++- src/test/.gitignore | 4 ++++ src/test/Makefile | 2 +- 10 files changed, 19 insertions(+), 5 deletions(-) create mode 100644 altosuilib/.gitignore (limited to 'src/test/Makefile') diff --git a/.gitignore b/.gitignore index 9f33ea3c..b7b8fda1 100644 --- a/.gitignore +++ b/.gitignore @@ -35,6 +35,7 @@ ao-tools/ao-load/ao-load ao-tools/ao-postflight/ao-postflight ao-tools/ao-rawload/ao-rawload ao-tools/ao-send-telem/ao-send-telem +ao-tools/ao-sky-flash/ao-sky-flash ao-tools/ao-view/ao-view ao-view/Makefile ao-view/ao-view diff --git a/altosdroid/.gitignore b/altosdroid/.gitignore index e3acba40..0d2ee6e0 100644 --- a/altosdroid/.gitignore +++ b/altosdroid/.gitignore @@ -1,4 +1,5 @@ local.properties bin gen +libs src/org/altusmetrum/AltosDroid/BuildInfo.java diff --git a/altoslib/.gitignore b/altoslib/.gitignore index a71d076c..ff0fd710 100644 --- a/altoslib/.gitignore +++ b/altoslib/.gitignore @@ -1,3 +1,3 @@ bin -classAltosLib.stamp -AltosLib.jar +classaltoslib.stamp +altoslib*.jar diff --git a/altosui/.gitignore b/altosui/.gitignore index 6d2d8c23..f8554c6a 100644 --- a/altosui/.gitignore +++ b/altosui/.gitignore @@ -12,6 +12,7 @@ altosui altosui-test altosui-jdb classaltosui.stamp +altos-windows.nsi Altos-Linux-*.tar.bz2 Altos-Mac-*.zip Altos-Windows-*.exe diff --git a/altosui/Makefile.am b/altosui/Makefile.am index 8ae1a72e..96cf77f2 100644 --- a/altosui/Makefile.am +++ b/altosui/Makefile.am @@ -182,7 +182,7 @@ all-local: classes/altosui $(JAR) altosui altosui-test altosui-jdb clean-local: -rm -rf classes $(JAR) $(FATJAR) \ $(LINUX_DIST) $(MACOSX_DIST) windows $(WINDOWS_DIST) $(ALTOSLIB_CLASS) $(ALTOSUILIB_CLASS) $(FREETTS_CLASS) \ - $(JFREECHART_CLASS) $(JCOMMON_CLASS) $(LIBALTOS) Manifest.txt Manifest-fat.txt altos-windows.log \ + $(JFREECHART_CLASS) $(JCOMMON_CLASS) $(LIBALTOS) Manifest.txt Manifest-fat.txt altos-windows.log altos-windows.nsi \ altosui altosui-test altosui-jdb macosx linux if FATINSTALL diff --git a/altosuilib/.gitignore b/altosuilib/.gitignore new file mode 100644 index 00000000..4ad8a77a --- /dev/null +++ b/altosuilib/.gitignore @@ -0,0 +1,4 @@ +AltosUIVersion.java +bin +classaltosuilib.stamp +altosuilib*.jar diff --git a/micropeak/.gitignore b/micropeak/.gitignore index ab80492b..6a6475f0 100644 --- a/micropeak/.gitignore +++ b/micropeak/.gitignore @@ -7,6 +7,7 @@ micropeak micropeak-test micropeak-jdb micropeak-windows.log +micropeak-windows.nsi MicroPeak-Linux-* MicroPeak-Mac-* MicroPeak-Windows-* @@ -17,3 +18,4 @@ linux macosx CDM*.exe FTDI*.dmg +Info.plist diff --git a/micropeak/Makefile.am b/micropeak/Makefile.am index d45ecab9..098a00fb 100644 --- a/micropeak/Makefile.am +++ b/micropeak/Makefile.am @@ -78,7 +78,8 @@ clean-local: $(ALTOSLIB_CLASS) \ $(ALTOSUILIB_CLASS) \ $(JFREECHART_CLASS) $(JCOMMON_CLASS) $(LIBALTOS) Manifest.txt Manifest-fat.txt \ - micropeak micropeak-test micropeak-jdb macosx linux windows micropeak-windows.log + micropeak micropeak-test micropeak-jdb macosx linux windows micropeak-windows.log \ + micropeak-windows.nsi LINUX_DIST=MicroPeak-Linux-$(VERSION).tar.bz2 MACOSX_DIST=MicroPeak-Mac-$(VERSION).dmg diff --git a/src/test/.gitignore b/src/test/.gitignore index 5d528ab9..8d79d168 100644 --- a/src/test/.gitignore +++ b/src/test/.gitignore @@ -1,3 +1,5 @@ +ao_aprs_data.wav +ao_aprs_test ao_flight_test ao_flight_test_baro ao_flight_test_accel @@ -6,4 +8,6 @@ ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test +ao_flight_test_mm ao_flight_test_noisy_accel +ao_micropeak_test diff --git a/src/test/Makefile b/src/test/Makefile index 1c2d771e..fccc7937 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -13,7 +13,7 @@ CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav clean: - rm -f $(PROGS) run-out.baro run-out.full + rm -f $(PROGS) ao_aprs_data.wav run-out.baro run-out.full install: -- cgit v1.2.3 From e14834817f78a04b4d9b44a8373119dffd42c966 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 27 Mar 2013 01:12:33 -0700 Subject: altos: Add SDCARD and FAT16 filesystem support This adds a fairly primitive FAT16 file system implementation along with support for SD cards. Signed-off-by: Keith Packard --- src/drivers/ao_bufio.c | 298 +++++++++++++++++++++ src/drivers/ao_bufio.h | 36 +++ src/drivers/ao_fat.c | 674 ++++++++++++++++++++++++++++++++++++++++++++++++ src/drivers/ao_fat.h | 73 ++++++ src/drivers/ao_sdcard.c | 398 ++++++++++++++++++++++++++++ src/drivers/ao_sdcard.h | 75 ++++++ src/test/Makefile | 5 +- src/test/ao_fat_test.c | 118 +++++++++ 8 files changed, 1676 insertions(+), 1 deletion(-) create mode 100644 src/drivers/ao_bufio.c create mode 100644 src/drivers/ao_bufio.h create mode 100644 src/drivers/ao_fat.c create mode 100644 src/drivers/ao_fat.h create mode 100644 src/drivers/ao_sdcard.c create mode 100644 src/drivers/ao_sdcard.h create mode 100644 src/test/ao_fat_test.c (limited to 'src/test/Makefile') diff --git a/src/drivers/ao_bufio.c b/src/drivers/ao_bufio.c new file mode 100644 index 00000000..9a5e801b --- /dev/null +++ b/src/drivers/ao_bufio.c @@ -0,0 +1,298 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef AO_FAT_TEST +#include "ao.h" +#endif + +#include "ao_sdcard.h" +#include "ao_bufio.h" + +#define AO_NUM_BUF 4 +#define AO_BUFSIZ 512 + +struct ao_bufio { + uint32_t block; + int16_t seqno; + uint8_t busy; + uint8_t dirty; +}; + +static struct ao_bufio ao_bufio[AO_NUM_BUF]; +static uint8_t ao_buffer[AO_NUM_BUF][AO_BUFSIZ]; +static int16_t ao_seqno; +static uint8_t ao_bufio_mutex; + +#if 0 +#define DBG(...) printf(__VA_ARGS__) +#else +#define DBG(...) +#endif + +static inline void +ao_bufio_lock(void) +{ + ao_mutex_get(&ao_bufio_mutex); +} + +static inline void +ao_bufio_unlock(void) +{ + ao_mutex_put(&ao_bufio_mutex); +} + +static inline int16_t +ao_seqno_age(int16_t s) +{ + return ao_seqno - s; +} + +static inline int16_t +ao_seqno_next(void) +{ + return ++ao_seqno; +} + +static inline int +ao_seqno_older(int16_t a, int16_t b) +{ + return ao_seqno_age(a) > ao_seqno_age(b); +} + +static inline void +ao_validate_bufno(int b) +{ + if (b < 0 || AO_NUM_BUF <= b) + ao_panic(AO_PANIC_BUFIO); +} + +static inline int +ao_buf_to_num(uint8_t *buf) +{ + int b = (buf - &ao_buffer[0][0]) / AO_BUFSIZ; + + ao_validate_bufno(b); + return b; +} + +static inline int +ao_bufio_to_num(struct ao_bufio *bufio) +{ + int b = (bufio - ao_bufio); + + ao_validate_bufno(b); + return b; +} + +static inline struct ao_bufio * +ao_buf_to_bufio(uint8_t *buf) +{ + int b = ao_buf_to_num(buf); + struct ao_bufio *bufio; + + bufio = &ao_bufio[b]; + DBG ("buf %08x is %d bufio %08x\n", buf, b, bufio); + return bufio; +} + +static inline uint8_t * +ao_bufio_to_buf(struct ao_bufio *bufio) +{ + int b = ao_bufio_to_num(bufio); + uint8_t *buf; + + buf = &ao_buffer[b][0]; + DBG ("bufio %08x is %d buf %08x\n", bufio, b, buf); + return buf; +} + +/* + * Write a buffer to storage if it is dirty + */ +static void +ao_bufio_write(struct ao_bufio *bufio) +{ + if (bufio->dirty) { + ao_sdcard_write_block(bufio->block, ao_bufio_to_buf(bufio)); + bufio->dirty = 0; + } +} + +/* + * Read a buffer from storage + */ +static uint8_t +ao_bufio_read(struct ao_bufio *bufio) +{ + uint8_t *buf = ao_bufio_to_buf(bufio); + + return ao_sdcard_read_block(bufio->block, buf); +} + +/* + * Find a buffer containing the specified block + */ +static struct ao_bufio * +ao_bufio_find_block(uint32_t block) +{ + int b; + + for (b = 0; b < AO_NUM_BUF; b++) { + struct ao_bufio *bufio = &ao_bufio[b]; + if (bufio->block == block) { + DBG ("Found existing buffer %d (seqno %d)\n", + ao_bufio_to_num(bufio), bufio->seqno); + return bufio; + } + } + return NULL; +} + +/* + * Find the least recently used idle buffer + */ +static struct ao_bufio * +ao_bufio_find_idle(void) +{ + int b; + struct ao_bufio *oldest = NULL; + + for (b = 0; b < AO_NUM_BUF; b++) { + struct ao_bufio *bufio = &ao_bufio[b]; + if (!bufio->busy) + if (!oldest || ao_seqno_older(bufio->seqno, oldest->seqno)) + oldest = bufio; + } + if (oldest) + DBG ("Using idle buffer %d (seqno %d)\n", + ao_bufio_to_num(oldest), oldest->seqno); + return oldest; +} + +/* + * Return a pointer to a buffer containing + * the contents of the specified block + */ +uint8_t * +ao_bufio_get(uint32_t block) +{ + struct ao_bufio *bufio; + uint8_t *buf = NULL; + + ao_bufio_lock(); + bufio = ao_bufio_find_block(block); + if (!bufio) { + bufio = ao_bufio_find_idle(); + if (bufio) { + ao_bufio_write(bufio); + bufio->block = block; + DBG ("read buffer\n"); + if (!ao_bufio_read(bufio)) { + bufio->block = 0xffffffff; + bufio = NULL; + } + } + } + if (bufio) { + bufio->busy++; + buf = ao_bufio_to_buf(bufio); + } + ao_bufio_unlock(); + return buf; +} + +/* + * Release a buffer, marking it dirty + * if it has been written to + */ +void +ao_bufio_put(uint8_t *buf, uint8_t write) +{ + struct ao_bufio *bufio; + + ao_bufio_lock(); + bufio = ao_buf_to_bufio(buf); + + DBG ("idle buffer %d write %d\n", ao_bufio_to_num(bufio), write); + bufio->dirty |= write; + if (!--bufio->busy) { + bufio->seqno = ao_seqno_next(); + DBG ("not busy, seqno %d\n", bufio->seqno); + } + ao_bufio_unlock(); +} + +/* + * Flush a single buffer immediately. Useful + * if write order is important + */ +void +ao_bufio_flush_one(uint8_t *buf) +{ + ao_bufio_lock(); + ao_bufio_write(ao_buf_to_bufio(buf)); + ao_bufio_unlock(); +} + +/* + * Flush all buffers to storage + */ +void +ao_bufio_flush(void) +{ + int b; + + ao_bufio_lock(); + for (b = 0; b < AO_NUM_BUF; b++) + ao_bufio_write(&ao_bufio[b]); + ao_bufio_unlock(); +} + +static void +ao_bufio_test_read(void) +{ + uint8_t *buf; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if ((buf = ao_bufio_get(ao_cmd_lex_u32))) { + int i; + for (i = 0; i < 512; i++) { + printf (" %02x", buf[i]); + if ((i & 0xf) == 0xf) + printf("\n"); + } + ao_bufio_put(buf, 0); + } +} + +static const struct ao_cmds ao_bufio_cmds[] = { + { ao_bufio_test_read, "q\0Test bufio read" }, + { 0, NULL }, +}; + +void +ao_bufio_init(void) +{ + int b; + + for (b = 0; b < AO_NUM_BUF; b++) + ao_bufio[b].block = 0xffffffff; + ao_sdcard_init(); + + ao_cmd_register(&ao_bufio_cmds[0]); +} diff --git a/src/drivers/ao_bufio.h b/src/drivers/ao_bufio.h new file mode 100644 index 00000000..c3bee906 --- /dev/null +++ b/src/drivers/ao_bufio.h @@ -0,0 +1,36 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef _AO_BUFIO_H_ +#define _AO_BUFIO_H_ + +uint8_t * +ao_bufio_get(uint32_t block); + +void +ao_bufio_put(uint8_t *buf, uint8_t write); + +void +ao_bufio_flush_one(uint8_t *buf); + +void +ao_bufio_flush(void); + +void +ao_bufio_init(void); + +#endif /* _AO_BUFIO_H_ */ diff --git a/src/drivers/ao_fat.c b/src/drivers/ao_fat.c new file mode 100644 index 00000000..a1476168 --- /dev/null +++ b/src/drivers/ao_fat.c @@ -0,0 +1,674 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef AO_FAT_TEST +#include "ao.h" +#endif + +#include "ao_fat.h" +#include "ao_bufio.h" + +/* Partition information, block numbers */ + +static uint8_t partition_type; +static uint32_t partition_start, partition_end; + +/* File system parameters */ +static uint8_t sectors_per_cluster; +static uint32_t bytes_per_cluster; +static uint16_t reserved_sector_count; +static uint8_t number_fat; +static uint16_t root_entries; +static uint16_t sectors_per_fat; +static uint32_t fat_start; +static uint32_t root_start; +static uint32_t data_start; + +static uint32_t +get_u32(uint8_t *base) +{ + return ((uint32_t) base[0] | + ((uint32_t) base[1] << 8) | + ((uint32_t) base[2] << 16) | + ((uint32_t) base[3] << 24)); +} + +static void +put_u32(uint8_t *base, uint32_t value) +{ + base[0] = value; + base[1] = value >> 8; + base[2] = value >> 16; + base[3] = value >> 24; +} + +static uint16_t +get_u16(uint8_t *base) +{ + return ((uint16_t) base[0] | ((uint16_t) base[1] << 8)); +} + +static void +put_u16(uint8_t *base, uint16_t value) +{ + base[0] = value; + base[1] = value >> 8; +} + +static uint8_t +ao_fat_cluster_valid(uint16_t cluster) +{ + return (2 <= cluster && cluster < 0xfff0); +} + +/* Start using a block */ +static uint8_t * +ao_fat_block_get(uint32_t block) +{ + block += partition_start; + if (block >= partition_end) + return NULL; + return ao_bufio_get(block); +} + +/* Finish using a block, 'w' is 1 if modified */ +#define ao_fat_block_put(b,w) ao_bufio_put(b,w) + +/* Start using a root directory entry */ +static uint8_t * +ao_fat_root_get(uint16_t e) +{ + uint32_t byte = e * 0x20; + uint32_t sector = byte >> 9; + uint16_t offset = byte & 0x1ff; + uint8_t *buf; + + buf = ao_fat_block_get(root_start + sector); + if (!buf) + return NULL; + return buf + offset; +} + +/* Finish using a root directory entry, 'w' is 1 if modified */ +static void +ao_fat_root_put(uint8_t *root, uint16_t e, uint8_t write) +{ + uint16_t offset = ((e * 0x20) & 0x1ff); + uint8_t *buf = root - offset; + + ao_fat_block_put(buf, write); +} + +/* Get the next cluster entry in the chain */ +static uint16_t +ao_fat_entry_read(uint16_t cluster) +{ + uint32_t sector; + uint16_t offset; + uint8_t *buf; + uint16_t ret; + + if (!ao_fat_cluster_valid(cluster)) + return 0xfff7; + + cluster -= 2; + sector = cluster >> 8; + offset = (cluster << 1) & 0x1ff; + buf = ao_fat_block_get(fat_start + sector); + if (!buf) + return 0; + ret = buf[offset] | (buf[offset+1] << 8); + ao_fat_block_put(buf, 0); + return ret; +} + +static uint16_t +ao_fat_entry_replace(uint16_t cluster, uint16_t new_value) +{ + uint32_t sector; + uint16_t offset; + uint8_t *buf; + uint16_t ret; + uint8_t other_fats; + + if (!ao_fat_cluster_valid(cluster)) + return 0; + + cluster -= 2; + sector = cluster >> 8; + offset = (cluster << 1) & 0x1ff; + buf = ao_fat_block_get(fat_start + sector); + if (!buf) + return 0; + ret = get_u16(buf + offset); + put_u16(buf + offset, new_value); + ao_fat_block_put(buf, 1); + for (other_fats = 1; other_fats < number_fat; other_fats++) { + buf = ao_fat_block_get(fat_start + other_fats * sectors_per_fat); + if (buf) { + put_u16(buf + offset, new_value); + ao_fat_block_put(buf, 1); + } + } + return ret; + +} + +static void +ao_fat_clear_cluster_chain(uint16_t cluster) +{ + while (ao_fat_cluster_valid(cluster)) + cluster = ao_fat_entry_replace(cluster, 0x0000); +} + +static uint16_t +ao_fat_cluster_seek(uint16_t cluster, uint16_t distance) +{ + while (distance) { + cluster = ao_fat_entry_read(cluster); + if (!ao_fat_cluster_valid(cluster)) + break; + distance--; + } + return cluster; +} + +static uint32_t +ao_fat_sector_seek(uint16_t cluster, uint32_t sector) +{ + cluster = ao_fat_cluster_seek(cluster, sector / sectors_per_cluster); + if (!ao_fat_cluster_valid(cluster)) + return 0xffffffff; + return data_start + (cluster-2) * sectors_per_cluster + sector % sectors_per_cluster; +} + +/* Load the boot block and find the first partition */ +static uint8_t +ao_fat_setup_partition(void) +{ + uint8_t *mbr; + uint8_t *partition; + uint32_t partition_size; + + mbr = ao_bufio_get(0); + if (!mbr) + return 0; + + /* Check the signature */ + if (mbr[0x1fe] != 0x55 || mbr[0x1ff] != 0xaa) { + printf ("Invalid MBR signature %02x %02x\n", + mbr[0x1fe], mbr[0x1ff]); + ao_bufio_put(mbr, 0); + return 0; + } + + /* Just use the first partition */ + partition = &mbr[0x1be]; + + partition_type = partition[4]; + switch (partition_type) { + case 4: /* FAT16 up to 32M */ + case 6: /* FAT16 over 32M */ + break; + case 0x0b: /* FAT32 up to 2047GB */ + case 0x0c: /* FAT32 LBA */ + break; + default: + printf ("Invalid partition type %02x\n", partition_type); + ao_bufio_put(mbr, 0); + return 0; + } + + partition_start = get_u32(partition+8); + partition_size = get_u32(partition+12); + if (partition_size == 0) { + printf ("Zero-sized partition\n"); + ao_bufio_put(mbr, 0); + return 0; + } + partition_end = partition_start + partition_size; + printf ("Partition type %02x start %08x end %08x\n", + partition_type, partition_start, partition_end); + ao_bufio_put(mbr, 0); + return 1; +} + +static uint8_t +ao_fat_setup_fs(void) +{ + uint8_t *boot = ao_fat_block_get(0); + + if (!boot) + return 0; + + /* Check the signature */ + if (boot[0x1fe] != 0x55 || boot[0x1ff] != 0xaa) { + printf ("Invalid BOOT signature %02x %02x\n", + boot[0x1fe], boot[0x1ff]); + ao_bufio_put(boot, 0); + return 0; + } + + /* Check the sector size */ + if (get_u16(boot + 0xb) != 0x200) { + printf ("Invalid sector size %d\n", + get_u16(boot + 0xb)); + ao_bufio_put(boot, 0); + return 0; + } + + sectors_per_cluster = boot[0xd]; + bytes_per_cluster = sectors_per_cluster << 9; + reserved_sector_count = get_u16(boot+0xe); + number_fat = boot[0x10]; + root_entries = get_u16(boot + 0x11); + sectors_per_fat = get_u16(boot+0x16); + + printf ("sectors per cluster %d\n", sectors_per_cluster); + printf ("reserved sectors %d\n", reserved_sector_count); + printf ("number of FATs %d\n", number_fat); + printf ("root entries %d\n", root_entries); + printf ("sectors per fat %d\n", sectors_per_fat); + + fat_start = reserved_sector_count;; + root_start = fat_start + number_fat * sectors_per_fat; + data_start = root_start + ((root_entries * 0x20 + 0x1ff) >> 9); + + printf ("fat start %d\n", fat_start); + printf ("root start %d\n", root_start); + printf ("data start %d\n", data_start); + + return 1; +} + +static uint8_t +ao_fat_setup(void) +{ + if (!ao_fat_setup_partition()) + return 0; + if (!ao_fat_setup_fs()) + return 0; + return 1; +} + +/* + * Low-level directory operations + */ + +/* + * Basic file operations + */ + +static struct ao_fat_dirent ao_file_dirent; +static uint32_t ao_file_offset; + +static uint32_t +ao_file_offset_to_sector(uint32_t offset) +{ + if (offset > ao_file_dirent.size) + return 0xffffffff; + return ao_fat_sector_seek(ao_file_dirent.cluster, offset >> 9); +} + +uint8_t +ao_fat_open(char name[11]) +{ + uint16_t entry = 0; + struct ao_fat_dirent dirent; + + while (ao_fat_readdir(&entry, &dirent)) { + if (!memcmp(name, dirent.name, 11)) { + ao_file_dirent = dirent; + ao_file_offset = 0; + return 1; + } + } + return 0; +} + + + +static uint8_t +ao_fat_set_size(uint32_t size) +{ + uint16_t clear_cluster = 0; + uint8_t *dent; + uint16_t first_cluster; + + first_cluster = ao_file_dirent.cluster; + printf ("set size to %d\n", size); + if (size == ao_file_dirent.size) + return 1; + if (size == 0) { + printf ("erase file\n"); + clear_cluster = ao_file_dirent.cluster; + first_cluster = 0; + } else { + uint16_t new_num; + uint16_t old_num; + + new_num = (size + bytes_per_cluster - 1) / bytes_per_cluster; + old_num = (ao_file_dirent.size + bytes_per_cluster - 1) / bytes_per_cluster; + if (new_num < old_num) { + uint16_t last_cluster; + + printf("Remove %d clusters\n", old_num - new_num); + /* Go find the last cluster we want to preserve in the file */ + last_cluster = ao_fat_cluster_seek(ao_file_dirent.cluster, new_num - 1); + + printf ("Last cluster is now %04x\n", last_cluster); + /* Rewrite that cluster entry with 0xffff to mark the end of the chain */ + clear_cluster = ao_fat_entry_replace(last_cluster, 0xffff); + } else if (new_num > old_num) { + uint16_t need; + uint16_t free; + uint16_t last_cluster; + + if (old_num) + last_cluster = ao_fat_cluster_seek(ao_file_dirent.cluster, old_num - 1); + else + last_cluster = 0; + + need = new_num - old_num; + printf ("Need %d clusters\n", need); + /* See if there are enough free clusters in the file system */ + for (free = 2; need > 0 && (free - 2) < sectors_per_fat * 256; free++) { + if (!ao_fat_entry_read(free)) { + printf ("\tCluster %04x available\n", free); + need--; + } + } + /* Still need some, tell the user that we've failed */ + if (need) { + printf ("File system full\n"); + return 0; + } + + need = new_num - old_num; + /* Now go allocate those clusters */ + for (free = 2; need > 0 && (free - 2) < sectors_per_fat * 256; free++) { + if (!ao_fat_entry_read(free)) { + printf ("\tAllocate %04x\n", free); + if (last_cluster) + ao_fat_entry_replace(last_cluster, free); + else + first_cluster = free; + last_cluster = free; + need--; + } + } + /* Mark the new end of the chain */ + ao_fat_entry_replace(last_cluster, 0xffff); + } + } + + /* Deallocate clusters off the end of the file */ + if (ao_fat_cluster_valid(clear_cluster)) { + printf ("Clear clusters starting with %04x\n", clear_cluster); + ao_fat_clear_cluster_chain(clear_cluster); + } + + dent = ao_fat_root_get(ao_file_dirent.entry); + if (!dent) + return 0; + put_u32(dent + 0x1c, size); + put_u16(dent + 0x1a, first_cluster); + ao_fat_root_put(dent, ao_file_dirent.entry, 1); + ao_file_dirent.size = size; + ao_file_dirent.cluster = first_cluster; + return 1; +} + +uint8_t +ao_fat_creat(char name[11]) +{ + uint16_t entry; + + if (ao_fat_open(name)) + return ao_fat_set_size(0); + + for (entry = 0; entry < root_entries; entry++) { + uint8_t *dent = ao_fat_root_get(entry); + + if (dent[0] == AO_FAT_DENT_EMPTY || + dent[0] == AO_FAT_DENT_END) { + memmove(dent, name, 11); + dent[0x0b] = 0x00; + dent[0x0c] = 0x00; + dent[0x0d] = 0x00; + /* XXX fix time */ + put_u16(dent + 0x0e, 0); + /* XXX fix date */ + put_u16(dent + 0x10, 0); + /* XXX fix date */ + put_u16(dent + 0x12, 0); + /* XXX FAT32 high cluster bytes */ + put_u16(dent + 0x14, 0); + /* XXX fix time */ + put_u16(dent + 0x16, 0); + /* XXX fix date */ + put_u16(dent + 0x18, 0); + /* cluster number */ + put_u16(dent + 0x1a, 0); + /* size */ + put_u32(dent + 0x1c, 0); + ao_fat_root_put(dent, entry, 1); + return ao_fat_open(name); + } + } + return 0; +} + +void +ao_fat_close(void) +{ + memset(&ao_file_dirent, '\0', sizeof (struct ao_fat_dirent)); + ao_bufio_flush(); +} + +int +ao_fat_read(uint8_t *dest, int len) +{ + uint32_t sector; + uint16_t this_time; + uint16_t offset; + uint8_t *buf; + int ret = 0; + + if (ao_file_offset + len > ao_file_dirent.size) + len = ao_file_dirent.size - ao_file_offset; + + while (len) { + offset = ao_file_offset & 0x1ff; + if (offset + len < 512) + this_time = len; + else + this_time = 512 - offset; + + sector = ao_file_offset_to_sector(ao_file_offset); + if (sector == 0xffffffff) + break; + buf = ao_fat_block_get(sector); + if (!buf) + break; + memcpy(dest, buf + offset, this_time); + ao_fat_block_put(buf, 0); + + ret += this_time; + len -= this_time; + dest += this_time; + ao_file_offset += this_time; + } + return ret; +} + +int +ao_fat_write(uint8_t *src, int len) +{ + uint32_t sector; + uint16_t this_time; + uint16_t offset; + uint8_t *buf; + int ret = 0; + + if (ao_file_offset + len > ao_file_dirent.size) { + if (!ao_fat_set_size(ao_file_offset + len)) + return 0; + } + + while (len) { + offset = ao_file_offset & 0x1ff; + if (offset + len < 512) + this_time = len; + else + this_time = 512 - offset; + + sector = ao_file_offset_to_sector(ao_file_offset); + if (sector == 0xffffffff) + break; + buf = ao_fat_block_get(sector); + if (!buf) + break; + memcpy(buf + offset, src, this_time); + ao_fat_block_put(buf, 1); + + ret += this_time; + len -= this_time; + src += this_time; + ao_file_offset += this_time; + } + return 0; +} + +uint32_t +ao_fat_seek(int32_t pos, uint8_t whence) +{ + switch (whence) { + case AO_FAT_SEEK_SET: + ao_file_offset = pos; + break; + case AO_FAT_SEEK_CUR: + ao_file_offset += pos; + break; + case AO_FAT_SEEK_END: + ao_file_offset = ao_file_dirent.size + pos; + break; + } + if (ao_file_offset > ao_file_dirent.size) + ao_fat_set_size(ao_file_offset); + return ao_file_offset; +} + +uint8_t +ao_fat_unlink(char name[11]) +{ + uint16_t entry = 0; + struct ao_fat_dirent dirent; + + while (ao_fat_readdir(&entry, &dirent)) { + if (memcmp(name, dirent.name, 11) == 0) { + uint8_t *next; + uint8_t *ent; + uint8_t delete; + ao_fat_clear_cluster_chain(dirent.cluster); + next = ao_fat_root_get(dirent.entry + 1); + if (next && next[0] != AO_FAT_DENT_END) + delete = AO_FAT_DENT_EMPTY; + else + delete = AO_FAT_DENT_END; + if (next) + ao_fat_root_put(next, dirent.entry + 1, 0); + ent = ao_fat_root_get(dirent.entry); + if (ent) { + memset(ent, '\0', 0x20); + *ent = delete; + ao_fat_root_put(ent, dirent.entry, 1); + } + ao_bufio_flush(); + return 1; + } + } + return 0; +} + +uint8_t +ao_fat_rename(char old[11], char new[11]) +{ + return 0; +} + +uint8_t +ao_fat_readdir(uint16_t *entry, struct ao_fat_dirent *dirent) +{ + uint8_t *dent; + + if (*entry >= root_entries) + return 0; + for (;;) { + dent = ao_fat_root_get(*entry); + + if (dent[0] == AO_FAT_DENT_END) { + ao_fat_root_put(dent, *entry, 0); + return 0; + } + if (dent[0] != AO_FAT_DENT_EMPTY && + (dent[0x0b] & (AO_FAT_FILE_DIRECTORY|AO_FAT_FILE_VOLUME_LABEL)) == 0) + break; + ao_fat_root_put(dent, *entry, 0); + (*entry)++; + } + memcpy(dirent->name, dent, 11); + dirent->attr = dent[0xb]; + dirent->size = get_u32(dent+0x1c); + dirent->cluster = get_u16(dent+0x1a); + dirent->entry = *entry; + ao_fat_root_put(dent, *entry, 0); + (*entry)++; + return 1; +} + +static void +ao_fat_list(void) +{ + uint16_t entry = 0; + struct ao_fat_dirent dirent; + + while (ao_fat_readdir(&entry, &dirent)) { + printf ("%-8.8s.%-3.3s %02x %d\n", + dirent.name, dirent.name + 8, dirent.attr, dirent.size); + } +} + +static void +ao_fat_test(void) +{ + ao_fat_setup(); + ao_fat_list(); +} + +static const struct ao_cmds ao_fat_cmds[] = { + { ao_fat_test, "F\0Test FAT" }, + { 0, NULL }, +}; + +void +ao_fat_init(void) +{ + ao_bufio_init(); + ao_cmd_register(&ao_fat_cmds[0]); +} + diff --git a/src/drivers/ao_fat.h b/src/drivers/ao_fat.h new file mode 100644 index 00000000..2bf6222e --- /dev/null +++ b/src/drivers/ao_fat.h @@ -0,0 +1,73 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef _AO_FAT_H_ +#define _AO_FAT_H_ + +void +ao_fat_init(void); + +#define AO_FAT_FILE_READ_ONLY 0x01 +#define AO_FAT_FILE_HIDDEN 0x02 +#define AO_FAT_FILE_SYSTEM 0x04 +#define AO_FAT_FILE_VOLUME_LABEL 0x08 +#define AO_FAT_FILE_DIRECTORY 0x10 +#define AO_FAT_FILE_ARCHIVE 0x20 + +#define AO_FAT_DENT_EMPTY 0xe5 +#define AO_FAT_DENT_END 0x00 + +uint8_t +ao_fat_open(char name[11]); + +uint8_t +ao_fat_creat(char name[11]); + +void +ao_fat_close(void); + +int +ao_fat_read(uint8_t *dest, int len); + +int +ao_fat_write(uint8_t *buf, int len); + +#define AO_FAT_SEEK_SET 0 +#define AO_FAT_SEEK_CUR 1 +#define AO_FAT_SEEK_END 2 + +uint32_t +bao_fat_seek(int32_t pos, uint8_t whence); + +uint8_t +ao_fat_unlink(char name[11]); + +uint8_t +ao_fat_rename(char old[11], char new[11]); + +struct ao_fat_dirent { + char name[11]; + uint8_t attr; + uint32_t size; + uint16_t cluster; + uint16_t entry; +}; + +uint8_t +ao_fat_readdir(uint16_t *entry, struct ao_fat_dirent *dirent); + +#endif /* _AO_FAT_H_ */ diff --git a/src/drivers/ao_sdcard.c b/src/drivers/ao_sdcard.c new file mode 100644 index 00000000..2174af1e --- /dev/null +++ b/src/drivers/ao_sdcard.c @@ -0,0 +1,398 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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 "ao.h" +#include "ao_sdcard.h" + +#define ao_sdcard_get_slow() ao_spi_get(AO_SDCARD_SPI_BUS, AO_SPI_SPEED_250kHz) +#define ao_sdcard_get() ao_spi_get(AO_SDCARD_SPI_BUS, AO_SPI_SPEED_FAST) +#define ao_sdcard_put() ao_spi_put(AO_SDCARD_SPI_BUS) +#define ao_sdcard_send_fixed(d,l) ao_spi_send_fixed((d), (l), AO_SDCARD_SPI_BUS) +#define ao_sdcard_send(d,l) ao_spi_send((d), (l), AO_SDCARD_SPI_BUS) +#define ao_sdcard_recv(d,l) ao_spi_recv((d), (l), AO_SDCARD_SPI_BUS) +#define ao_sdcard_select() ao_gpio_set(AO_SDCARD_SPI_CS_PORT,AO_SDCARD_SPI_CS_PIN,AO_SDCARD_SPI_CS,0) +#define ao_sdcard_deselect() ao_gpio_set(AO_SDCARD_SPI_CS_PORT,AO_SDCARD_SPI_CS_PIN,AO_SDCARD_SPI_CS,1) + + +static uint8_t initialized; +static uint8_t present; +static uint8_t mutex; +static enum ao_sdtype sdtype; + +#define ao_sdcard_lock() ao_mutex_get(&mutex) +#define ao_sdcard_unlock() ao_mutex_put(&mutex) + +#if 0 +#define DBG(...) printf(__VA_ARGS__) +#else +#define DBG(...) +#endif + +/* + * Send an SD command and await the status reply + */ + +static uint8_t +ao_sdcard_send_cmd(uint8_t cmd, uint32_t arg) +{ + uint8_t data[6]; + uint8_t reply; + int i; + + DBG ("\tsend_cmd %d arg %08x\n", cmd, arg); + if (cmd != SDCARD_GO_IDLE_STATE) { + for (i = 0; i < SDCARD_CMD_TIMEOUT; i++) { + ao_sdcard_recv(&reply, 1); + if (reply == 0xff) + break; + } + if (i == SDCARD_CMD_TIMEOUT) + return SDCARD_STATUS_TIMEOUT; + } + + data[0] = cmd & 0x3f | 0x40; + data[1] = arg >> 24; + data[2] = arg >> 16; + data[3] = arg >> 8; + data[4] = arg; + if (cmd == SDCARD_GO_IDLE_STATE) + data[5] = 0x95; /* Valid for 0 arg */ + else if (cmd == SDCARD_SEND_IF_COND) + data[5] = 0x87; /* Valid for 0x1aa arg */ + else + data[5] = 0xff; /* no CRC */ + ao_sdcard_send(data, 6); + + /* The first reply byte will be the status, + * which must have the high bit clear + */ + for (i = 0; i < SDCARD_CMD_TIMEOUT; i++) { + ao_sdcard_recv(&reply, 1); + DBG ("\t\tgot byte %02x\n", reply); + if ((reply & 0x80) == 0) + return reply; + } + return SDCARD_STATUS_TIMEOUT; +} + +/* + * Retrieve any reply, discarding the trailing CRC byte + */ +static void +ao_sdcard_recv_reply(uint8_t *reply, int len) +{ + uint8_t discard; + + if (len) + ao_sdcard_recv(reply, len); + /* trailing byte */ + ao_sdcard_recv(&discard, 1); +} + +/* + * Wait while the card is busy. The + * card will return a stream of 0xff + * until it isn't busy anymore + */ +static void +ao_sdcard_wait_busy(void) +{ + uint8_t v; + + do { + ao_sdcard_recv(&v, 1); + } while (v != 0xff); + ao_sdcard_send_fixed(0xff, 1); +} + +static uint8_t +ao_sdcard_go_idle_state(void) +{ + uint8_t ret; + + DBG ("go_idle_state\n"); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_GO_IDLE_STATE, 0); + ao_sdcard_recv_reply(NULL, 0); + ao_sdcard_deselect(); + DBG ("\tgo_idle_state status %02x\n", ret); + return ret; +} + +static uint8_t +ao_sdcard_send_op_cond(void) +{ + uint8_t ret; + + DBG ("send_op_cond\n"); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_SEND_OP_COND, 0); + ao_sdcard_recv_reply(NULL, 0); + ao_sdcard_deselect(); + DBG ("\tsend_op_cond %02x\n", ret); + return ret; +} + +static uint8_t +ao_sdcard_send_if_cond(uint32_t arg, uint8_t send_if_cond_response[4]) +{ + uint8_t ret; + + DBG ("send_if_cond\n"); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_SEND_IF_COND, arg); + if (ret != SDCARD_STATUS_IDLE_STATE) { + DBG ("\tsend_if_cond failed %02x\n", ret); + return ret; + } + ao_sdcard_recv_reply(send_if_cond_response, 4); + DBG ("send_if_cond status %02x response %02x %02x %02x %02x\n", + ret, + send_if_cond_response[0], + send_if_cond_response[1], + send_if_cond_response[2], + send_if_cond_response[3]); + ao_sdcard_deselect(); + return ret; +} + +static uint8_t +ao_sdcard_set_blocklen(uint32_t blocklen) +{ + uint8_t ret; + + DBG ("set_blocklen %d\n", blocklen); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_SET_BLOCKLEN, blocklen); + ao_sdcard_recv_reply(NULL, 0); + if (ret != SDCARD_STATUS_READY_STATE) + DBG ("\tsend_if_cond failed %02x\n", ret); + return ret; + +} + +static uint8_t +ao_sdcard_app_cmd(void) +{ + uint8_t ret; + + DBG ("app_cmd\n"); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_APP_CMD, 0); + ao_sdcard_recv_reply(NULL, 0); + ao_sdcard_deselect(); + DBG ("\tapp_cmd status %02x\n"); + return ret; +} + +static uint8_t +ao_sdcard_app_send_op_cond(uint32_t arg) +{ + uint8_t ret; + + ret = ao_sdcard_app_cmd(); + if (ret != SDCARD_STATUS_IDLE_STATE) + return ret; + DBG("send_op_comd\n"); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_APP_SEND_OP_COMD, arg); + ao_sdcard_recv_reply(NULL, 0); + ao_sdcard_deselect(); + DBG ("\tapp_send_op_cond status %02x\n", ret); + return ret; +} + +static uint8_t +ao_sdcard_read_ocr(uint8_t read_ocr_response[4]) +{ + uint8_t ret; + + DBG ("read_ocr\n"); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_READ_OCR, 0); + if (ret != SDCARD_STATUS_READY_STATE) + DBG ("\tread_ocr failed %02x\n", ret); + else { + ao_sdcard_recv_reply(read_ocr_response, 4); + DBG ("\tread_ocr status %02x response %02x %02x %02x %02x\n", ret, + read_ocr_response[0], read_ocr_response[1], + read_ocr_response[2], read_ocr_response[3]); + } + ao_sdcard_deselect(); + return ret; +} + +static void +ao_sdcard_setup(void) +{ + int i; + uint8_t ret; + uint8_t response[10]; + + DBG ("Testing sdcard\n"); + + ao_sdcard_get_slow(); + /* + * min 74 clocks with CS high + */ + ao_sdcard_send_fixed(0xff, 10); + + ao_delay(AO_MS_TO_TICKS(10)); + + /* Reset the card and get it into SPI mode */ + + for (i = 0; i < SDCARD_IDLE_WAIT; i++) { + if (ao_sdcard_go_idle_state() == SDCARD_STATUS_IDLE_STATE) + break; + } + if (i == SDCARD_IDLE_WAIT) + goto bail; + + /* Figure out what kind of card we have */ + + sdtype = ao_sdtype_unknown; + + if (ao_sdcard_send_if_cond(0x1aa, response) == SDCARD_STATUS_IDLE_STATE) { + uint32_t arg = 0; + uint8_t sdver2 = 0; + + /* Check for SD version 2 */ + if ((response[2] & 0xf) == 1 && response[3] == 0xaa) { + arg = 0x40000000; + sdver2 = 1; + } + + for (i = 0; i < SDCARD_IDLE_WAIT; i++) { + ret = ao_sdcard_app_send_op_cond(arg); + if (ret != SDCARD_STATUS_IDLE_STATE) + break; + } + if (ret != SDCARD_STATUS_READY_STATE) { + /* MMC */ + for (i = 0; i < SDCARD_IDLE_WAIT; i++) { + ret = ao_sdcard_send_op_cond(); + if (ret != SDCARD_STATUS_IDLE_STATE) + break; + } + if (ret != SDCARD_STATUS_READY_STATE) + goto bail; + sdtype = ao_sdtype_mmc3; + } else { + /* SD */ + if (sdver2 != 0) { + ret = ao_sdcard_read_ocr(response); + if (ret != SDCARD_STATUS_READY_STATE) + goto bail; + if ((response[0] & 0xc0) == 0xc0) + sdtype = ao_sdtype_sd2block; + else + sdtype = ao_sdtype_sd2byte; + } else { + sdtype = ao_sdtype_sd1; + } + } + + /* For everything but SDHC cards, set the block length */ + if (sdtype != ao_sdtype_sd2block) { + ret = ao_sdcard_set_blocklen(512); + if (ret != SDCARD_STATUS_READY_STATE) + DBG ("set_blocklen failed, ignoring\n"); + } + } + + DBG ("SD card detected, type %d\n", sdtype); +bail: + ao_sdcard_put(); +} + +static uint8_t +ao_sdcard_wait_block_start(void) +{ + int i; + uint8_t v; + + DBG ("\twait_block_start\n"); + for (i = 0; i < SDCARD_BLOCK_TIMEOUT; i++) { + ao_sdcard_recv(&v, 1); + DBG("\t\trecv %02x\n", v); + if (v != 0xff) + break; + } + return v; +} + +/* + * Read a block of 512 bytes from the card + */ +uint8_t +ao_sdcard_read_block(uint32_t block, uint8_t *data) +{ + uint8_t ret; + uint8_t crc[2]; + + ao_sdcard_lock(); + if (!initialized) { + ao_sdcard_setup(); + initialized = 1; + if (sdtype != ao_sdtype_unknown) + present = 1; + } + if (!present) { + ao_sdcard_unlock(); + return 0; + } + if (sdtype != ao_sdtype_sd2block) + block <<= 9; + ao_sdcard_get(); + ao_sdcard_select(); + ret = ao_sdcard_send_cmd(SDCARD_READ_BLOCK, block); + ao_sdcard_recv_reply(NULL, 0); + if (ret != SDCARD_STATUS_READY_STATE) + goto bail; + + if (ao_sdcard_wait_block_start() != 0xfe) { + ret = 0x3f; + goto bail; + } + + ao_sdcard_recv(data, 512); + ao_sdcard_recv(crc, 2); +bail: + ao_sdcard_deselect(); + ao_sdcard_put(); + ao_sdcard_unlock(); + return ret == SDCARD_STATUS_READY_STATE; +} + +/* + * Write a block of 512 bytes to the card + */ +uint8_t +ao_sdcard_write_block(uint32_t block, uint8_t *data) +{ + /* Not doing anything until the file system code seems reasonable + */ + return 1; +} + +void +ao_sdcard_init(void) +{ + ao_spi_init_cs(AO_SDCARD_SPI_CS_PORT, (1 << AO_SDCARD_SPI_CS_PIN)); +} + + diff --git a/src/drivers/ao_sdcard.h b/src/drivers/ao_sdcard.h new file mode 100644 index 00000000..b9f737c5 --- /dev/null +++ b/src/drivers/ao_sdcard.h @@ -0,0 +1,75 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef _AO_SDCARD_H_ +#define _AO_SDCARD_H_ + +uint8_t +ao_sdcard_read_block(uint32_t block, uint8_t *data); + +uint8_t +ao_sdcard_write_block(uint32_t block, uint8_t *data); + +void +ao_sdcard_init(void); + +/* Commands */ +#define SDCARD_GO_IDLE_STATE 0 +#define SDCARD_SEND_OP_COND 1 +#define SDCARD_SEND_IF_COND 8 +#define SDCARD_SEND_CSD 9 +#define SDCARD_SEND_CID 10 +#define SDCARD_SEND_STATUS 13 +#define SDCARD_SET_BLOCKLEN 16 +#define SDCARD_READ_BLOCK 17 +#define SDCARD_WRITE_BLOCK 24 +#define SDCARD_WRITE_MULTIPLE_BLOCK 25 +#define SDCARD_ERASE_WR_BLK_START 32 +#define SDCARD_ERASE_WR_BLK_END 33 +#define SDCARD_ERASE 38 +#define SDCARD_APP_CMD 55 +#define SDCARD_READ_OCR 58 + +/* App commands */ +#define SDCARD_APP_SET_WR_BLK_ERASE_COUNT 23 +#define SDCARD_APP_SEND_OP_COMD 41 + +/* Status */ +#define SDCARD_STATUS_READY_STATE 0 +#define SDCARD_STATUS_IDLE_STATE 1 +#define SDCARD_STATUS_ILLEGAL_COMMAND 4 +#define SDCARD_STATUS_TIMEOUT 0xff + +#define SDCARD_DATA_START_BLOCK 0xfe +#define SDCARD_STOP_TRAN_TOKEN 0xfd +#define SDCARD_WRITE_MULTIPLE_TOKEN 0xfc +#define SDCARD_DATA_RES_MASK 0x1f +#define SDCARD_DATA_RES_ACCEPTED 0x05 + +#define SDCARD_CMD_TIMEOUT 100 +#define SDCARD_IDLE_WAIT 100 +#define SDCARD_BLOCK_TIMEOUT 100 + +enum ao_sdtype { + ao_sdtype_unknown, + ao_sdtype_mmc3, + ao_sdtype_sd1, + ao_sdtype_sd2byte, + ao_sdtype_sd2block, +}; + +#endif /* _AO_SDCARD_H_ */ diff --git a/src/test/Makefile b/src/test/Makefile index fccc7937..96170cc1 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -2,7 +2,7 @@ vpath % ..:../core:../drivers:../util:../micropeak 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_aprs_test ao_micropeak_test + ao_aprs_test ao_micropeak_test ao_fat_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -64,3 +64,6 @@ check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests ao_micropeak_test: ao_micropeak_test.c ao_microflight.c ao_kalman.h cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm + +ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c + cc $(CFLAGS) -o $@ ao_fat_test.c diff --git a/src/test/ao_fat_test.c b/src/test/ao_fat_test.c new file mode 100644 index 00000000..22ac03ad --- /dev/null +++ b/src/test/ao_fat_test.c @@ -0,0 +1,118 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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 +#include +#include +#include +#include +#include +#include +#include + +#define AO_FAT_TEST + +void +ao_mutex_get(uint8_t *mutex) +{ +} + +void +ao_mutex_put(uint8_t *mutex) +{ +} + +void +ao_panic(uint8_t panic) +{ + printf ("panic %d\n", panic); + exit(1); +} + +#define AO_PANIC_BUFIO 15 + +#define ao_cmd_success 0 + +uint8_t ao_cmd_status; +uint32_t ao_cmd_lex_u32; + +void +ao_cmd_decimal() +{ +} + +#define ao_cmd_register(x) + +struct ao_cmds { + void (*func)(void); + const char *help; +}; + +int fs_fd; + +uint8_t +ao_sdcard_read_block(uint32_t block, uint8_t *data) +{ + lseek(fs_fd, block * 512, 0); + return read(fs_fd, data, 512) == 512; +} + +uint8_t +ao_sdcard_write_block(uint32_t block, uint8_t *data) +{ + lseek(fs_fd, block * 512, 0); + return write(fs_fd, data, 512) == 512; +} + +void +ao_sdcard_init(void) +{ + fs_fd = open("fat.fs", 2); +} + +#include "ao_bufio.c" +#include "ao_fat.c" + +int +main(int argc, char **argv) +{ + uint8_t data[15]; + int len; + ao_fat_init(); + ao_fat_test(); + if (ao_fat_open("DATALOG TXT")) { + printf ("DATALOG.TXT\n"); + while ((len = ao_fat_read(data, sizeof (data))) > 0) { + write(1, data, len); + } + ao_fat_close(); +// ao_fat_unlink("DATALOG TXT"); + } + if (ao_fat_open("NEWFILE TXT")) { + printf ("NEWFILE.TXT\n"); + while ((len = ao_fat_read(data, sizeof (data))) > 0) { + write(1, data, len); + } + ao_fat_close(); + } + if (ao_fat_creat ("NEWFILE TXT")) { + for (len = 0; len < 4095; len++) + ao_fat_write((uint8_t *) "hello, world!\n", 14); + ao_fat_close(); + } + return 0; +} -- cgit v1.2.3 From c7b606e93a4e4fbd2c0e883352ed74619ee24cf7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 28 Mar 2013 16:05:24 -0700 Subject: altos: Clean up fat driver API. Improve fat test Make FAT api provide reasonable error return values, change the tests to write and then read a pile of files, checking that the contents are correct (using md5sum). Signed-off-by: Keith Packard --- src/drivers/ao_fat.c | 602 +++++++++++++++++++++++++++++++++---------------- src/drivers/ao_fat.h | 45 +++- src/test/Makefile | 2 +- src/test/ao_fat_test.c | 310 +++++++++++++++++++++++-- 4 files changed, 732 insertions(+), 227 deletions(-) (limited to 'src/test/Makefile') diff --git a/src/drivers/ao_fat.c b/src/drivers/ao_fat.c index a1476168..c0412380 100644 --- a/src/drivers/ao_fat.c +++ b/src/drivers/ao_fat.c @@ -22,11 +22,17 @@ #include "ao_fat.h" #include "ao_bufio.h" -/* Partition information, block numbers */ +/* Partition information, sector numbers */ static uint8_t partition_type; static uint32_t partition_start, partition_end; +#define SECTOR_SIZE 512 +#define SECTOR_MASK (SECTOR_SIZE - 1) +#define SECTOR_SHIFT 9 + +#define DIRENT_SIZE 32 + /* File system parameters */ static uint8_t sectors_per_cluster; static uint32_t bytes_per_cluster; @@ -34,10 +40,15 @@ static uint16_t reserved_sector_count; static uint8_t number_fat; static uint16_t root_entries; static uint16_t sectors_per_fat; +static uint16_t number_cluster; static uint32_t fat_start; static uint32_t root_start; static uint32_t data_start; +static uint16_t first_free_cluster; +/* + * Deal with LSB FAT data structures + */ static uint32_t get_u32(uint8_t *base) { @@ -72,32 +83,32 @@ put_u16(uint8_t *base, uint16_t value) static uint8_t ao_fat_cluster_valid(uint16_t cluster) { - return (2 <= cluster && cluster < 0xfff0); + return (2 <= cluster && cluster < number_cluster); } -/* Start using a block */ +/* Start using a sector */ static uint8_t * -ao_fat_block_get(uint32_t block) +ao_fat_sector_get(uint32_t sector) { - block += partition_start; - if (block >= partition_end) + sector += partition_start; + if (sector >= partition_end) return NULL; - return ao_bufio_get(block); + return ao_bufio_get(sector); } -/* Finish using a block, 'w' is 1 if modified */ -#define ao_fat_block_put(b,w) ao_bufio_put(b,w) +/* Finish using a sector, 'w' is 1 if modified */ +#define ao_fat_sector_put(b,w) ao_bufio_put(b,w) /* Start using a root directory entry */ static uint8_t * ao_fat_root_get(uint16_t e) { - uint32_t byte = e * 0x20; - uint32_t sector = byte >> 9; - uint16_t offset = byte & 0x1ff; + uint32_t byte = e * DIRENT_SIZE; + uint32_t sector = byte >> SECTOR_SHIFT; + uint16_t offset = byte & SECTOR_MASK; uint8_t *buf; - buf = ao_fat_block_get(root_start + sector); + buf = ao_fat_sector_get(root_start + sector); if (!buf) return NULL; return buf + offset; @@ -107,10 +118,10 @@ ao_fat_root_get(uint16_t e) static void ao_fat_root_put(uint8_t *root, uint16_t e, uint8_t write) { - uint16_t offset = ((e * 0x20) & 0x1ff); + uint16_t offset = ((e * DIRENT_SIZE) & SECTOR_MASK); uint8_t *buf = root - offset; - ao_fat_block_put(buf, write); + ao_fat_sector_put(buf, write); } /* Get the next cluster entry in the chain */ @@ -125,17 +136,20 @@ ao_fat_entry_read(uint16_t cluster) if (!ao_fat_cluster_valid(cluster)) return 0xfff7; - cluster -= 2; - sector = cluster >> 8; - offset = (cluster << 1) & 0x1ff; - buf = ao_fat_block_get(fat_start + sector); +// cluster -= 2; + sector = cluster >> (SECTOR_SHIFT - 1); + offset = (cluster << 1) & SECTOR_MASK; + buf = ao_fat_sector_get(fat_start + sector); if (!buf) return 0; - ret = buf[offset] | (buf[offset+1] << 8); - ao_fat_block_put(buf, 0); + ret = get_u16(buf + offset); + ao_fat_sector_put(buf, 0); return ret; } +/* Replace the referenced cluster entry in the chain with + * 'new_value'. Return the previous value. + */ static uint16_t ao_fat_entry_replace(uint16_t cluster, uint16_t new_value) { @@ -148,33 +162,50 @@ ao_fat_entry_replace(uint16_t cluster, uint16_t new_value) if (!ao_fat_cluster_valid(cluster)) return 0; - cluster -= 2; - sector = cluster >> 8; - offset = (cluster << 1) & 0x1ff; - buf = ao_fat_block_get(fat_start + sector); +// cluster -= 2; + sector = cluster >> (SECTOR_SHIFT - 1); + offset = (cluster << 1) & SECTOR_MASK; + buf = ao_fat_sector_get(fat_start + sector); if (!buf) return 0; ret = get_u16(buf + offset); put_u16(buf + offset, new_value); - ao_fat_block_put(buf, 1); + ao_fat_sector_put(buf, 1); + + /* + * Keep the other FATs in sync + */ for (other_fats = 1; other_fats < number_fat; other_fats++) { - buf = ao_fat_block_get(fat_start + other_fats * sectors_per_fat); + buf = ao_fat_sector_get(fat_start + other_fats * sectors_per_fat + sector); if (buf) { put_u16(buf + offset, new_value); - ao_fat_block_put(buf, 1); + ao_fat_sector_put(buf, 1); } } return ret; } +/* + * Walk a cluster chain and mark + * all of them as free + */ static void -ao_fat_clear_cluster_chain(uint16_t cluster) +ao_fat_free_cluster_chain(uint16_t cluster) { - while (ao_fat_cluster_valid(cluster)) + while (ao_fat_cluster_valid(cluster)) { + if (cluster < first_free_cluster) + first_free_cluster = cluster; cluster = ao_fat_entry_replace(cluster, 0x0000); + } } +/* + * ao_fat_cluster_seek + * + * Walk a cluster chain the specified distance and return what we find + * there. If distance is zero, return the provided cluster. + */ static uint16_t ao_fat_cluster_seek(uint16_t cluster, uint16_t distance) { @@ -187,16 +218,39 @@ ao_fat_cluster_seek(uint16_t cluster, uint16_t distance) return cluster; } +/* + * ao_fat_sector_seek + * + * The basic file system operation -- map a file sector number to a + * partition sector number. Done by computing the cluster number and + * then walking that many clusters from the first cluster, then + * adding the sector offset from the start of the cluster. Returns + * 0xffffffff if we walk off the end of the file or the cluster chain + * is damaged somehow + */ static uint32_t ao_fat_sector_seek(uint16_t cluster, uint32_t sector) { - cluster = ao_fat_cluster_seek(cluster, sector / sectors_per_cluster); + uint16_t distance; + uint16_t offset; + + distance = sector / sectors_per_cluster; + offset = sector % sectors_per_cluster; + + cluster = ao_fat_cluster_seek(cluster, distance); + if (!ao_fat_cluster_valid(cluster)) return 0xffffffff; - return data_start + (cluster-2) * sectors_per_cluster + sector % sectors_per_cluster; + + /* Compute the sector within the partition and return it */ + return data_start + (uint32_t) (cluster-2) * sectors_per_cluster + offset; } -/* Load the boot block and find the first partition */ +/* + * ao_fat_setup_partition + * + * Load the boot block and find the first partition + */ static uint8_t ao_fat_setup_partition(void) { @@ -216,29 +270,40 @@ ao_fat_setup_partition(void) return 0; } - /* Just use the first partition */ - partition = &mbr[0x1be]; + /* Check to see if it's actually a boot block, in which + * case it's presumably not a paritioned device + */ + + if (mbr[0] == 0xeb) { + partition_start = 0; + partition_size = get_u16(mbr + 0x13); + if (partition_size == 0) + partition_size = get_u32(mbr + 0x20); + } else { + /* Just use the first partition */ + partition = &mbr[0x1be]; - partition_type = partition[4]; - switch (partition_type) { - case 4: /* FAT16 up to 32M */ - case 6: /* FAT16 over 32M */ - break; - case 0x0b: /* FAT32 up to 2047GB */ - case 0x0c: /* FAT32 LBA */ - break; - default: - printf ("Invalid partition type %02x\n", partition_type); - ao_bufio_put(mbr, 0); - return 0; - } + partition_type = partition[4]; + switch (partition_type) { + case 4: /* FAT16 up to 32M */ + case 6: /* FAT16 over 32M */ + break; + case 0x0b: /* FAT32 up to 2047GB */ + case 0x0c: /* FAT32 LBA */ + break; + default: + printf ("Invalid partition type %02x\n", partition_type); + ao_bufio_put(mbr, 0); + return 0; + } - partition_start = get_u32(partition+8); - partition_size = get_u32(partition+12); - if (partition_size == 0) { - printf ("Zero-sized partition\n"); - ao_bufio_put(mbr, 0); - return 0; + partition_start = get_u32(partition+8); + partition_size = get_u32(partition+12); + if (partition_size == 0) { + printf ("Zero-sized partition\n"); + ao_bufio_put(mbr, 0); + return 0; + } } partition_end = partition_start + partition_size; printf ("Partition type %02x start %08x end %08x\n", @@ -250,7 +315,8 @@ ao_fat_setup_partition(void) static uint8_t ao_fat_setup_fs(void) { - uint8_t *boot = ao_fat_block_get(0); + uint8_t *boot = ao_fat_sector_get(0); + uint32_t data_sectors; if (!boot) return 0; @@ -259,39 +325,45 @@ ao_fat_setup_fs(void) if (boot[0x1fe] != 0x55 || boot[0x1ff] != 0xaa) { printf ("Invalid BOOT signature %02x %02x\n", boot[0x1fe], boot[0x1ff]); - ao_bufio_put(boot, 0); + ao_fat_sector_put(boot, 0); return 0; } /* Check the sector size */ - if (get_u16(boot + 0xb) != 0x200) { + if (get_u16(boot + 0xb) != SECTOR_SIZE) { printf ("Invalid sector size %d\n", get_u16(boot + 0xb)); - ao_bufio_put(boot, 0); + ao_fat_sector_put(boot, 0); return 0; } sectors_per_cluster = boot[0xd]; - bytes_per_cluster = sectors_per_cluster << 9; + bytes_per_cluster = sectors_per_cluster << SECTOR_SHIFT; reserved_sector_count = get_u16(boot+0xe); number_fat = boot[0x10]; root_entries = get_u16(boot + 0x11); sectors_per_fat = get_u16(boot+0x16); + fat_start = reserved_sector_count;; + root_start = fat_start + number_fat * sectors_per_fat; + data_start = root_start + ((root_entries * DIRENT_SIZE + SECTOR_MASK) >> SECTOR_SHIFT); + + data_sectors = (partition_end - partition_start) - data_start; + + number_cluster = data_sectors / sectors_per_cluster; + printf ("sectors per cluster %d\n", sectors_per_cluster); printf ("reserved sectors %d\n", reserved_sector_count); printf ("number of FATs %d\n", number_fat); printf ("root entries %d\n", root_entries); printf ("sectors per fat %d\n", sectors_per_fat); - fat_start = reserved_sector_count;; - root_start = fat_start + number_fat * sectors_per_fat; - data_start = root_start + ((root_entries * 0x20 + 0x1ff) >> 9); - printf ("fat start %d\n", fat_start); printf ("root start %d\n", root_start); printf ("data start %d\n", data_start); + ao_fat_sector_put(boot, 0); + return 1; } @@ -300,49 +372,36 @@ ao_fat_setup(void) { if (!ao_fat_setup_partition()) return 0; + check_bufio("partition setup"); if (!ao_fat_setup_fs()) return 0; + check_bufio("fs setup"); return 1; } -/* - * Low-level directory operations - */ - /* * Basic file operations */ static struct ao_fat_dirent ao_file_dirent; static uint32_t ao_file_offset; +static uint8_t ao_file_opened; static uint32_t -ao_file_offset_to_sector(uint32_t offset) +ao_fat_offset_to_sector(uint32_t offset) { if (offset > ao_file_dirent.size) return 0xffffffff; - return ao_fat_sector_seek(ao_file_dirent.cluster, offset >> 9); -} - -uint8_t -ao_fat_open(char name[11]) -{ - uint16_t entry = 0; - struct ao_fat_dirent dirent; - - while (ao_fat_readdir(&entry, &dirent)) { - if (!memcmp(name, dirent.name, 11)) { - ao_file_dirent = dirent; - ao_file_offset = 0; - return 1; - } - } - return 0; + return ao_fat_sector_seek(ao_file_dirent.cluster, offset >> SECTOR_SHIFT); } - - -static uint8_t +/* + * ao_fat_set_size + * + * Set the size of the current file, truncating or extending + * the cluster chain as needed + */ +static int8_t ao_fat_set_size(uint32_t size) { uint16_t clear_cluster = 0; @@ -350,11 +409,9 @@ ao_fat_set_size(uint32_t size) uint16_t first_cluster; first_cluster = ao_file_dirent.cluster; - printf ("set size to %d\n", size); if (size == ao_file_dirent.size) - return 1; + return AO_FAT_SUCCESS; if (size == 0) { - printf ("erase file\n"); clear_cluster = ao_file_dirent.cluster; first_cluster = 0; } else { @@ -366,43 +423,50 @@ ao_fat_set_size(uint32_t size) if (new_num < old_num) { uint16_t last_cluster; - printf("Remove %d clusters\n", old_num - new_num); /* Go find the last cluster we want to preserve in the file */ last_cluster = ao_fat_cluster_seek(ao_file_dirent.cluster, new_num - 1); - printf ("Last cluster is now %04x\n", last_cluster); /* Rewrite that cluster entry with 0xffff to mark the end of the chain */ clear_cluster = ao_fat_entry_replace(last_cluster, 0xffff); } else if (new_num > old_num) { uint16_t need; uint16_t free; uint16_t last_cluster; + uint16_t highest_allocated = 0; if (old_num) last_cluster = ao_fat_cluster_seek(ao_file_dirent.cluster, old_num - 1); else last_cluster = 0; - need = new_num - old_num; - printf ("Need %d clusters\n", need); + if (first_free_cluster < 2 || number_cluster <= first_free_cluster) + first_free_cluster = 2; + /* See if there are enough free clusters in the file system */ - for (free = 2; need > 0 && (free - 2) < sectors_per_fat * 256; free++) { - if (!ao_fat_entry_read(free)) { - printf ("\tCluster %04x available\n", free); + need = new_num - old_num; + +#define loop_cluster for (free = first_free_cluster; need > 0;) +#define next_cluster \ + if (++free == number_cluster) \ + free = 2; \ + if (free == first_free_cluster) \ + break; \ + + loop_cluster { + if (!ao_fat_entry_read(free)) need--; - } + next_cluster; } /* Still need some, tell the user that we've failed */ - if (need) { - printf ("File system full\n"); - return 0; - } + if (need) + return -AO_FAT_ENOSPC; - need = new_num - old_num; /* Now go allocate those clusters */ - for (free = 2; need > 0 && (free - 2) < sectors_per_fat * 256; free++) { + need = new_num - old_num; + loop_cluster { if (!ao_fat_entry_read(free)) { - printf ("\tAllocate %04x\n", free); + if (free > highest_allocated) + highest_allocated = free; if (last_cluster) ao_fat_entry_replace(last_cluster, free); else @@ -410,153 +474,297 @@ ao_fat_set_size(uint32_t size) last_cluster = free; need--; } + next_cluster; } + first_free_cluster = highest_allocated + 1; + if (first_free_cluster >= number_cluster) + first_free_cluster = 2; + /* Mark the new end of the chain */ ao_fat_entry_replace(last_cluster, 0xffff); } } /* Deallocate clusters off the end of the file */ - if (ao_fat_cluster_valid(clear_cluster)) { - printf ("Clear clusters starting with %04x\n", clear_cluster); - ao_fat_clear_cluster_chain(clear_cluster); - } + if (ao_fat_cluster_valid(clear_cluster)) + ao_fat_free_cluster_chain(clear_cluster); + /* Update the directory entry */ dent = ao_fat_root_get(ao_file_dirent.entry); if (!dent) - return 0; + return -AO_FAT_EIO; put_u32(dent + 0x1c, size); put_u16(dent + 0x1a, first_cluster); ao_fat_root_put(dent, ao_file_dirent.entry, 1); ao_file_dirent.size = size; ao_file_dirent.cluster = first_cluster; - return 1; + return AO_FAT_SUCCESS; +} + +/* + * ao_fat_root_init + * + * Initialize a root directory entry + */ +void +ao_fat_root_init(uint8_t *dent, char name[11], uint8_t attr) +{ + memset(dent, '\0', 0x20); + memmove(dent, name, 11); + + dent[0x0b] = 0x00; + dent[0x0c] = 0x00; + dent[0x0d] = 0x00; + + /* XXX fix time */ + put_u16(dent + 0x0e, 0); + /* XXX fix date */ + put_u16(dent + 0x10, 0); + /* XXX fix date */ + put_u16(dent + 0x12, 0); + + /* XXX fix time */ + put_u16(dent + 0x16, 0); + /* XXX fix date */ + put_u16(dent + 0x18, 0); + + /* cluster number */ + /* Low cluster bytes */ + put_u16(dent + 0x1a, 0); + /* FAT32 high cluster bytes */ + put_u16(dent + 0x14, 0); + + /* size */ + put_u32(dent + 0x1c, 0); +} + + +static void +ao_fat_dirent_init(uint8_t *dent, uint16_t entry, struct ao_fat_dirent *dirent) +{ + memcpy(dirent->name, dent + 0x00, 11); + dirent->attr = dent[0x0b]; + dirent->size = get_u32(dent+0x1c); + dirent->cluster = get_u16(dent+0x1a); + dirent->entry = entry; +} + +/* + * Public API + */ + +/* + * ao_fat_open + * + * Open an existing file. + */ +int8_t +ao_fat_open(char name[11], uint8_t mode) +{ + uint16_t entry = 0; + struct ao_fat_dirent dirent; + + if (ao_file_opened) + return -AO_FAT_EMFILE; + + while (ao_fat_readdir(&entry, &dirent)) { + if (!memcmp(name, dirent.name, 11)) { + if (AO_FAT_IS_DIR(dirent.attr)) + return -AO_FAT_EISDIR; + if (!AO_FAT_IS_FILE(dirent.attr)) + return -AO_FAT_EPERM; + if (mode > AO_FAT_OPEN_READ && (dirent.attr & AO_FAT_FILE_READ_ONLY)) + return -AO_FAT_EACCESS; + ao_file_dirent = dirent; + ao_file_offset = 0; + ao_file_opened = 1; + return AO_FAT_SUCCESS; + } + } + return -AO_FAT_ENOENT; } -uint8_t +/* + * ao_fat_creat + * + * Open and truncate an existing file or + * create a new file + */ +int8_t ao_fat_creat(char name[11]) { uint16_t entry; + int8_t status; + + if (ao_file_opened) + return -AO_FAT_EMFILE; + + status = ao_fat_open(name, AO_FAT_OPEN_WRITE); - if (ao_fat_open(name)) - return ao_fat_set_size(0); - - for (entry = 0; entry < root_entries; entry++) { - uint8_t *dent = ao_fat_root_get(entry); - - if (dent[0] == AO_FAT_DENT_EMPTY || - dent[0] == AO_FAT_DENT_END) { - memmove(dent, name, 11); - dent[0x0b] = 0x00; - dent[0x0c] = 0x00; - dent[0x0d] = 0x00; - /* XXX fix time */ - put_u16(dent + 0x0e, 0); - /* XXX fix date */ - put_u16(dent + 0x10, 0); - /* XXX fix date */ - put_u16(dent + 0x12, 0); - /* XXX FAT32 high cluster bytes */ - put_u16(dent + 0x14, 0); - /* XXX fix time */ - put_u16(dent + 0x16, 0); - /* XXX fix date */ - put_u16(dent + 0x18, 0); - /* cluster number */ - put_u16(dent + 0x1a, 0); - /* size */ - put_u32(dent + 0x1c, 0); - ao_fat_root_put(dent, entry, 1); - return ao_fat_open(name); + switch (status) { + case -AO_FAT_SUCCESS: + status = ao_fat_set_size(0); + break; + case -AO_FAT_ENOENT: + for (entry = 0; entry < root_entries; entry++) { + uint8_t *dent = ao_fat_root_get(entry); + + if (!dent) { + status = -AO_FAT_EIO; + ao_fat_root_put(dent, entry, 0); + break; + } + + if (dent[0] == AO_FAT_DENT_EMPTY || dent[0] == AO_FAT_DENT_END) { + ao_fat_root_init(dent, name, AO_FAT_FILE_REGULAR); + ao_fat_dirent_init(dent, entry, &ao_file_dirent); + ao_fat_root_put(dent, entry, 1); + ao_file_opened = 1; + status = -AO_FAT_SUCCESS; + break; + } else { + ao_fat_root_put(dent, entry, 0); + } } + if (entry == root_entries) + status = -AO_FAT_ENOSPC; } - return 0; + return status; } -void +/* + * ao_fat_close + * + * Close the currently open file + */ +int8_t ao_fat_close(void) { + if (!ao_file_opened) + return -AO_FAT_EBADF; + memset(&ao_file_dirent, '\0', sizeof (struct ao_fat_dirent)); + ao_file_offset = 0; + ao_file_opened = 0; ao_bufio_flush(); + return AO_FAT_SUCCESS; } +/* + * ao_fat_read + * + * Read from the file + */ int -ao_fat_read(uint8_t *dest, int len) +ao_fat_read(void *dst, int len) { + uint8_t *dst_b = dst; uint32_t sector; uint16_t this_time; uint16_t offset; uint8_t *buf; int ret = 0; + if (!ao_file_opened) + return -AO_FAT_EBADF; + if (ao_file_offset + len > ao_file_dirent.size) len = ao_file_dirent.size - ao_file_offset; + if (len < 0) + len = 0; + while (len) { - offset = ao_file_offset & 0x1ff; - if (offset + len < 512) + offset = ao_file_offset & SECTOR_MASK; + if (offset + len < SECTOR_SIZE) this_time = len; else - this_time = 512 - offset; + this_time = SECTOR_SIZE - offset; - sector = ao_file_offset_to_sector(ao_file_offset); + sector = ao_fat_offset_to_sector(ao_file_offset); if (sector == 0xffffffff) break; - buf = ao_fat_block_get(sector); - if (!buf) + buf = ao_fat_sector_get(sector); + if (!buf) { + ret = -AO_FAT_EIO; break; - memcpy(dest, buf + offset, this_time); - ao_fat_block_put(buf, 0); + } + memcpy(dst_b, buf + offset, this_time); + ao_fat_sector_put(buf, 0); ret += this_time; len -= this_time; - dest += this_time; + dst_b += this_time; ao_file_offset += this_time; } return ret; } +/* + * ao_fat_write + * + * Write to the file, extended as necessary + */ int -ao_fat_write(uint8_t *src, int len) +ao_fat_write(void *src, int len) { + uint8_t *src_b = src; uint32_t sector; uint16_t this_time; uint16_t offset; uint8_t *buf; int ret = 0; + if (!ao_file_opened) + return -AO_FAT_EBADF; + if (ao_file_offset + len > ao_file_dirent.size) { - if (!ao_fat_set_size(ao_file_offset + len)) - return 0; + ret = ao_fat_set_size(ao_file_offset + len); + if (ret < 0) + return ret; } while (len) { - offset = ao_file_offset & 0x1ff; - if (offset + len < 512) + offset = ao_file_offset & SECTOR_MASK; + if (offset + len < SECTOR_SIZE) this_time = len; else - this_time = 512 - offset; + this_time = SECTOR_SIZE - offset; - sector = ao_file_offset_to_sector(ao_file_offset); + sector = ao_fat_offset_to_sector(ao_file_offset); if (sector == 0xffffffff) break; - buf = ao_fat_block_get(sector); - if (!buf) + buf = ao_fat_sector_get(sector); + if (!buf) { + ret = -AO_FAT_EIO; break; - memcpy(buf + offset, src, this_time); - ao_fat_block_put(buf, 1); + } + memcpy(buf + offset, src_b, this_time); + ao_fat_sector_put(buf, 1); ret += this_time; len -= this_time; - src += this_time; + src_b += this_time; ao_file_offset += this_time; } - return 0; + return ret; } -uint32_t +/* + * ao_fat_seek + * + * Set the position for the next I/O operation + * Note that this doesn't actually change the size + * of the file if the requested position is beyond + * the current file length, that would take a future + * write + */ +int32_t ao_fat_seek(int32_t pos, uint8_t whence) { + if (!ao_file_opened) + return -AO_FAT_EBADF; + switch (whence) { case AO_FAT_SEEK_SET: ao_file_offset = pos; @@ -568,12 +776,16 @@ ao_fat_seek(int32_t pos, uint8_t whence) ao_file_offset = ao_file_dirent.size + pos; break; } - if (ao_file_offset > ao_file_dirent.size) - ao_fat_set_size(ao_file_offset); return ao_file_offset; } -uint8_t +/* + * ao_fat_unlink + * + * Remove a file from the directory, marking + * all clusters as free + */ +int8_t ao_fat_unlink(char name[11]) { uint16_t entry = 0; @@ -584,7 +796,13 @@ ao_fat_unlink(char name[11]) uint8_t *next; uint8_t *ent; uint8_t delete; - ao_fat_clear_cluster_chain(dirent.cluster); + + if (AO_FAT_IS_DIR(dirent.attr)) + return -AO_FAT_EISDIR; + if (!AO_FAT_IS_FILE(dirent.attr)) + return -AO_FAT_EPERM; + + ao_fat_free_cluster_chain(dirent.cluster); next = ao_fat_root_get(dirent.entry + 1); if (next && next[0] != AO_FAT_DENT_END) delete = AO_FAT_DENT_EMPTY; @@ -594,24 +812,24 @@ ao_fat_unlink(char name[11]) ao_fat_root_put(next, dirent.entry + 1, 0); ent = ao_fat_root_get(dirent.entry); if (ent) { - memset(ent, '\0', 0x20); + memset(ent, '\0', DIRENT_SIZE); *ent = delete; ao_fat_root_put(ent, dirent.entry, 1); } ao_bufio_flush(); - return 1; + return AO_FAT_SUCCESS; } } - return 0; + return -AO_FAT_ENOENT; } -uint8_t +int8_t ao_fat_rename(char old[11], char new[11]) { - return 0; + return -AO_FAT_EIO; } -uint8_t +int8_t ao_fat_readdir(uint16_t *entry, struct ao_fat_dirent *dirent) { uint8_t *dent; @@ -625,20 +843,15 @@ ao_fat_readdir(uint16_t *entry, struct ao_fat_dirent *dirent) ao_fat_root_put(dent, *entry, 0); return 0; } - if (dent[0] != AO_FAT_DENT_EMPTY && - (dent[0x0b] & (AO_FAT_FILE_DIRECTORY|AO_FAT_FILE_VOLUME_LABEL)) == 0) - break; + if (dent[0] != AO_FAT_DENT_EMPTY && (dent[0xb] & 0xf) != 0xf) { + ao_fat_dirent_init(dent, *entry, dirent); + ao_fat_root_put(dent, *entry, 0); + (*entry)++; + return 1; + } ao_fat_root_put(dent, *entry, 0); (*entry)++; } - memcpy(dirent->name, dent, 11); - dirent->attr = dent[0xb]; - dirent->size = get_u32(dent+0x1c); - dirent->cluster = get_u16(dent+0x1a); - dirent->entry = *entry; - ao_fat_root_put(dent, *entry, 0); - (*entry)++; - return 1; } static void @@ -648,8 +861,12 @@ ao_fat_list(void) struct ao_fat_dirent dirent; while (ao_fat_readdir(&entry, &dirent)) { - printf ("%-8.8s.%-3.3s %02x %d\n", - dirent.name, dirent.name + 8, dirent.attr, dirent.size); + printf ("%-8.8s.%-3.3s %02x %04x %d\n", + dirent.name, + dirent.name + 8, + dirent.attr, + dirent.cluster, + dirent.size); } } @@ -671,4 +888,3 @@ ao_fat_init(void) ao_bufio_init(); ao_cmd_register(&ao_fat_cmds[0]); } - diff --git a/src/drivers/ao_fat.h b/src/drivers/ao_fat.h index 2bf6222e..5b9b300f 100644 --- a/src/drivers/ao_fat.h +++ b/src/drivers/ao_fat.h @@ -21,6 +21,7 @@ void ao_fat_init(void); +#define AO_FAT_FILE_REGULAR 0x00 #define AO_FAT_FILE_READ_ONLY 0x01 #define AO_FAT_FILE_HIDDEN 0x02 #define AO_FAT_FILE_SYSTEM 0x04 @@ -31,32 +32,52 @@ ao_fat_init(void); #define AO_FAT_DENT_EMPTY 0xe5 #define AO_FAT_DENT_END 0x00 -uint8_t -ao_fat_open(char name[11]); - -uint8_t +#define AO_FAT_IS_FILE(attr) (((attr) & (AO_FAT_FILE_VOLUME_LABEL|AO_FAT_FILE_DIRECTORY|AO_FAT_FILE_ARCHIVE)) == 0) +#define AO_FAT_IS_DIR(attr) (((attr) & (AO_FAT_FILE_DIRECTORY)) == AO_FAT_FILE_DIRECTORY) + +#define AO_FAT_SUCCESS 0 +#define AO_FAT_EPERM 1 +#define AO_FAT_ENOENT 2 +#define AO_FAT_EIO 4 +#define AO_FAT_EBADF 9 +#define AO_FAT_EACCESS 13 +#define AO_FAT_EEXIST 17 +#define AO_FAT_ENOTDIR 20 +#define AO_FAT_EISDIR 21 +#define AO_FAT_EMFILE 24 +#define AO_FAT_EFBIG 27 +#define AO_FAT_ENOSPC 28 + +int8_t +ao_fat_open(char name[11], uint8_t mode); + +#define AO_FAT_OPEN_READ 0 +#define AO_FAT_OPEN_WRITE 1 +#define AO_FAT_OPEN_RW 2 + +int8_t ao_fat_creat(char name[11]); -void +int8_t ao_fat_close(void); int -ao_fat_read(uint8_t *dest, int len); +ao_fat_read(void *dest, int len); int -ao_fat_write(uint8_t *buf, int len); +ao_fat_write(void *src, int len); #define AO_FAT_SEEK_SET 0 #define AO_FAT_SEEK_CUR 1 #define AO_FAT_SEEK_END 2 -uint32_t -bao_fat_seek(int32_t pos, uint8_t whence); +int32_t +ao_fat_seek(int32_t pos, uint8_t whence); -uint8_t +int8_t ao_fat_unlink(char name[11]); -uint8_t +int8_t ao_fat_rename(char old[11], char new[11]); struct ao_fat_dirent { @@ -67,7 +88,7 @@ struct ao_fat_dirent { uint16_t entry; }; -uint8_t +int8_t ao_fat_readdir(uint16_t *entry, struct ao_fat_dirent *dirent); #endif /* _AO_FAT_H_ */ diff --git a/src/test/Makefile b/src/test/Makefile index 96170cc1..991bdbfc 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -66,4 +66,4 @@ ao_micropeak_test: ao_micropeak_test.c ao_microflight.c ao_kalman.h cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c - cc $(CFLAGS) -o $@ ao_fat_test.c + cc $(CFLAGS) -o $@ ao_fat_test.c -lssl diff --git a/src/test/ao_fat_test.c b/src/test/ao_fat_test.c index 22ac03ad..3f947034 100644 --- a/src/test/ao_fat_test.c +++ b/src/test/ao_fat_test.c @@ -18,11 +18,13 @@ #include #include #include +#include #include #include #include #include #include +#include #define AO_FAT_TEST @@ -40,7 +42,7 @@ void ao_panic(uint8_t panic) { printf ("panic %d\n", panic); - exit(1); + abort(); } #define AO_PANIC_BUFIO 15 @@ -64,9 +66,12 @@ struct ao_cmds { int fs_fd; +uint64_t total_reads, total_writes; + uint8_t ao_sdcard_read_block(uint32_t block, uint8_t *data) { + ++total_reads; lseek(fs_fd, block * 512, 0); return read(fs_fd, data, 512) == 512; } @@ -74,45 +79,308 @@ ao_sdcard_read_block(uint32_t block, uint8_t *data) uint8_t ao_sdcard_write_block(uint32_t block, uint8_t *data) { + ++total_writes; lseek(fs_fd, block * 512, 0); return write(fs_fd, data, 512) == 512; } +char *fs = "fs.fat"; + void ao_sdcard_init(void) { - fs_fd = open("fat.fs", 2); + char cmd[1024]; + + snprintf(cmd, sizeof(cmd), "rm -f %s && mkfs.vfat -C %s 16384", fs, fs); + if (system (cmd) != 0) { + fprintf(stderr, "'%s' failed\n", cmd); + exit(1); + } + fs_fd = open(fs, 2); + if (fs_fd < 0) { + perror (fs); + exit(1); + } } #include "ao_bufio.c" +void +check_bufio(char *where) +{ + int b; + + for (b = 0; b < AO_NUM_BUF; b++) { + if (ao_bufio[b].busy) { + printf ("%s: buffer %d busy. block %d seqno %u\n", + where, b, ao_bufio[b].block, ao_bufio[b].seqno & 0xffff); + abort(); + } + } +} + + +void +check_fat(void); + #include "ao_fat.c" +/* Get the next cluster entry in the chain */ +static uint16_t +ao_fat_entry_raw_read(uint16_t cluster, uint8_t fat) +{ + uint32_t sector; + uint16_t offset; + uint8_t *buf; + uint16_t ret; + +// cluster -= 2; + sector = cluster >> (SECTOR_SHIFT - 1); + offset = (cluster << 1) & SECTOR_MASK; + buf = ao_fat_sector_get(fat_start + fat * sectors_per_fat + sector); + if (!buf) + return 0; + ret = get_u16(buf + offset); + ao_fat_sector_put(buf, 0); + return ret; +} + +void +dump_fat(void) +{ + int e; + + printf ("\n **** FAT ****\n\n"); + for (e = 0; e < number_cluster; e++) { + if ((e & 0xf) == 0x0) + printf ("%04x: ", e); + printf (" %04x", ao_fat_entry_raw_read(e, 0)); + if ((e & 0xf) == 0xf) + putchar ('\n'); + } +} + +void +fat_list(void) +{ + uint16_t entry = 0; + struct ao_fat_dirent dirent; + + printf (" **** Root directory ****\n"); + while (ao_fat_readdir(&entry, &dirent)) { + printf ("%04x: %-8.8s.%-3.3s %02x %04x %d\n", + entry, + dirent.name, + dirent.name + 8, + dirent.attr, + dirent.cluster, + dirent.size); + } + + printf (" **** End of root directory ****\n"); +} + +void +fatal(char *msg, ...) +{ + dump_fat(); + fat_list(); + + va_list l; + va_start(l, msg); + vfprintf(stderr, msg, l); + va_end(l); + + abort(); +} + +void +check_fat(void) +{ + int e; + int f; + + for (e = 0; e < number_cluster; e++) { + uint16_t v = ao_fat_entry_raw_read(e, 0); + for (f = 1; f < number_fat; f++) { + if (ao_fat_entry_raw_read(e, f) != v) + fatal ("fats differ at %d\n", e); + } + } +} + +uint16_t +check_file(uint16_t dent, uint16_t first_cluster, uint8_t *used) +{ + uint16_t clusters = 0; + uint16_t cluster; + + if (!first_cluster) + return 0; + + for (cluster = first_cluster; + (cluster & 0xfff8) != 0xfff8; + cluster = ao_fat_entry_raw_read(cluster, 0)) + { + if (!ao_fat_cluster_valid(cluster)) + fatal("file %d: invalid cluster %04x\n", dent, cluster); + if (used[cluster]) + fatal("file %d: duplicate cluster %04x\n", dent, cluster); + used[cluster] = 1; + clusters++; + } + return clusters; +} + +void +check_fs(void) +{ + uint16_t r; + uint16_t cluster, chain; + uint8_t *used; + + check_fat(); + + used = calloc(1, number_cluster); + + for (r = 0; r < root_entries; r++) { + uint8_t *dent = ao_fat_root_get(r); + uint16_t clusters; + uint32_t size; + uint16_t first_cluster; + uint8_t name[11]; + + if (!dent) + fatal("cannot map dent %d\n", r); + memcpy(name, dent+0, 11); + first_cluster = get_u16(dent + 0x1a); + size = get_u32(dent + 0x1c); + ao_fat_root_put(dent, r, 0); + + if (name[0] == AO_FAT_DENT_END) { + break; + } + + clusters = check_file(r, first_cluster, used); + if (size > clusters * bytes_per_cluster) + fatal("file %d: size %u beyond clusters %d (%u)\n", + r, size, clusters, clusters * bytes_per_cluster); + if (size <= (clusters - 1) * bytes_per_cluster) + fatal("file %d: size %u too small clusters %d (%u)\n", + r, size, clusters, clusters * bytes_per_cluster); + } + for (; r < root_entries; r++) { + uint8_t *dent = ao_fat_root_get(r); + if (!dent) + fatal("cannot map dent %d\n", r); + if (dent[0] != AO_FAT_DENT_END) + fatal("found non-zero dent past end %d\n", r); + ao_fat_root_put(dent, r, 0); + } + + for (cluster = 0; cluster < 2; cluster++) { + chain = ao_fat_entry_raw_read(cluster, 0); + + if ((chain & 0xfff8) != 0xfff8) + fatal("cluster %d: not marked busy\n", cluster); + } + for (; cluster < number_cluster; cluster++) { + chain = ao_fat_entry_raw_read(cluster, 0); + + if (chain != 0) { + if (used[cluster] == 0) + fatal("cluster %d: marked busy, but not in any file\n", cluster); + } else { + if (used[cluster] != 0) + fatal("cluster %d: marked free, but foudn in file\n", cluster); + } + } +} + +#define NUM_FILES 512 +#define LINES_FILE 1000 + +uint32_t sizes[NUM_FILES]; + +unsigned char md5[NUM_FILES][MD5_DIGEST_LENGTH]; + int main(int argc, char **argv) { - uint8_t data[15]; - int len; + char name[12]; + int id; + MD5_CTX ctx; + unsigned char md5_check[MD5_DIGEST_LENGTH]; + + if (argv[1]) + fs = argv[1]; + ao_fat_init(); - ao_fat_test(); - if (ao_fat_open("DATALOG TXT")) { - printf ("DATALOG.TXT\n"); - while ((len = ao_fat_read(data, sizeof (data))) > 0) { - write(1, data, len); + + check_bufio("top"); + ao_fat_setup(); + + check_fs(); + check_bufio("after setup"); + printf (" **** Creating %d files\n", NUM_FILES); + + for (id = 0; id < NUM_FILES; id++) { + sprintf(name, "D%07dTXT", id); + if (ao_fat_creat(name) == AO_FAT_SUCCESS) { + int j; + char line[64]; + check_bufio("file created"); + MD5_Init(&ctx); + for (j = 0; j < 1000; j++) { + int len; + sprintf (line, "Hello, world %d %d\r\n", id, j); + len = strlen(line); + ao_fat_write((uint8_t *) line, len); + MD5_Update(&ctx, line, len); + sizes[id] += len; + } + ao_fat_close(); + MD5_Final(&md5[id][0], &ctx); + if (id == 0) { + printf ("MD5 write %d:", id); + for (j = 0; j < MD5_DIGEST_LENGTH; j++) + printf(" %02x", md5[id][j]); + printf ("\n"); + } + check_bufio("file written"); } - ao_fat_close(); -// ao_fat_unlink("DATALOG TXT"); } - if (ao_fat_open("NEWFILE TXT")) { - printf ("NEWFILE.TXT\n"); - while ((len = ao_fat_read(data, sizeof (data))) > 0) { - write(1, data, len); + + check_bufio("all files created"); + printf (" **** All done creating files\n"); + check_fs(); + + printf (" **** Comparing %d files\n", NUM_FILES); + + for (id = 0; id < NUM_FILES; id++) { + char buf[337]; + sprintf(name, "D%07dTXT", id); + if (ao_fat_open(name, AO_FAT_OPEN_READ) == AO_FAT_SUCCESS) { + int len; + + MD5_Init(&ctx); + while ((len = ao_fat_read((uint8_t *) buf, sizeof(buf))) > 0) { + MD5_Update(&ctx, buf, len); + } + ao_fat_close(); + MD5_Final(md5_check, &ctx); + if (id == 0) { + int j; + printf ("MD5 read %d:", id); + for (j = 0; j < MD5_DIGEST_LENGTH; j++) + printf(" %02x", md5_check[j]); + printf ("\n"); + } + if (memcmp(md5_check, &md5[id][0], sizeof (md5_check)) != 0) + fatal ("checksum failed file %d\n", id); + check_bufio("file shown"); } - ao_fat_close(); - } - if (ao_fat_creat ("NEWFILE TXT")) { - for (len = 0; len < 4095; len++) - ao_fat_write((uint8_t *) "hello, world!\n", 14); - ao_fat_close(); } + + printf ("\n **** Total IO: read %llu write %llu\n", total_reads, total_writes); return 0; } -- cgit v1.2.3 From aa7eac32adf4c2cdf441991d02411758f2682d1e Mon Sep 17 00:00:00 2001 From: Bdale Garbee Date: Mon, 22 Apr 2013 13:00:26 -0600 Subject: name change from MegaMetrum to TeleMega --- altoslib/AltosConfigData.java | 2 +- altoslib/AltosIdleMonitor.java | 4 +- altoslib/AltosLib.java | 6 +- altosui/AltosDataChooser.java | 2 +- altosui/AltosEepromDownload.java | 2 +- altosui/Makefile.am | 2 +- altosui/altos-windows.nsi.in | 2 +- altosuilib/AltosUSBDevice.java | 6 +- ao-bringup/megametrum.cfg | 4 - ao-bringup/megametrum.gdb | 2 - ao-bringup/telemega.cfg | 4 + ao-bringup/telemega.gdb | 2 + debian/docs | 2 +- doc/altos.xsl | 2 +- doc/megametrum-outline.pdf | Bin 4349 -> 0 bytes doc/megametrum-outline.svg | 244 ------------------------ doc/release-notes-1.2.xsl | 2 +- src/Makefile | 2 +- src/core/ao_fec_rx.c | 2 +- src/core/ao_log.h | 2 +- src/core/ao_log_mega.c | 2 +- src/core/ao_telemetry.c | 2 +- src/drivers/ao_companion.c | 2 +- src/megametrum-v0.1/.gitignore | 2 - src/megametrum-v0.1/Makefile | 131 ------------- src/megametrum-v0.1/ao_megametrum.c | 100 ---------- src/megametrum-v0.1/ao_pins.h | 359 ------------------------------------ src/megametrum-v0.1/stlink-pins | 57 ------ src/stm/ao_i2c_stm.c | 2 +- src/telelco-v0.1/Makefile | 2 +- src/test/Makefile | 2 +- src/test/ao_flight_test.c | 22 +-- telemetrum.inf | 8 +- 33 files changed, 46 insertions(+), 939 deletions(-) delete mode 100644 ao-bringup/megametrum.cfg delete mode 100644 ao-bringup/megametrum.gdb create mode 100644 ao-bringup/telemega.cfg create mode 100644 ao-bringup/telemega.gdb delete mode 100644 doc/megametrum-outline.pdf delete mode 100644 doc/megametrum-outline.svg delete mode 100644 src/megametrum-v0.1/.gitignore delete mode 100644 src/megametrum-v0.1/Makefile delete mode 100644 src/megametrum-v0.1/ao_megametrum.c delete mode 100644 src/megametrum-v0.1/ao_pins.h delete mode 100644 src/megametrum-v0.1/stlink-pins (limited to 'src/test/Makefile') diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java index 57605607..2ca5a7a5 100644 --- a/altoslib/AltosConfigData.java +++ b/altoslib/AltosConfigData.java @@ -504,7 +504,7 @@ public class AltosConfigData implements Iterable { switch (log_format) { case AltosLib.AO_LOG_FORMAT_FULL: case AltosLib.AO_LOG_FORMAT_TINY: - case AltosLib.AO_LOG_FORMAT_MEGAMETRUM: + case AltosLib.AO_LOG_FORMAT_TELEMEGA: link.printf("l\n"); read_link(link, "done"); default: diff --git a/altoslib/AltosIdleMonitor.java b/altoslib/AltosIdleMonitor.java index c379547f..2e4ddef2 100644 --- a/altoslib/AltosIdleMonitor.java +++ b/altoslib/AltosIdleMonitor.java @@ -52,11 +52,11 @@ public class AltosIdleMonitor extends Thread { } boolean has_sensor_mm(AltosConfigData config_data) { - return config_data.product.startsWith("MegaMetrum"); + return config_data.product.startsWith("TeleMega"); } boolean has_gps(AltosConfigData config_data) { - return config_data.product.startsWith("TeleMetrum") || config_data.product.startsWith("MegaMetrum"); + return config_data.product.startsWith("TeleMetrum") || config_data.product.startsWith("TeleMega"); } AltosRecord sensor_mm(AltosConfigData config_data) throws InterruptedException, TimeoutException { diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index 0b5475f7..25d17e72 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -50,7 +50,7 @@ public class AltosLib { public static final int AO_LOG_SERIAL_NUMBER = 2002; public static final int AO_LOG_LOG_FORMAT = 2003; - /* Added for header fields in megametrum files */ + /* Added for header fields in telemega files */ public static final int AO_LOG_BARO_RESERVED = 3000; public static final int AO_LOG_BARO_SENS = 3001; public static final int AO_LOG_BARO_OFF = 3002; @@ -89,7 +89,7 @@ public class AltosLib { public final static int product_telelco = 0x0010; public final static int product_telescience = 0x0011; public final static int product_telepyro =0x0012; - public final static int product_megametrum = 0x0023; + public final static int product_telemega = 0x0023; public final static int product_megadongle = 0x0024; public final static int product_telegps = 0x0025; public final static int product_altusmetrum_min = 0x000a; @@ -215,7 +215,7 @@ public class AltosLib { public static final int AO_LOG_FORMAT_TINY = 2; public static final int AO_LOG_FORMAT_TELEMETRY = 3; public static final int AO_LOG_FORMAT_TELESCIENCE = 4; - public static final int AO_LOG_FORMAT_MEGAMETRUM = 5; + public static final int AO_LOG_FORMAT_TELEMEGA = 5; public static final int AO_LOG_FORMAT_NONE = 127; public static boolean isspace(int c) { diff --git a/altosui/AltosDataChooser.java b/altosui/AltosDataChooser.java index 7de18afb..f914f138 100644 --- a/altosui/AltosDataChooser.java +++ b/altosui/AltosDataChooser.java @@ -75,7 +75,7 @@ public class AltosDataChooser extends JFileChooser { "eeprom")); setFileFilter(new FileNameExtensionFilter("Telemetry file", "telem")); - setFileFilter(new FileNameExtensionFilter("MegaMetrum eeprom file", + setFileFilter(new FileNameExtensionFilter("TeleMega eeprom file", "mega")); setFileFilter(new FileNameExtensionFilter("Flight data file", "telem", "eeprom", "mega")); diff --git a/altosui/AltosEepromDownload.java b/altosui/AltosEepromDownload.java index 801d4ec0..a0523b58 100644 --- a/altosui/AltosEepromDownload.java +++ b/altosui/AltosEepromDownload.java @@ -366,7 +366,7 @@ public class AltosEepromDownload implements Runnable { extension = "science"; CaptureTeleScience(eechunk); break; - case AltosLib.AO_LOG_FORMAT_MEGAMETRUM: + case AltosLib.AO_LOG_FORMAT_TELEMEGA: extension = "mega"; CaptureMega(eechunk); } diff --git a/altosui/Makefile.am b/altosui/Makefile.am index 96cf77f2..4bfef47c 100644 --- a/altosui/Makefile.am +++ b/altosui/Makefile.am @@ -157,7 +157,7 @@ FIRMWARE=$(FIRMWARE_TM) $(FIRMWARE_TELEMINI) $(FIRMWARE_TD) ALTUSMETRUM_DOC=$(top_srcdir)/doc/altusmetrum.pdf ALTOS_DOC=$(top_srcdir)/doc/altos.pdf TELEMETRY_DOC=$(top_srcdir)/doc/telemetry.pdf -TEMPLATE_DOC=$(top_srcdir)/doc/telemetrum-outline.pdf $(top_srcdir)/doc/megametrum-outline.pdf +TEMPLATE_DOC=$(top_srcdir)/doc/telemetrum-outline.pdf $(top_srcdir)/doc/telemega-outline.pdf DOC=$(ALTUSMETRUM_DOC) $(ALTOS_DOC) $(TELEMETRY_DOC) $(TEMPLATE_DOC) diff --git a/altosui/altos-windows.nsi.in b/altosui/altos-windows.nsi.in index cde54b41..9886e4a2 100644 --- a/altosui/altos-windows.nsi.in +++ b/altosui/altos-windows.nsi.in @@ -131,7 +131,7 @@ Section "Documentation" File "../doc/altos.pdf" File "../doc/telemetry.pdf" File "../doc/telemetrum-outline.pdf" - File "../doc/megametrum-outline.pdf" + File "../doc/telemega-outline.pdf" SectionEnd Section "Uninstaller" diff --git a/altosuilib/AltosUSBDevice.java b/altosuilib/AltosUSBDevice.java index 5268927c..0f6cbd10 100644 --- a/altosuilib/AltosUSBDevice.java +++ b/altosuilib/AltosUSBDevice.java @@ -72,11 +72,11 @@ public class AltosUSBDevice extends altos_device implements AltosDevice { return matchProduct(AltosUILib.product_teledongle) || matchProduct(AltosUILib.product_teleterra) || matchProduct(AltosUILib.product_telebt) || - matchProduct(AltosUILib.product_megadongle); + matchProduct(AltosUILib.product_telemega); if (want_product == AltosUILib.product_altimeter) return matchProduct(AltosUILib.product_telemetrum) || - matchProduct(AltosUILib.product_megametrum) || + matchProduct(AltosUILib.product_telemega) || matchProduct(AltosUILib.product_telegps); int have_product = getProduct(); @@ -110,4 +110,4 @@ public class AltosUSBDevice extends altos_device implements AltosDevice { return device_list; } -} \ No newline at end of file +} diff --git a/ao-bringup/megametrum.cfg b/ao-bringup/megametrum.cfg deleted file mode 100644 index e95c6f2b..00000000 --- a/ao-bringup/megametrum.cfg +++ /dev/null @@ -1,4 +0,0 @@ -# openocd config for MegaMetrum using the Olimex ARM-USB-OCD dongle - -source /opt/stm32/share/openocd/scripts/interface/olimex-arm-usb-ocd.cfg -source /opt/stm32/share/openocd/scripts/target/stm32l.cfg diff --git a/ao-bringup/megametrum.gdb b/ao-bringup/megametrum.gdb deleted file mode 100644 index 964ae1f3..00000000 --- a/ao-bringup/megametrum.gdb +++ /dev/null @@ -1,2 +0,0 @@ -target remote localhost:3333 -monitor poll diff --git a/ao-bringup/telemega.cfg b/ao-bringup/telemega.cfg new file mode 100644 index 00000000..f6b96c13 --- /dev/null +++ b/ao-bringup/telemega.cfg @@ -0,0 +1,4 @@ +# openocd config for TeleMega using the Olimex ARM-USB-OCD dongle + +source /opt/stm32/share/openocd/scripts/interface/olimex-arm-usb-ocd.cfg +source /opt/stm32/share/openocd/scripts/target/stm32l.cfg diff --git a/ao-bringup/telemega.gdb b/ao-bringup/telemega.gdb new file mode 100644 index 00000000..964ae1f3 --- /dev/null +++ b/ao-bringup/telemega.gdb @@ -0,0 +1,2 @@ +target remote localhost:3333 +monitor poll diff --git a/debian/docs b/debian/docs index 3ac75ad4..dcdb7763 100644 --- a/debian/docs +++ b/debian/docs @@ -7,4 +7,4 @@ doc/telemetry.pdf doc/altos.html doc/altos.pdf doc/telemetrum-outline.pdf -doc/megametrum-outline.pdf +doc/telemega-outline.pdf diff --git a/doc/altos.xsl b/doc/altos.xsl index c301adde..5af94725 100644 --- a/doc/altos.xsl +++ b/doc/altos.xsl @@ -50,7 +50,7 @@ STM32L series from ST Microelectronics. This ARM Cortex-M3 based microcontroller offers low power consumption and a wide variety of built-in peripherals. Altus Metrum uses - this in the MegaMetrum, MegaDongle and TeleLCO projects. + this in the TeleMega, MegaDongle and TeleLCO projects. diff --git a/doc/megametrum-outline.pdf b/doc/megametrum-outline.pdf deleted file mode 100644 index f8fc26e2..00000000 Binary files a/doc/megametrum-outline.pdf and /dev/null differ diff --git a/doc/megametrum-outline.svg b/doc/megametrum-outline.svg deleted file mode 100644 index e8d74d38..00000000 --- a/doc/megametrum-outline.svg +++ /dev/null @@ -1,244 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - image/svg+xml - - - - - - - - - - - - - - - - - - - - - - - - - - - - - UP - - diff --git a/doc/release-notes-1.2.xsl b/doc/release-notes-1.2.xsl index 610fa1a2..b254c7b5 100644 --- a/doc/release-notes-1.2.xsl +++ b/doc/release-notes-1.2.xsl @@ -41,7 +41,7 @@ raised, breaking the Descent tab contents. - Add preliminary MegaMetrum support, including configuration, + Add preliminary TeleMega support, including configuration, data download and analysis. diff --git a/src/Makefile b/src/Makefile index d91a235a..e271ddf3 100644 --- a/src/Makefile +++ b/src/Makefile @@ -29,7 +29,7 @@ AVRDIRS=\ telescience-v0.1 telescience-pwm telepyro-v0.1 micropeak ARMDIRS=\ - megametrum-v0.1 megadongle-v0.1 stm-bringup stm-demo telelco-v0.1 \ + telemega-v0.1 megadongle-v0.1 stm-bringup stm-demo telelco-v0.1 \ telescience-v0.2 ifneq ($(shell which sdcc),) diff --git a/src/core/ao_fec_rx.c b/src/core/ao_fec_rx.c index 072a9e90..c4f5559a 100644 --- a/src/core/ao_fec_rx.c +++ b/src/core/ao_fec_rx.c @@ -18,7 +18,7 @@ #include #include -#ifdef MEGAMETRUM +#ifdef TELEMEGA #include #endif diff --git a/src/core/ao_log.h b/src/core/ao_log.h index cac78771..a68a40dd 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -43,7 +43,7 @@ extern __pdata enum ao_flight_state ao_log_state; #define AO_LOG_FORMAT_TINY 2 /* two byte state/baro records */ #define AO_LOG_FORMAT_TELEMETRY 3 /* 32 byte ao_telemetry records */ #define AO_LOG_FORMAT_TELESCIENCE 4 /* 32 byte typed telescience records */ -#define AO_LOG_FORMAT_MEGAMETRUM 5 /* 32 byte typed megametrum records */ +#define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index ba3f7bfc..abf953a6 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -23,7 +23,7 @@ static __xdata uint8_t ao_log_mutex; static __xdata struct ao_log_mega log; -__code uint8_t ao_log_format = AO_LOG_FORMAT_MEGAMETRUM; +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA; static uint8_t ao_log_csum(__xdata uint8_t *b) __reentrant diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 3aa315c7..03aa48d8 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -29,7 +29,7 @@ static __pdata uint16_t ao_aprs_time; #include #endif -#if defined(MEGAMETRUM) +#if defined(TELEMEGA) #define AO_SEND_MEGA 1 #endif diff --git a/src/drivers/ao_companion.c b/src/drivers/ao_companion.c index 0ebe8429..0f405253 100644 --- a/src/drivers/ao_companion.c +++ b/src/drivers/ao_companion.c @@ -18,7 +18,7 @@ #include #include -#ifdef MEGAMETRUM +#ifdef TELEMEGA #define ao_spi_slow(b) #define ao_spi_fast(b) #endif diff --git a/src/megametrum-v0.1/.gitignore b/src/megametrum-v0.1/.gitignore deleted file mode 100644 index b04d3950..00000000 --- a/src/megametrum-v0.1/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -ao_product.h -megametrum-*.elf diff --git a/src/megametrum-v0.1/Makefile b/src/megametrum-v0.1/Makefile deleted file mode 100644 index a5fdcbb2..00000000 --- a/src/megametrum-v0.1/Makefile +++ /dev/null @@ -1,131 +0,0 @@ -# -# AltOS build -# -# - -include ../stm/Makefile.defs - -INC = \ - ao.h \ - ao_arch.h \ - ao_arch_funcs.h \ - ao_companion.h \ - ao_data.h \ - ao_sample.h \ - ao_pins.h \ - altitude-pa.h \ - ao_kalman.h \ - ao_product.h \ - ao_ms5607.h \ - ao_hmc5883.h \ - ao_mpu6000.h \ - ao_mma655x.h \ - ao_cc1120_CC1120.h \ - ao_profile.h \ - ao_task.h \ - ao_whiten.h \ - ao_sample_profile.h \ - ao_mpu.h \ - stm32l.h \ - Makefile - -# -# Common AltOS sources -# -# ao_hmc5883.c - -#PROFILE=ao_profile.c -#PROFILE_DEF=-DAO_PROFILE=1 - -#SAMPLE_PROFILE=ao_sample_profile.c \ -# ao_sample_profile_timer.c -#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1 - -#STACK_GUARD=ao_mpu_stm.c -#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 - -ALTOS_SRC = \ - ao_interrupt.c \ - ao_product.c \ - ao_romconfig.c \ - ao_cmd.c \ - ao_config.c \ - ao_task.c \ - ao_led.c \ - ao_stdio.c \ - ao_panic.c \ - ao_timer.c \ - ao_mutex.c \ - ao_serial_stm.c \ - ao_gps_skytraq.c \ - ao_gps_report_mega.c \ - ao_ignite.c \ - ao_freq.c \ - ao_dma_stm.c \ - ao_spi_stm.c \ - ao_cc1120.c \ - ao_fec_tx.c \ - ao_fec_rx.c \ - ao_data.c \ - ao_ms5607.c \ - ao_mma655x.c \ - ao_hmc5883.c \ - ao_adc_stm.c \ - ao_beep_stm.c \ - ao_storage.c \ - ao_m25.c \ - ao_usb_stm.c \ - ao_exti_stm.c \ - ao_report.c \ - ao_i2c_stm.c \ - ao_mpu6000.c \ - ao_convert_pa.c \ - ao_log.c \ - ao_log_mega.c \ - ao_sample.c \ - ao_kalman.c \ - ao_flight.c \ - ao_telemetry.c \ - ao_packet_slave.c \ - ao_packet.c \ - ao_companion.c \ - ao_pyro.c \ - ao_aprs.c \ - $(PROFILE) \ - $(SAMPLE_PROFILE) \ - $(STACK_GUARD) - -PRODUCT=MegaMetrum-v0.1 -PRODUCT_DEF=-DMEGAMETRUM -IDPRODUCT=0x0023 - -CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g - -PROGNAME=megametrum-v0.1 -PROG=$(PROGNAME)-$(VERSION).elf - -SRC=$(ALTOS_SRC) ao_megametrum.c -OBJ=$(SRC:.c=.o) - -all: $(PROG) - -$(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc - -../altitude-pa.h: make-altitude-pa - nickle $< > $@ - -$(OBJ): $(INC) - -ao_product.h: ao-make-product.5c ../Version - $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ - -distclean: clean - -clean: - rm -f *.o $(PROGNAME)-*.elf - rm -f ao_product.h - -install: - -uninstall: diff --git a/src/megametrum-v0.1/ao_megametrum.c b/src/megametrum-v0.1/ao_megametrum.c deleted file mode 100644 index fbdab64a..00000000 --- a/src/megametrum-v0.1/ao_megametrum.c +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Copyright © 2011 Keith Packard - * - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#if HAS_SAMPLE_PROFILE -#include -#endif -#include -#if HAS_STACK_GUARD -#include -#endif - -int -main(void) -{ - ao_clock_init(); - -#if HAS_STACK_GUARD - ao_mpu_init(); -#endif - - ao_task_init(); - ao_serial_init(); - ao_led_init(LEDS_AVAILABLE); - ao_led_on(AO_LED_GREEN); - ao_timer_init(); - - ao_i2c_init(); - ao_spi_init(); - ao_dma_init(); - ao_exti_init(); - - ao_adc_init(); -#if HAS_BEEP - ao_beep_init(); -#endif - ao_cmd_init(); - -#if HAS_MS5607 - ao_ms5607_init(); -#endif -#if HAS_HMC5883 - ao_hmc5883_init(); -#endif -#if HAS_MPU6000 - ao_mpu6000_init(); -#endif -#if HAS_MMA655X - ao_mma655x_init(); -#endif - - ao_storage_init(); - - ao_flight_init(); - ao_log_init(); - ao_report_init(); - - ao_usb_init(); - ao_gps_init(); - ao_gps_report_mega_init(); - ao_telemetry_init(); - ao_radio_init(); - ao_packet_slave_init(FALSE); - ao_igniter_init(); - ao_companion_init(); - ao_pyro_init(); - - ao_config_init(); -#if AO_PROFILE - ao_profile_init(); -#endif -#if HAS_SAMPLE_PROFILE - ao_sample_profile_init(); -#endif - - ao_start_scheduler(); - return 0; -} diff --git a/src/megametrum-v0.1/ao_pins.h b/src/megametrum-v0.1/ao_pins.h deleted file mode 100644 index 4c645871..00000000 --- a/src/megametrum-v0.1/ao_pins.h +++ /dev/null @@ -1,359 +0,0 @@ -/* - * Copyright © 2012 Keith Packard - * - * 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. - */ - -#ifndef _AO_PINS_H_ -#define _AO_PINS_H_ - -#define HAS_TASK_QUEUE 1 - -/* 8MHz High speed external crystal */ -#define AO_HSE 8000000 - -/* PLLVCO = 96MHz (so that USB will work) */ -#define AO_PLLMUL 12 -#define AO_RCC_CFGR_PLLMUL (STM_RCC_CFGR_PLLMUL_12) - -/* SYSCLK = 32MHz (no need to go faster than CPU) */ -#define AO_PLLDIV 3 -#define AO_RCC_CFGR_PLLDIV (STM_RCC_CFGR_PLLDIV_3) - -/* HCLK = 32MHz (CPU clock) */ -#define AO_AHB_PRESCALER 1 -#define AO_RCC_CFGR_HPRE_DIV STM_RCC_CFGR_HPRE_DIV_1 - -/* Run APB1 at 16MHz (HCLK/2) */ -#define AO_APB1_PRESCALER 2 -#define AO_RCC_CFGR_PPRE1_DIV STM_RCC_CFGR_PPRE2_DIV_2 - -/* Run APB2 at 16MHz (HCLK/2) */ -#define AO_APB2_PRESCALER 2 -#define AO_RCC_CFGR_PPRE2_DIV STM_RCC_CFGR_PPRE2_DIV_2 - -#define HAS_SERIAL_1 1 -#define USE_SERIAL_1_STDIN 0 -#define SERIAL_1_PB6_PB7 0 -#define SERIAL_1_PA9_PA10 1 - -#define HAS_SERIAL_2 0 -#define USE_SERIAL_2_STDIN 0 -#define SERIAL_2_PA2_PA3 0 -#define SERIAL_2_PD5_PD6 0 - -#define HAS_SERIAL_3 1 -#define USE_SERIAL_3_STDIN 0 -#define SERIAL_3_PB10_PB11 0 -#define SERIAL_3_PC10_PC11 1 -#define SERIAL_3_PD8_PD9 0 - -#define ao_gps_getchar ao_serial3_getchar -#define ao_gps_putchar ao_serial3_putchar -#define ao_gps_set_speed ao_serial3_set_speed -#define ao_gps_fifo (ao_stm_usart3.rx_fifo) - -#define HAS_EEPROM 1 -#define USE_INTERNAL_FLASH 0 -#define HAS_USB 1 -#define HAS_BEEP 1 -#define HAS_RADIO 1 -#define HAS_TELEMETRY 1 -#define HAS_APRS 1 - -#define HAS_SPI_1 1 -#define SPI_1_PA5_PA6_PA7 1 /* Barometer */ -#define SPI_1_PB3_PB4_PB5 0 -#define SPI_1_PE13_PE14_PE15 1 /* Accelerometer */ -#define SPI_1_OSPEEDR STM_OSPEEDR_10MHz - -#define HAS_SPI_2 1 -#define SPI_2_PB13_PB14_PB15 1 /* Flash, Companion */ -#define SPI_2_PD1_PD3_PD4 0 -#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz - -#define SPI_2_PORT (&stm_gpiob) -#define SPI_2_SCK_PIN 13 -#define SPI_2_MISO_PIN 14 -#define SPI_2_MOSI_PIN 15 - -#define HAS_I2C_1 1 -#define I2C_1_PB8_PB9 1 - -#define HAS_I2C_2 1 -#define I2C_2_PB10_PB11 1 - -#define PACKET_HAS_SLAVE 1 -#define PACKET_HAS_MASTER 0 - -#define LOW_LEVEL_DEBUG 0 - -#define LED_PORT_ENABLE STM_RCC_AHBENR_GPIOCEN -#define LED_PORT (&stm_gpioc) -#define LED_PIN_RED 8 -#define LED_PIN_GREEN 9 -#define AO_LED_RED (1 << LED_PIN_RED) -#define AO_LED_GREEN (1 << LED_PIN_GREEN) - -#define LEDS_AVAILABLE (AO_LED_RED | AO_LED_GREEN) - -#define HAS_GPS 1 -#define HAS_FLIGHT 1 -#define HAS_ADC 1 -#define HAS_LOG 1 - -/* - * Igniter - */ - -#define HAS_IGNITE 1 -#define HAS_IGNITE_REPORT 1 - -#define AO_SENSE_DROGUE(p) ((p)->adc.sense[0]) -#define AO_SENSE_MAIN(p) ((p)->adc.sense[1]) -#define AO_IGNITER_CLOSED 400 -#define AO_IGNITER_OPEN 60 - -#define AO_IGNITER_DROGUE_PORT (&stm_gpiod) -#define AO_IGNITER_DROGUE_PIN 6 - -#define AO_IGNITER_MAIN_PORT (&stm_gpiod) -#define AO_IGNITER_MAIN_PIN 7 - -#define AO_PYRO_PORT_0 (&stm_gpiob) -#define AO_PYRO_PIN_0 5 - -#define AO_PYRO_PORT_1 (&stm_gpioe) -#define AO_PYRO_PIN_1 4 - -#define AO_PYRO_PORT_2 (&stm_gpioe) -#define AO_PYRO_PIN_2 6 - -#define AO_PYRO_PORT_3 (&stm_gpioe) -#define AO_PYRO_PIN_3 5 - -/* Number of general purpose pyro channels available */ -#define AO_PYRO_NUM 4 - -#define AO_IGNITER_SET_DROGUE(v) stm_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, v) -#define AO_IGNITER_SET_MAIN(v) stm_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, v) - -/* - * ADC - */ -#define AO_DATA_RING 32 -#define AO_ADC_NUM_SENSE 6 - -struct ao_adc { - int16_t sense[AO_ADC_NUM_SENSE]; - int16_t v_batt; - int16_t v_pbatt; - int16_t accel_ref; - int16_t accel; - int16_t temp; -}; - -#define AO_ADC_SENSE_A 0 -#define AO_ADC_SENSE_A_PORT (&stm_gpioa) -#define AO_ADC_SENSE_A_PIN 0 - -#define AO_ADC_SENSE_B 1 -#define AO_ADC_SENSE_B_PORT (&stm_gpioa) -#define AO_ADC_SENSE_B_PIN 1 - -#define AO_ADC_SENSE_C 2 -#define AO_ADC_SENSE_C_PORT (&stm_gpioa) -#define AO_ADC_SENSE_C_PIN 2 - -#define AO_ADC_SENSE_D 3 -#define AO_ADC_SENSE_D_PORT (&stm_gpioa) -#define AO_ADC_SENSE_D_PIN 3 - -#define AO_ADC_SENSE_E 4 -#define AO_ADC_SENSE_E_PORT (&stm_gpioa) -#define AO_ADC_SENSE_E_PIN 4 - -#define AO_ADC_SENSE_F 22 -#define AO_ADC_SENSE_F_PORT (&stm_gpioe) -#define AO_ADC_SENSE_F_PIN 7 - -#define AO_ADC_V_BATT 8 -#define AO_ADC_V_BATT_PORT (&stm_gpiob) -#define AO_ADC_V_BATT_PIN 0 - -#define AO_ADC_V_PBATT 9 -#define AO_ADC_V_PBATT_PORT (&stm_gpiob) -#define AO_ADC_V_PBATT_PIN 1 - -#define AO_ADC_ACCEL_REF 10 -#define AO_ADC_ACCEL_REF_PORT (&stm_gpioc) -#define AO_ADC_ACCEL_REF_PIN 0 - -#define AO_ADC_ACCEL 11 -#define AO_ADC_ACCEL_PORT (&stm_gpioc) -#define AO_ADC_ACCEL_PIN 1 - -#define AO_ADC_TEMP 16 - -#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ - (1 << STM_RCC_AHBENR_GPIOEEN) | \ - (1 << STM_RCC_AHBENR_GPIOBEN) | \ - (1 << STM_RCC_AHBENR_GPIOCEN)) - -#define AO_NUM_ADC_PIN (AO_ADC_NUM_SENSE + 4) - -#define AO_ADC_PIN0_PORT AO_ADC_SENSE_A_PORT -#define AO_ADC_PIN0_PIN AO_ADC_SENSE_A_PIN -#define AO_ADC_PIN1_PORT AO_ADC_SENSE_B_PORT -#define AO_ADC_PIN1_PIN AO_ADC_SENSE_B_PIN -#define AO_ADC_PIN2_PORT AO_ADC_SENSE_C_PORT -#define AO_ADC_PIN2_PIN AO_ADC_SENSE_C_PIN -#define AO_ADC_PIN3_PORT AO_ADC_SENSE_D_PORT -#define AO_ADC_PIN3_PIN AO_ADC_SENSE_D_PIN -#define AO_ADC_PIN4_PORT AO_ADC_SENSE_E_PORT -#define AO_ADC_PIN4_PIN AO_ADC_SENSE_E_PIN -#define AO_ADC_PIN5_PORT AO_ADC_SENSE_F_PORT -#define AO_ADC_PIN5_PIN AO_ADC_SENSE_F_PIN -#define AO_ADC_PIN6_PORT AO_ADC_V_BATT_PORT -#define AO_ADC_PIN6_PIN AO_ADC_V_BATT_PIN -#define AO_ADC_PIN7_PORT AO_ADC_V_PBATT_PORT -#define AO_ADC_PIN7_PIN AO_ADC_V_PBATT_PIN -#define AO_ADC_PIN8_PORT AO_ADC_ACCEL_REF_PORT -#define AO_ADC_PIN8_PIN AO_ADC_ACCEL_REF_PIN -#define AO_ADC_PIN9_PORT AO_ADC_ACCEL_PORT -#define AO_ADC_PIN9_PIN AO_ADC_ACCEL_PIN - -#define AO_NUM_ADC (AO_ADC_NUM_SENSE + 5) - -#define AO_ADC_SQ1 AO_ADC_SENSE_A -#define AO_ADC_SQ2 AO_ADC_SENSE_B -#define AO_ADC_SQ3 AO_ADC_SENSE_C -#define AO_ADC_SQ4 AO_ADC_SENSE_D -#define AO_ADC_SQ5 AO_ADC_SENSE_E -#define AO_ADC_SQ6 AO_ADC_SENSE_F -#define AO_ADC_SQ7 AO_ADC_V_BATT -#define AO_ADC_SQ8 AO_ADC_V_PBATT -#define AO_ADC_SQ9 AO_ADC_ACCEL_REF -#define AO_ADC_SQ10 AO_ADC_ACCEL -#define AO_ADC_SQ11 AO_ADC_TEMP - -/* - * Pressure sensor settings - */ -#define HAS_MS5607 1 -#define HAS_MS5611 0 -#define AO_MS5607_PRIVATE_PINS 1 -#define AO_MS5607_CS_PORT (&stm_gpioc) -#define AO_MS5607_CS_PIN 4 -#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS) -#define AO_MS5607_MISO_PORT (&stm_gpioa) -#define AO_MS5607_MISO_PIN 6 -#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO) -#define AO_MS5607_SPI_INDEX AO_SPI_1_PA5_PA6_PA7 - -/* - * SPI Flash memory - */ - -#define M25_MAX_CHIPS 1 -#define AO_M25_SPI_CS_PORT (&stm_gpiod) -#define AO_M25_SPI_CS_MASK (1 << 3) -#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 - -/* - * Radio (cc1120) - */ - -/* gets pretty close to 434.550 */ - -#define AO_RADIO_CAL_DEFAULT 0x6ca333 - -#define AO_FEC_DEBUG 0 -#define AO_CC1120_SPI_CS_PORT (&stm_gpioc) -#define AO_CC1120_SPI_CS_PIN 5 -#define AO_CC1120_SPI_BUS AO_SPI_2_PB13_PB14_PB15 -#define AO_CC1120_SPI stm_spi2 - -#define AO_CC1120_INT_PORT (&stm_gpioc) -#define AO_CC1120_INT_PIN 14 -#define AO_CC1120_MCU_WAKEUP_PORT (&stm_gpioc) -#define AO_CC1120_MCU_WAKEUP_PIN (0) - -#define AO_CC1120_INT_GPIO 2 -#define AO_CC1120_INT_GPIO_IOCFG CC1120_IOCFG2 - -#define AO_CC1120_MARC_GPIO 3 -#define AO_CC1120_MARC_GPIO_IOCFG CC1120_IOCFG3 - - -#define HAS_BOOT_RADIO 0 - -/* - * Mag sensor (hmc5883) - */ - -#define HAS_HMC5883 0 -#define AO_HMC5883_INT_PORT (&stm_gpioc) -#define AO_HMC5883_INT_PIN 12 -#define AO_HMC5883_I2C_INDEX STM_I2C_INDEX(1) - -/* - * mpu6000 - */ - -#define HAS_MPU6000 1 -#define AO_MPU6000_INT_PORT (&stm_gpioc) -#define AO_MPU6000_INT_PIN 13 -#define AO_MPU6000_I2C_INDEX STM_I2C_INDEX(1) - -#define HAS_HIGHG_ACCEL 0 - -/* - * mma655x - */ - -#define HAS_MMA655X 1 -#define AO_MMA655X_SPI_INDEX AO_SPI_1_PE13_PE14_PE15 -#define AO_MMA655X_CS_PORT (&stm_gpiod) -#define AO_MMA655X_CS_PIN 4 - -#define NUM_CMDS 16 - -/* - * Companion - */ - -#define AO_COMPANION_CS_PORT (&stm_gpiod) -#define AO_COMPANION_CS_PIN (0) -#define AO_COMPANION_SPI_BUS AO_SPI_2_PB13_PB14_PB15 - -/* - * Monitor - */ - -#define HAS_MONITOR 0 -#define LEGACY_MONITOR 0 -#define HAS_MONITOR_PUT 1 -#define AO_MONITOR_LED 0 -#define HAS_RSSI 0 - -/* - * Profiling Viterbi decoding - */ - -#ifndef AO_PROFILE -#define AO_PROFILE 0 -#endif - -#endif /* _AO_PINS_H_ */ diff --git a/src/megametrum-v0.1/stlink-pins b/src/megametrum-v0.1/stlink-pins deleted file mode 100644 index 390f8e5d..00000000 --- a/src/megametrum-v0.1/stlink-pins +++ /dev/null @@ -1,57 +0,0 @@ -ST discovery card pins - -1 AIN-1 -2 JTCK -3 GND -4 JTMS -5 NRST -6 SWO - -MegaMetrum v0.1 misc connector - -1 GND -2 reset_n -3 boot0 -4 tx1 -5 rx1 -6 +3.3V -7 GND -8 jtms -9 jtck -10 jtdi -11 jtdo -12 jntrst -13 sda2 -14 scl2 -15 pe1 -16 pe0 - -For debugging: - - ST MM v0.1 -JTCK 2 9 -GND 3 7 -JTMS 4 8 -NRST 5 2 - -Altus Metrum STM32L standard debug connector (4 pin MicoMaTch): - - TL ST -GND 1 3 -NRST 2 5 -SWDIO 3 4 -SWCLK 4 2 - -Altus Metrum standard 4-pin connector to MegaMetrum v0.1 misc connector: - - AMstd MM v0.1 -gnd 1 1 -nrst 2 2 -swdio 3 8 -swclk 4 9 - -MegaAccel: - -Jumpers -PC0 (pin15) (blue) PE0 (pin97) accel_ref (debug 16) -PC1 (pin16) (green) PE1 (pin98) accel (debug 15) diff --git a/src/stm/ao_i2c_stm.c b/src/stm/ao_i2c_stm.c index 779e2275..809b5c6f 100644 --- a/src/stm/ao_i2c_stm.c +++ b/src/stm/ao_i2c_stm.c @@ -36,7 +36,7 @@ static uint16_t ao_i2c_addr[STM_NUM_I2C]; uint8_t ao_i2c_mutex[STM_NUM_I2C]; # define I2C_HIGH_SLOW 5000 /* ns, 100kHz clock */ -#ifdef MEGAMETRUM +#ifdef TELEMEGA # define I2C_HIGH_FAST 2000 /* ns, 167kHz clock */ #else # define I2C_HIGH_FAST 1000 /* ns, 333kHz clock */ diff --git a/src/telelco-v0.1/Makefile b/src/telelco-v0.1/Makefile index d2702dd6..a4a83d02 100644 --- a/src/telelco-v0.1/Makefile +++ b/src/telelco-v0.1/Makefile @@ -61,7 +61,7 @@ ALTOS_SRC = \ ao_radio_cmac_cmd.c PRODUCT=TeleLCO-v0.1 -PRODUCT_DEF=-DMEGAMETRUM +PRODUCT_DEF=-DTELEMEGA IDPRODUCT=0x0023 CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g diff --git a/src/test/Makefile b/src/test/Makefile index 991bdbfc..a62b59c5 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -30,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 $@ $< -lm + cc -DTELEMEGA=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 $@ $< diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index cdd1f236..99bed7ee 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -35,7 +35,7 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) -#if MEGAMETRUM +#if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 #define HAS_MPU6000 1 @@ -195,7 +195,7 @@ struct ao_cmds { #define ao_xmemcmp(d,s,c) memcmp(d,s,c) #define AO_NEED_ALTITUDE_TO_PRES 1 -#if MEGAMETRUM +#if TELEMEGA #include "ao_convert_pa.c" #include struct ao_ms5607_prom ms5607_prom; @@ -333,7 +333,7 @@ ao_insert(void) #else double accel = 0.0; #endif -#if MEGAMETRUM +#if TELEMEGA double height; ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); @@ -373,7 +373,7 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " -#if MEGAMETRUM +#if TELEMEGA "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 @@ -381,7 +381,7 @@ ao_insert(void) time, height, accel, -#if MEGAMETRUM +#if TELEMEGA ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, ao_mpu6000_gyro(ao_sample_angle) / 100.0, ao_sample_qangle, @@ -555,7 +555,7 @@ int32(uint8_t *bytes, int off) static int log_format; -#if MEGAMETRUM +#if TELEMEGA static double ao_vec_norm(double x, double y, double z) @@ -774,7 +774,7 @@ ao_sleep(void *wchan) for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { -#if MEGAMETRUM +#if TELEMEGA ao_data_static.mpu6000 = ao_ground_mpu6000; #else ao_data_static.adc.accel = ao_flight_ground_accel; @@ -800,8 +800,8 @@ ao_sleep(void *wchan) if (words[nword] == NULL) break; } -#if MEGAMETRUM - if (log_format == AO_LOG_FORMAT_MEGAMETRUM && nword == 30 && strlen(words[0]) == 1) { +#if TELEMEGA + if (log_format == AO_LOG_FORMAT_TELEMEGA && nword == 30 && strlen(words[0]) == 1) { int i; struct ao_ms5607_value value; @@ -885,7 +885,7 @@ ao_sleep(void *wchan) continue; } #else - if (nword == 4 && log_format != AO_LOG_FORMAT_MEGAMETRUM) { + if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { type = words[0][0]; tick = strtoul(words[1], NULL, 16); a = strtoul(words[2], NULL, 16); @@ -1002,7 +1002,7 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; -#if MEGAMETRUM +#if TELEMEGA (void) a; (void) b; #else diff --git a/telemetrum.inf b/telemetrum.inf index 91416bca..386dd286 100755 --- a/telemetrum.inf +++ b/telemetrum.inf @@ -30,7 +30,7 @@ DefaultDestDir = 12 %TeleScience% = AltusMetrum.Install, USB\VID_FFFE&PID_0011, AltusMetrumSerial %TelePyro% = AltusMetrum.Install, USB\VID_FFFE&PID_0012, AltusMetrumSerial %TeleShield% = AltusMetrum.Install, USB\VID_FFFE&PID_0013, AltusMetrumSerial -%MegaMetrum% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial +%TeleMega% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial %MegaDongle = AltusMetrum.Install, USB\VID_FFFE&PID_0024, AltusMetrumSerial %TeleGPS% = AltusMetrum.Install, USB\VID_FFFE&PID_0025, AltusMetrumSerial %AltusMetrum26% = AltusMetrum.Install, USB\VID_FFFE&PID_0026, AltusMetrumSerial @@ -52,7 +52,7 @@ DefaultDestDir = 12 %TeleScience% = AltusMetrum.Install, USB\VID_FFFE&PID_0011, AltusMetrumSerial %TelePyro% = AltusMetrum.Install, USB\VID_FFFE&PID_0012, AltusMetrumSerial %TeleShield% = AltusMetrum.Install, USB\VID_FFFE&PID_0013, AltusMetrumSerial -%MegaMetrum% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial +%TeleMega% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial %MegaDongle = AltusMetrum.Install, USB\VID_FFFE&PID_0024, AltusMetrumSerial %TeleGPS% = AltusMetrum.Install, USB\VID_FFFE&PID_0025, AltusMetrumSerial %AltusMetrum26% = AltusMetrum.Install, USB\VID_FFFE&PID_0026, AltusMetrumSerial @@ -74,7 +74,7 @@ DefaultDestDir = 12 %TeleScience% = AltusMetrum.Install, USB\VID_FFFE&PID_0011, AltusMetrumSerial %TelePyro% = AltusMetrum.Install, USB\VID_FFFE&PID_0012, AltusMetrumSerial %TeleShield% = AltusMetrum.Install, USB\VID_FFFE&PID_0013, AltusMetrumSerial -%MegaMetrum% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial +%TeleMega% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial %MegaDongle = AltusMetrum.Install, USB\VID_FFFE&PID_0024, AltusMetrumSerial %TeleGPS% = AltusMetrum.Install, USB\VID_FFFE&PID_0025, AltusMetrumSerial %AltusMetrum26% = AltusMetrum.Install, USB\VID_FFFE&PID_0026, AltusMetrumSerial @@ -96,7 +96,7 @@ DefaultDestDir = 12 %TeleScience% = AltusMetrum.Install, USB\VID_FFFE&PID_0011, AltusMetrumSerial %TelePyro% = AltusMetrum.Install, USB\VID_FFFE&PID_0012, AltusMetrumSerial %TeleShield% = AltusMetrum.Install, USB\VID_FFFE&PID_0013, AltusMetrumSerial -%MegaMetrum% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial +%TeleMega% = AltusMetrum.Install, USB\VID_FFFE&PID_0023, AltusMetrumSerial %MegaDongle = AltusMetrum.Install, USB\VID_FFFE&PID_0024, AltusMetrumSerial %TeleGPS% = AltusMetrum.Install, USB\VID_FFFE&PID_0025, AltusMetrumSerial %AltusMetrum26% = AltusMetrum.Install, USB\VID_FFFE&PID_0026, AltusMetrumSerial -- cgit v1.2.3 From 0488cd9cffc837e99490a0761216bbc5847ff400 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 28 Apr 2013 22:52:23 -0700 Subject: altos: Build test framework for AES code Simple CBC-CMAC test with a constant 0 key and constant 0 data for now. Signed-off-by: Keith Packard --- src/aes/ao_aes.c | 9 ++++++++- src/aes/ao_aes_int.h | 2 ++ src/test/Makefile | 7 +++++-- src/test/ao_aes_test.c | 52 ++++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 67 insertions(+), 3 deletions(-) create mode 100644 src/test/ao_aes_test.c (limited to 'src/test/Makefile') diff --git a/src/aes/ao_aes.c b/src/aes/ao_aes.c index 4977aaf8..71ffadb1 100644 --- a/src/aes/ao_aes.c +++ b/src/aes/ao_aes.c @@ -11,7 +11,9 @@ * Vincent Rijmen */ +#ifndef AO_AES_TEST #include +#endif #include #include "ao_aes_int.h" @@ -193,6 +195,7 @@ static inline void xAddInvMix(word32 res[MAXBC], word32 a[MAXBC], #endif /* code included for reference */ +static int xrijndaelKeySched(word32 key[], int keyBits, int blockBits, roundkey *rkk) { @@ -280,6 +283,7 @@ int xrijndaelKeySched(word32 key[], int keyBits, int blockBits, /* Encryption of one block. */ +static void xrijndaelEncrypt(word32 block[], roundkey *rkk) { word32 block2[MAXBC]; /* hold intermediate result */ @@ -306,6 +310,7 @@ void xrijndaelEncrypt(word32 block[], roundkey *rkk) xKeyAddition(block, block2, rp, BC); } +static void xrijndaelDecrypt(word32 block[], roundkey *rkk) { word32 block2[MAXBC]; /* hold intermediate result */ @@ -349,6 +354,7 @@ void xrijndaelDecrypt(word32 block[], roundkey *rkk) } uint8_t ao_aes_mutex; +static uint8_t key[16]; static roundkey rkk; static uint8_t iv[16]; @@ -362,7 +368,8 @@ ao_aes_set_mode(enum ao_aes_mode mode) void ao_aes_set_key(__xdata uint8_t *in) { - xrijndaelKeySched((word32 *) in, 128, 128, &rkk); + memcpy(key, in, 16); + xrijndaelKeySched((word32 *) key, 128, 128, &rkk); } void diff --git a/src/aes/ao_aes_int.h b/src/aes/ao_aes_int.h index 7990a2e1..4d9b3ba7 100644 --- a/src/aes/ao_aes_int.h +++ b/src/aes/ao_aes_int.h @@ -41,6 +41,7 @@ typedef struct { word32 rk[MAXRK]; } roundkey; +#if 0 /* keys and blocks are externally treated as word32 arrays, to make sure they are aligned on 4-byte boundaries on architectures that require it. */ @@ -60,5 +61,6 @@ int xrijndaelKeySched(word32 key[], int keyBits, int blockBits, void xrijndaelEncrypt(word32 block[], roundkey *rkk); void xrijndaelDecrypt(word32 block[], roundkey *rkk); +#endif #endif /* __RIJNDAEL_H */ diff --git a/src/test/Makefile b/src/test/Makefile index a62b59c5..d4d98e54 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,8 +1,8 @@ -vpath % ..:../core:../drivers:../util:../micropeak +vpath % ..:../core:../drivers:../util:../micropeak:../aes 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_aprs_test ao_micropeak_test ao_fat_test + ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -67,3 +67,6 @@ ao_micropeak_test: ao_micropeak_test.c ao_microflight.c ao_kalman.h ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c cc $(CFLAGS) -o $@ ao_fat_test.c -lssl + +ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c + cc $(CFLAGS) -o $@ ao_aes_test.c diff --git a/src/test/ao_aes_test.c b/src/test/ao_aes_test.c new file mode 100644 index 00000000..dcedbfca --- /dev/null +++ b/src/test/ao_aes_test.c @@ -0,0 +1,52 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#define __pdata +#define __data +#define __xdata +#define __code +#define __reentrant + +#include +#include + +#define AO_AES_TEST 1 + +#include "../aes/ao_aes_tables.c" +#include "../aes/ao_aes.c" + +static uint8_t key[16]; +static uint8_t text[16]; +static uint8_t cbc[16]; + +int +main (int argc, char **argv) +{ + int i; + + ao_aes_init(); + ao_aes_set_mode(ao_aes_mode_cbc_mac); + ao_aes_set_key(key); + ao_aes_zero_iv(); + ao_aes_run(text, cbc); + + printf ("CBC"); + for (i = 0; i < sizeof (cbc); i++) + printf (" %02x", cbc[i]); + printf ("\n"); + return 0; +} -- cgit v1.2.3 From 50457f9983ec0a432f1050464382749436e3da94 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 13 May 2013 22:31:31 -0700 Subject: altos: Add U-Blox GPS driver Uses binary mode. Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 626 +++++++++++++++++++++++++++++++++++++++++++ src/drivers/ao_gps_ublox.h | 241 +++++++++++++++++ src/test/Makefile | 5 +- src/test/ao_gps_test_ublox.c | 409 ++++++++++++++++++++++++++++ 4 files changed, 1280 insertions(+), 1 deletion(-) create mode 100644 src/drivers/ao_gps_ublox.c create mode 100644 src/drivers/ao_gps_ublox.h create mode 100644 src/test/ao_gps_test_ublox.c (limited to 'src/test/Makefile') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c new file mode 100644 index 00000000..32405ea5 --- /dev/null +++ b/src/drivers/ao_gps_ublox.c @@ -0,0 +1,626 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef AO_GPS_TEST +#include "ao.h" +#endif + +#include "ao_gps_ublox.h" + +__xdata uint8_t ao_gps_mutex; +__pdata uint16_t ao_gps_tick; +__xdata struct ao_telemetry_location ao_gps_data; +__xdata struct ao_telemetry_satellite ao_gps_tracking_data; + +static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n"; + +const char ao_gps_config[] = { + +}; + +struct ao_ublox_cksum { + uint8_t a, b; +}; + +static __pdata struct ao_ublox_cksum ao_ublox_cksum; +static __pdata uint16_t ao_ublox_len; + +#ifndef ao_ublox_getchar +#define ao_ublox_getchar ao_serial1_getchar +#define ao_ublox_putchar ao_serial1_putchar +#define ao_ublox_set_speed ao_serial1_set_speed +#endif + +#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar()) + +static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c) +{ + cksum->a += c; + cksum->b += cksum->a; +} + +static void ao_ublox_init_cksum(void) +{ + ao_ublox_cksum.a = ao_ublox_cksum.b = 0; +} + +static void ao_ublox_putchar_cksum(uint8_t c) +{ + add_cksum(&ao_ublox_cksum, c); + ao_ublox_putchar(c); +} + +static uint8_t header_byte(void) +{ + uint8_t c = ao_ublox_byte(); + add_cksum(&ao_ublox_cksum, c); + return c; +} + +static uint8_t data_byte(void) +{ + --ao_ublox_len; + return header_byte(); +} + +static char __xdata *ublox_target; + +static void ublox_u16(uint8_t offset) +{ + uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset); + uint16_t val; + + val = data_byte(); + val |= data_byte () << 8; + *ptr = val; +} + +static void ublox_u8(uint8_t offset) +{ + uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset); + uint8_t val; + + val = data_byte (); + *ptr = val; +} + +static void ublox_u32(uint8_t offset) __reentrant +{ + uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset); + uint32_t val; + + val = ((uint32_t) data_byte ()); + val |= ((uint32_t) data_byte ()) << 8; + val |= ((uint32_t) data_byte ()) << 16; + val |= ((uint32_t) data_byte ()) << 24; + *ptr = val; +} + +static void ublox_discard(uint8_t len) +{ + while (len--) + data_byte(); +} + +#define UBLOX_END 0 +#define UBLOX_DISCARD 1 +#define UBLOX_U8 2 +#define UBLOX_U16 3 +#define UBLOX_U32 4 + +struct ublox_packet_parse { + uint8_t type; + uint8_t offset; +}; + +static void +ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant +{ + uint8_t i, offset; + + ublox_target = target; + for (i = 0; ; i++) { + offset = parse[i].offset; + switch (parse[i].type) { + case UBLOX_END: + return; + case UBLOX_DISCARD: + ublox_discard(offset); + break; + case UBLOX_U8: + ublox_u8(offset); + break; + case UBLOX_U16: + ublox_u16(offset); + break; + case UBLOX_U32: + ublox_u32(offset); + break; + } + } +} + +/* + * NAV-DOP message parsing + */ + +static struct nav_dop { + uint16_t pdop; + uint16_t hdop; + uint16_t vdop; +} nav_dop; + +static const struct ublox_packet_parse nav_dop_packet[] = { + { UBLOX_DISCARD, 6 }, /* 0 GPS Millisecond Time of Week, gDOP */ + { UBLOX_U16, offsetof(struct nav_dop, pdop) }, /* 6 pDOP */ + { UBLOX_DISCARD, 2 }, /* 8 tDOP */ + { UBLOX_U16, offsetof(struct nav_dop, vdop) }, /* 10 vDOP */ + { UBLOX_U16, offsetof(struct nav_dop, hdop) }, /* 12 hDOP */ + { UBLOX_DISCARD, 4 }, /* 14 nDOP, eDOP */ + { UBLOX_END, 0 } +}; + +static void +ao_ublox_parse_nav_dop(void) +{ + ao_ublox_parse(&nav_dop, nav_dop_packet); +} + +/* + * NAV-POSLLH message parsing + */ +static struct nav_posllh { + int32_t lat; + int32_t lon; + int32_t alt_msl; +} nav_posllh; + +static const struct ublox_packet_parse nav_posllh_packet[] = { + { UBLOX_DISCARD, 4 }, /* 0 GPS Millisecond Time of Week */ + { UBLOX_U32, offsetof(struct nav_posllh, lon) }, /* 4 Longitude */ + { UBLOX_U32, offsetof(struct nav_posllh, lat) }, /* 8 Latitude */ + { UBLOX_DISCARD, 4 }, /* 12 Height above Ellipsoid */ + { UBLOX_U32, offsetof(struct nav_posllh, alt_msl) }, /* 16 Height above mean sea level */ + { UBLOX_DISCARD, 8 }, /* 20 hAcc, vAcc */ + { UBLOX_END, 0 }, /* 28 */ +}; + +static void +ao_ublox_parse_nav_posllh(void) +{ + ao_ublox_parse(&nav_posllh, nav_posllh_packet); +} + +/* + * NAV-SOL message parsing + */ +static struct nav_sol { + uint8_t gps_fix; + uint8_t flags; + uint8_t nsat; +} nav_sol; + +static const struct ublox_packet_parse nav_sol_packet[] = { + { UBLOX_DISCARD, 10 }, /* 0 iTOW, fTOW, week */ + { UBLOX_U8, offsetof(struct nav_sol, gps_fix) }, /* 10 gpsFix */ + { UBLOX_U8, offsetof(struct nav_sol, flags) }, /* 11 flags */ + { UBLOX_DISCARD, 35 }, /* 12 ecefX, ecefY, ecefZ, pAcc, ecefVX, ecefVY, ecefVZ, sAcc, pDOP, reserved1 */ + { UBLOX_U8, offsetof(struct nav_sol, nsat) }, /* 47 numSV */ + { UBLOX_DISCARD, 4 }, /* 48 reserved2 */ + { UBLOX_END, 0 } +}; + +#define NAV_SOL_FLAGS_GPSFIXOK 0 +#define NAV_SOL_FLAGS_DIFFSOLN 1 +#define NAV_SOL_FLAGS_WKNSET 2 +#define NAV_SOL_FLAGS_TOWSET 3 + +static void +ao_ublox_parse_nav_sol(void) +{ + ao_ublox_parse(&nav_sol, nav_sol_packet); +} + +/* + * NAV-SVINFO message parsing + */ + +static struct nav_svinfo { + uint8_t num_ch; + uint8_t flags; +} nav_svinfo; + +static const struct ublox_packet_parse nav_svinfo_packet[] = { + { UBLOX_DISCARD, 4 }, /* 0 iTOW */ + { UBLOX_U8, offsetof(struct nav_svinfo, num_ch) }, /* 4 numCh */ + { UBLOX_U8, offsetof(struct nav_svinfo, flags) }, /* 5 globalFlags */ + { UBLOX_DISCARD, 2 }, /* 6 reserved2 */ + { UBLOX_END, 0 } +}; + +#define NAV_SVINFO_MAX_SAT 16 + +static struct nav_svinfo_sat { + uint8_t chn; + uint8_t svid; + uint8_t flags; + uint8_t quality; + uint8_t cno; +} nav_svinfo_sat[NAV_SVINFO_MAX_SAT]; + +static uint8_t nav_svinfo_nsat; + +static const struct ublox_packet_parse nav_svinfo_sat_packet[] = { + { UBLOX_U8, offsetof(struct nav_svinfo_sat, chn) }, /* 8 + 12*N chn */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, svid) }, /* 9 + 12*N svid */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, flags) }, /* 10 + 12*N flags */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, quality) }, /* 11 + 12*N quality */ + { UBLOX_U8, offsetof(struct nav_svinfo_sat, cno) }, /* 12 + 12*N cno */ + { UBLOX_DISCARD, 7 }, /* 13 + 12*N elev, azim, prRes */ + { UBLOX_END, 0 } +}; + +#define NAV_SVINFO_SAT_FLAGS_SVUSED 0 +#define NAV_SVINFO_SAT_FLAGS_DIFFCORR 1 +#define NAV_SVINFO_SAT_FLAGS_ORBITAVAIL 2 +#define NAV_SVINFO_SAT_FLAGS_ORBITEPH 3 +#define NAV_SVINFO_SAT_FLAGS_UNHEALTHY 4 +#define NAV_SVINFO_SAT_FLAGS_ORBITALM 5 +#define NAV_SVINFO_SAT_FLAGS_ORBITAOP 6 +#define NAV_SVINFO_SAT_FLAGS_SMOOTHED 7 + +#define NAV_SVINFO_SAT_QUALITY_IDLE 0 +#define NAV_SVINFO_SAT_QUALITY_SEARCHING 1 +#define NAV_SVINFO_SAT_QUALITY_ACQUIRED 2 +#define NAV_SVINFO_SAT_QUALITY_UNUSABLE 3 +#define NAV_SVINFO_SAT_QUALITY_LOCKED 4 +#define NAV_SVINFO_SAT_QUALITY_RUNNING 5 + +static void +ao_ublox_parse_nav_svinfo(void) +{ + uint8_t nsat; + nav_svinfo_nsat = 0; + ao_ublox_parse(&nav_svinfo, nav_svinfo_packet); + for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) { + if (nsat < NAV_SVINFO_MAX_SAT) { + ao_ublox_parse(&nav_svinfo_sat[nav_svinfo_nsat++], nav_svinfo_sat_packet); + } else { + ublox_discard(12); + } + } +} + +/* + * NAV-TIMEUTC message parsing + */ +static struct nav_timeutc { + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; +} nav_timeutc; + +#define NAV_TIMEUTC_VALID_TOW 0 +#define NAV_TIMEUTC_VALID_WKN 1 +#define NAV_TIMEUTC_VALID_UTC 2 + +static const struct ublox_packet_parse nav_timeutc_packet[] = { + { UBLOX_DISCARD, 12 }, /* 0 iTOW, tAcc, nano */ + { UBLOX_U16, offsetof(struct nav_timeutc, year) }, /* 12 year */ + { UBLOX_U8, offsetof(struct nav_timeutc, month) }, /* 14 month */ + { UBLOX_U8, offsetof(struct nav_timeutc, day) }, /* 15 day */ + { UBLOX_U8, offsetof(struct nav_timeutc, hour) }, /* 16 hour */ + { UBLOX_U8, offsetof(struct nav_timeutc, min) }, /* 17 min */ + { UBLOX_U8, offsetof(struct nav_timeutc, sec) }, /* 18 sec */ + { UBLOX_U8, offsetof(struct nav_timeutc, valid) }, /* 19 valid */ + { UBLOX_END, 0 } +}; + +static void +ao_ublox_parse_nav_timeutc(void) +{ + ao_ublox_parse(&nav_timeutc, nav_timeutc_packet); +} + +/* + * NAV-VELNED message parsing + */ + +static struct nav_velned { + int32_t vel_d; + uint32_t g_speed; + int32_t heading; +} nav_velned; + +static const struct ublox_packet_parse nav_velned_packet[] = { + { UBLOX_DISCARD, 12 }, /* 0 iTOW, velN, velE */ + { UBLOX_U32, offsetof(struct nav_velned, vel_d) }, /* 12 velD */ + { UBLOX_DISCARD, 4 }, /* 16 speed */ + { UBLOX_U32, offsetof(struct nav_velned, g_speed) }, /* 20 gSpeed */ + { UBLOX_U32, offsetof(struct nav_velned, heading) }, /* 24 heading */ + { UBLOX_DISCARD, 8 }, /* 28 sAcc, cAcc */ + { UBLOX_END, 0 } +}; + +static void +ao_ublox_parse_nav_velned(void) +{ + ao_ublox_parse(&nav_velned, nav_velned_packet); +} + +/* + * Set the protocol mode and baud rate + */ + +static void +ao_gps_setup(void) +{ + uint8_t i, k; + ao_ublox_set_speed(AO_SERIAL_SPEED_9600); + + /* + * A bunch of nulls so the start bit + * is clear + */ + for (i = 0; i < 64; i++) + ao_ublox_putchar(0x00); + + /* + * Send the baud-rate setting and protocol-setting + * command three times + */ + for (k = 0; k < 3; k++) + for (i = 0; i < sizeof (ao_gps_set_nmea); i++) + ao_ublox_putchar(ao_gps_set_nmea[i]); + + /* + * Increase the baud rate + */ + ao_ublox_set_speed(AO_SERIAL_SPEED_57600); + + /* + * Pad with nulls to give the chip + * time to see the baud rate switch + */ + for (i = 0; i < 64; i++) + ao_ublox_putchar(0x00); +} + +void +ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len) +{ + ao_ublox_init_cksum(); + ao_ublox_putchar(0xb5); + ao_ublox_putchar(0x62); + ao_ublox_putchar_cksum(class); + ao_ublox_putchar_cksum(id); + ao_ublox_putchar_cksum(len); + ao_ublox_putchar_cksum(len >> 8); +} + +void +ao_ublox_putend(void) +{ + ao_ublox_putchar(ao_ublox_cksum.a); + ao_ublox_putchar(ao_ublox_cksum.b); +} + +void +ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate) +{ + ao_ublox_putstart(0x06, 0x01, 3); + ao_ublox_putchar_cksum(class); + ao_ublox_putchar_cksum(msgid); + ao_ublox_putchar_cksum(rate); + ao_ublox_putend(); +} + +/* + * Disable all MON message + */ +static const uint8_t ublox_disable_mon[] = { + 0x0b, 0x09, 0x02, 0x06, 0x07, 0x21, 0x08, 0x04 +}; + +/* + * Disable all NAV messages. The desired + * ones will be explicitly re-enabled + */ + +static const uint8_t ublox_disable_nav[] = { + 0x60, 0x22, 0x31, 0x04, 0x40, 0x01, 0x02, 0x32, + 0x06, 0x03, 0x30, 0x20, 0x21, 0x11, 0x12 +}; + +void +ao_gps(void) __reentrant +{ + uint8_t class, id; + struct ao_ublox_cksum cksum; + uint8_t i; + + ao_gps_setup(); + + for (i = 0; i < sizeof (ublox_disable_mon); i++) + ao_ublox_set_message_rate(0x0a, ublox_disable_mon[i], 0); + for (i = 0; i < sizeof (ublox_disable_nav); i++) + ao_ublox_set_message_rate(0x01, ublox_disable_nav[i], 0); + + /* Enable all of the messages we want */ + + /* DOP */ + ao_ublox_set_message_rate(0x01, 0x04, 1); + + /* POSLLH */ + ao_ublox_set_message_rate(0x01, 0x02, 1); + + /* SOL */ + ao_ublox_set_message_rate(0x01, 0x06, 1); + + /* SVINFO */ + ao_ublox_set_message_rate(0x01, 0x30, 1); + + /* VELNED */ + ao_ublox_set_message_rate(0x01, 0x12, 1); + + /* TIMEUTC */ + ao_ublox_set_message_rate(0x01, 0x21, 1); + + for (;;) { + /* Locate the begining of the next record */ + while (ao_ublox_byte() != (uint8_t) 0xb5) + ; + if (ao_ublox_byte() != (uint8_t) 0x62) + continue; + + ao_ublox_init_cksum(); + + class = header_byte(); + id = header_byte(); + + /* Length */ + ao_ublox_len = header_byte(); + ao_ublox_len |= header_byte() << 8; + + if (ao_ublox_len > 1023) + continue; + + switch (class) { + case UBLOX_NAV: + switch (id) { + case UBLOX_NAV_DOP: + if (ao_ublox_len != 18) + break; + ao_ublox_parse_nav_dop(); + break; + case UBLOX_NAV_POSLLH: + if (ao_ublox_len != 28) + break; + ao_ublox_parse_nav_posllh(); + break; + case UBLOX_NAV_SOL: + if (ao_ublox_len != 52) + break; + ao_ublox_parse_nav_sol(); + break; + case UBLOX_NAV_SVINFO: + if (ao_ublox_len < 8) + break; + ao_ublox_parse_nav_svinfo(); + break; + case UBLOX_NAV_VELNED: + if (ao_ublox_len != 36) + break; + ao_ublox_parse_nav_velned(); + break; + case UBLOX_NAV_TIMEUTC: + if (ao_ublox_len != 20) + break; + ao_ublox_parse_nav_timeutc(); + break; + } + break; + } + + if (ao_ublox_len != 0) + continue; + + /* verify checksum and end sequence */ + cksum.a = ao_ublox_byte(); + cksum.b = ao_ublox_byte(); + if (ao_ublox_cksum.a != cksum.a || ao_ublox_cksum.b != cksum.b) + continue; + + switch (class) { + case 0x01: + switch (id) { + case 0x21: + ao_mutex_get(&ao_gps_mutex); + ao_gps_tick = ao_time(); + + ao_gps_data.flags = 0; + ao_gps_data.flags |= AO_GPS_RUNNING; + if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) { + uint8_t nsat = nav_sol.nsat; + ao_gps_data.flags |= AO_GPS_VALID; + if (nsat > 15) + nsat = 15; + ao_gps_data.flags |= nsat; + } + if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC)) + ao_gps_data.flags |= AO_GPS_DATE_VALID; + + ao_gps_data.altitude = nav_posllh.alt_msl / 1000; + ao_gps_data.latitude = nav_posllh.lat; + ao_gps_data.longitude = nav_posllh.lon; + + ao_gps_data.year = nav_timeutc.year - 2000; + ao_gps_data.month = nav_timeutc.month; + ao_gps_data.day = nav_timeutc.day; + + ao_gps_data.hour = nav_timeutc.hour; + ao_gps_data.minute = nav_timeutc.min; + ao_gps_data.second = nav_timeutc.sec; + + ao_gps_data.pdop = nav_dop.pdop; + ao_gps_data.hdop = nav_dop.hdop; + ao_gps_data.vdop = nav_dop.vdop; + + /* mode is not set */ + + ao_gps_data.ground_speed = nav_velned.g_speed; + ao_gps_data.climb_rate = -nav_velned.vel_d; + ao_gps_data.course = nav_velned.heading / 200000; + + ao_gps_tracking_data.channels = 0; + + struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0]; + + for (i = 0; i < nav_svinfo_nsat; i++) { + struct nav_svinfo_sat *src = &nav_svinfo_sat[i]; + + if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) && + src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED) + { + dst->svid = src->svid; + dst->c_n_1 = src->cno; + dst++; + ao_gps_tracking_data.channels++; + } + } + + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_tracking_data); + break; + } + break; + } + } +} + +__xdata struct ao_task ao_gps_task; + +void +ao_gps_init(void) +{ + ao_add_task(&ao_gps_task, ao_gps, "gps"); +} diff --git a/src/drivers/ao_gps_ublox.h b/src/drivers/ao_gps_ublox.h new file mode 100644 index 00000000..13bf6955 --- /dev/null +++ b/src/drivers/ao_gps_ublox.h @@ -0,0 +1,241 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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. + */ + +#ifndef _AO_GPS_UBLOX_H_ +#define _AO_GPS_UBLOX_H_ + +struct ublox_hdr { + uint8_t class, message; + uint16_t length; +}; + +#define UBLOX_NAV 0x01 + +#define UBLOX_NAV_DOP 0x04 + +struct ublox_nav_dop { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x04 */ + uint16_t length; /* 18 */ + + uint32_t itow; /* ms */ + uint16_t gdop; + uint16_t ddop; + uint16_t tdop; + uint16_t vdop; + uint16_t hdop; + uint16_t ndop; + uint16_t edop; +}; + +#define UBLOX_NAV_POSLLH 0x02 + +struct ublox_nav_posllh { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x02 */ + uint16_t length; /* 28 */ + + uint32_t itow; /* ms */ + int32_t lat; /* deg * 1e7 */ + int32_t lon; /* deg * 1e7 */ + int32_t height; /* mm */ + int32_t hmsl; /* mm */ + uint32_t hacc; /* mm */ + uint32_t vacc; /* mm */ +}; + +#define UBLOX_NAV_SOL 0x06 + +struct ublox_nav_sol { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x06 */ + uint16_t length; /* 52 */ + + uint32_t itow; /* ms */ + int32_t ftow; /* ns */ + int16_t week; + int8_t gpsfix; + uint8_t flags; + int32_t exefx; /* cm */ + int32_t exefy; /* cm */ + int32_t exefz; /* cm */ + uint32_t pacc; /* cm */ + int32_t exefvx; /* cm/s */ + int32_t exefvy; /* cm/s */ + int32_t exefvz; /* cm/s */ + uint32_t sacc; /* cm/s */ + uint16_t pdop; /* * 100 */ + uint8_t reserved1; + uint8_t numsv; + uint32_t reserved2; +}; + +#define UBLOX_NAV_SOL_GPSFIX_NO_FIX 0 +#define UBLOX_NAV_SOL_GPSFIX_DEAD_RECKONING 1 +#define UBLOX_NAV_SOL_GPSFIX_2D 2 +#define UBLOX_NAV_SOL_GPSFIX_3D 3 +#define UBLOX_NAV_SOL_GPSFIX_GPS_DEAD_RECKONING 4 +#define UBLOX_NAV_SOL_GPSFIX_TIME_ONLY 5 + +#define UBLOX_NAV_SOL_FLAGS_GPSFIXOK 0 +#define UBLOX_NAV_SOL_FLAGS_DIFFSOLN 1 +#define UBLOX_NAV_SOL_FLAGS_WKNSET 2 +#define UBLOX_NAV_SOL_FLAGS_TOWSET 3 + +#define UBLOX_NAV_STATUS 0x03 + +struct ublox_nav_status { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x03 */ + uint16_t length; /* 16 */ + + uint8_t gpsfix; + uint8_t flags; + uint8_t fixstat; + uint8_t flags2; + + uint32_t ttff; /* ms */ + uint32_t msss; /* ms */ +}; + +#define UBLOX_NAV_STATUS_GPSFIX_NO_FIX 0 +#define UBLOX_NAV_STATUS_GPSFIX_DEAD_RECKONING 1 +#define UBLOX_NAV_STATUS_GPSFIX_2D 2 +#define UBLOX_NAV_STATUS_GPSFIX_3D 3 +#define UBLOX_NAV_STATUS_GPSFIX_GPS_DEAD_RECKONING 4 +#define UBLOX_NAV_STATUS_GPSFIX_TIME_ONLY 5 + +#define UBLOX_NAV_STATUS_FLAGS_GPSFIXOK 0 +#define UBLOX_NAV_STATUS_FLAGS_DIFFSOLN 1 +#define UBLOX_NAV_STATUS_FLAGS_WKNSET 2 +#define UBLOX_NAV_STATUS_FLAGS_TOWSET 3 + +#define UBLOX_NAV_STATUS_FIXSTAT_DGPSISTAT 0 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING 6 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_NONE 0 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_VALID 1 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_USED 2 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_DR 3 +#define UBLOX_NAV_STATUS_FIXSTAT_MAPMATCHING_MASK 3 + +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE 0 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_ACQUISITION 0 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_TRACKING 1 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_POWER_OPTIMIZED_TRACKING 2 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_INACTIVE 3 +#define UBLOX_NAV_STATUS_FLAGS2_PSMSTATE_MASK 3 + +#define UBLOX_NAV_SVINFO 0x30 + +struct ublox_nav_svinfo { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x30 */ + uint16_t length; /* 8 + 12 * numch */ + + uint32_t itow; /* ms */ + + uint8_t numch; + uint8_t globalflags; + uint16_t reserved; +}; + +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN 0 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_ANTARIS 0 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_U_BLOX_5 1 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_U_BLOX_6 2 +#define UBLOX_NAV_SVINFO_GLOBAL_FLAGS_CHIPGEN_MASK 7 + +struct ublox_nav_svinfo_block { + uint8_t chn; + uint8_t svid; + uint8_t flags; + uint8_t quality; + + uint8_t cno; /* dbHz */ + int8_t elev; /* deg */ + int16_t azim; /* deg */ + + int32_t prres; /* cm */ +}; + +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_SVUSED 0 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_DIFFCORR 1 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITAVAIL 2 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITEPH 3 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_UNHEALTHY 4 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITALM 5 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_ORBITAOP 6 +#define UBLOX_NAV_SVINFO_BLOCK_FLAGS_SMOOTHED 7 + +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND 0 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_IDLE 0 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_SEARCHING 1 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_ACQUIRED 2 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_UNUSABLE 3 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CODE_LOCK 4 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CARRIER_LOCKED_5 5 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CARRIER_LOCKED_6 6 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_CARRIER_LOCKED_7 7 +#define UBLOX_NAV_SVINFO_BLOCK_QUALITY_QUALITYIND_MASK 7 + +#define UBLOX_NAV_TIMEUTC 0x21 + +struct ublox_nav_timeutc { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x21 */ + uint16_t length; /* 20 */ + + uint32_t itow; /* ms */ + uint32_t tacc; /* ns */ + int32_t nano; /* ns */ + + uint16_t year; + uint8_t month; + uint8_t day; + + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; +}; + +#define UBLOX_NAV_TIMEUTC_VALID_VALIDTOW 0 +#define UBLOX_NAV_TIMEUTC_VALID_VALIDWKN 1 +#define UBLOX_NAV_TIMEUTC_VALID_VALIDUTC 2 + +#define UBLOX_NAV_VELNED 0x12 + +struct ublox_nav_velned { + uint8_t class; /* 0x01 */ + uint8_t message; /* 0x12 */ + uint16_t length; /* 36 */ + + uint32_t itow; /* ms */ + + int32_t veln; /* cm/s */ + int32_t vele; /* cm/s */ + int32_t veld; /* cm/s */ + + uint32_t speed; /* cm/s */ + uint32_t gspeed; /* cm/s */ + + int32_t heading; /* deg */ + uint32_t sacc; /* cm/s */ + uint32_t cacc; /* deg */ +}; + +#endif /* _AO_GPS_UBLOX_H_ */ diff --git a/src/test/Makefile b/src/test/Makefile index d4d98e54..08aa8cd5 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,7 +1,7 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes 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_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -38,6 +38,9 @@ ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< +ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h + cc $(CFLAGS) -o $@ $< + ao_convert_test: ao_convert_test.c ao_convert.c altitude.h cc $(CFLAGS) -o $@ $< diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c new file mode 100644 index 00000000..80671735 --- /dev/null +++ b/src/test/ao_gps_test_ublox.c @@ -0,0 +1,409 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#define AO_GPS_TEST +#include "ao_host.h" +#include +#include +#include +#include +#include +#include +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#define AO_GPS_NUM_SAT_SHIFT (0) + +#define AO_GPS_VALID (1 << 4) +#define AO_GPS_RUNNING (1 << 5) +#define AO_GPS_DATE_VALID (1 << 6) +#define AO_GPS_COURSE_VALID (1 << 7) + +struct ao_telemetry_location { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + int32_t latitude; /* degrees * 10⁷ */ + int32_t longitude; /* degrees * 10⁷ */ + int16_t altitude; /* m */ + uint16_t ground_speed; /* cm/s */ + uint8_t course; /* degrees / 2 */ + uint8_t pdop; /* * 5 */ + uint8_t hdop; /* * 5 */ + uint8_t vdop; /* * 5 */ + int16_t climb_rate; /* cm/s */ + uint16_t h_error; /* m */ + uint16_t v_error; /* m */ +}; + +#define UBLOX_SAT_STATE_ACQUIRED (1 << 0) +#define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define UBLOX_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define UBLOX_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define UBLOX_SAT_CODE_LOCKED (1 << 5) +#define UBLOX_SAT_ACQUISITION_FAILED (1 << 6) +#define UBLOX_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct ao_telemetry_satellite_info { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 + +struct ao_telemetry_satellite { + uint8_t channels; + struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; +}; + +#define ao_gps_orig ao_telemetry_location +#define ao_gps_tracking_orig ao_telemetry_satellite +#define ao_gps_sat_orig ao_telemetry_satellite_info + +void +ao_mutex_get(uint8_t *mutex) +{ +} + +void +ao_mutex_put(uint8_t *mutex) +{ +} + +static int ao_gps_fd; +static FILE *ao_gps_file; + +#if 0 +static void +ao_dbg_char(char c) +{ + char line[128]; + line[0] = '\0'; + if (c < ' ') { + if (c == '\n') + sprintf (line, "\n"); + else + sprintf (line, "\\%02x", ((int) c) & 0xff); + } else { + sprintf (line, "%c", c); + } + write(1, line, strlen(line)); +} +#endif + +#include + +int +get_millis(void) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec * 1000 + tv.tv_usec / 1000; +} + +static uint8_t in_message[4096]; +static int in_len; +static uint16_t recv_len; + +static void check_ublox_message(char *which, uint8_t *msg); + +char +ao_serial1_getchar(void) +{ + char c; + uint8_t uc; + int i; + + i = getc(ao_gps_file); + if (i == EOF) { + perror("getchar"); + exit(1); + } + c = i; + uc = (uint8_t) c; + if (in_len || uc == 0xb5) { + in_message[in_len++] = c; + if (in_len == 6) { + recv_len = in_message[4] | (in_message[5] << 8); + } else if (in_len > 6 && in_len == recv_len + 8) { + check_ublox_message("recv", in_message + 2); + in_len = 0; + } + + } + return c; +} + +#define MESSAGE_LEN 4096 + +static uint8_t message[MESSAGE_LEN]; +static int message_len; +static uint16_t send_len; + +void +ao_serial1_putchar(char c) +{ + int i; + uint8_t uc = (uint8_t) c; + + if (message_len || uc == 0xb5) { + if (message_len < MESSAGE_LEN) + message[message_len++] = uc; + if (message_len == 6) { + send_len = message[4] | (message[5] << 8); + } else if (message_len > 6 && message_len == send_len + 8) { + check_ublox_message("send", message + 2); + message_len = 0; + } + } + + for (;;) { + i = write(ao_gps_fd, &c, 1); + if (i == 1) + break; + if (i < 0 && (errno == EINTR || errno == EAGAIN)) + continue; + perror("putchar"); + exit(1); + } +} + +#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) +{ + int fd = ao_gps_fd; + struct termios termios; + + printf ("\t\tset speed %d\n", speed); + tcdrain(fd); + tcgetattr(fd, &termios); + switch (speed) { + case AO_SERIAL_SPEED_4800: + cfsetspeed(&termios, B4800); + break; + case AO_SERIAL_SPEED_9600: + 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); +} + +#define ao_time() 0 + +uint8_t ao_task_minimize_latency; + +#define ao_usb_getchar() 0 + +#include "ao_gps_print.c" +#include "ao_gps_ublox.c" + +static void +check_ublox_message(char *which, uint8_t *msg) +{ + uint8_t class = msg[0]; + uint8_t id = msg[1]; + uint16_t len = msg[2] | (msg[3] << 8); + uint16_t i; + struct ao_ublox_cksum cksum_msg = { .a = msg[4 + len], + .b = msg[4 + len + 1] }; + struct ao_ublox_cksum cksum= { 0, 0 }; + + for (i = 0; i < 4 + len; i++) { + add_cksum(&cksum, msg[i]); + } + if (cksum.a != cksum_msg.a || cksum.b != cksum_msg.b) { + printf ("\t%s: cksum mismatch %02x,%02x != %02x,%02x\n", + which, + cksum_msg.a & 0xff, + cksum_msg.b & 0xff, + cksum.a & 0xff, + cksum.b & 0xff); + return; + } + switch (class) { + case UBLOX_NAV: + switch (id) { + case UBLOX_NAV_DOP: ; + struct ublox_nav_dop *nav_dop = (void *) msg; + printf ("\tnav-dop iTOW %9u gDOP %5u dDOP %5u tDOP %5u vDOP %5u hDOP %5u nDOP %5u eDOP %5u\n", + nav_dop->itow, + nav_dop->gdop, + nav_dop->ddop, + nav_dop->tdop, + nav_dop->vdop, + nav_dop->hdop, + nav_dop->ndop, + nav_dop->edop); + return; + case UBLOX_NAV_POSLLH: ; + struct ublox_nav_posllh *nav_posllh = (void *) msg; + printf ("\tnav-posllh iTOW %9u lon %12.7f lat %12.7f height %10.3f hMSL %10.3f hAcc %10.3f vAcc %10.3f\n", + nav_posllh->itow, + nav_posllh->lon / 1e7, + nav_posllh->lat / 1e7, + nav_posllh->height / 1e3, + nav_posllh->hmsl / 1e3, + nav_posllh->hacc / 1e3, + nav_posllh->vacc / 1e3); + return; + case UBLOX_NAV_SOL: ; + struct ublox_nav_sol *nav_sol = (struct ublox_nav_sol *) msg; + printf ("\tnav-sol iTOW %9u fTOW %9d week %5d gpsFix %2d flags %02x\n", + nav_sol->itow, nav_sol->ftow, nav_sol->week, + nav_sol->gpsfix, nav_sol->flags); + return; + case UBLOX_NAV_SVINFO: ; + struct ublox_nav_svinfo *nav_svinfo = (struct ublox_nav_svinfo *) msg; + printf ("\tnav-svinfo iTOW %9u numCH %3d globalFlags %02x\n", + nav_svinfo->itow, nav_svinfo->numch, nav_svinfo->globalflags); + int i; + for (i = 0; i < nav_svinfo->numch; i++) { + struct ublox_nav_svinfo_block *nav_svinfo_block = (void *) (msg + 12 + 12 * i); + printf ("\t\tchn %3u svid %3u flags %02x quality %3u cno %3u elev %3d azim %6d prRes %9d\n", + nav_svinfo_block->chn, + nav_svinfo_block->svid, + nav_svinfo_block->flags, + nav_svinfo_block->quality, + nav_svinfo_block->cno, + nav_svinfo_block->elev, + nav_svinfo_block->azim, + nav_svinfo_block->prres); + } + return; + case UBLOX_NAV_VELNED: ; + struct ublox_nav_velned *nav_velned = (void *) msg; + printf ("\tnav-velned iTOW %9u velN %10.2f velE %10.2f velD %10.2f speed %10.2f gSpeed %10.2f heading %10.5f sAcc %10.2f cAcc %10.5f\n", + nav_velned->itow, + nav_velned->veln / 1e2, + nav_velned->vele / 1e2, + nav_velned->veld / 1e2, + nav_velned->speed / 1e2, + nav_velned->gspeed / 1e2, + nav_velned->heading / 1e5, + nav_velned->sacc / 1e5, + nav_velned->cacc / 1e6); + return; + case UBLOX_NAV_TIMEUTC:; + struct ublox_nav_timeutc *nav_timeutc = (void *) msg; + printf ("\tnav-timeutc iTOW %9u tAcc %5u nano %5d %4u-%2d-%2d %2d:%02d:%02d flags %02x\n", + nav_timeutc->itow, + nav_timeutc->tacc, + nav_timeutc->nano, + nav_timeutc->year, + nav_timeutc->month, + nav_timeutc->day, + nav_timeutc->hour, + nav_timeutc->min, + nav_timeutc->sec, + nav_timeutc->valid); + return; + } + break; + } +#if 1 + printf ("\t%s: class %02x id %02x len %d:", which, class & 0xff, id & 0xff, len & 0xffff); + for (i = 0; i < len; i++) + printf (" %02x", msg[4 + i]); + printf (" cksum %02x %02x", cksum_msg.a & 0xff, cksum_msg.b & 0xff); +#endif + printf ("\n"); +} + +void +ao_dump_state(void *wchan) +{ + if (wchan == &ao_gps_data) + ao_gps_print(&ao_gps_data); + else + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); + return; +} + +int +ao_gps_open(const char *tty) +{ + struct termios termios; + int fd; + + fd = open (tty, O_RDWR); + if (fd < 0) + return -1; + + tcgetattr(fd, &termios); + cfmakeraw(&termios); + cfsetspeed(&termios, B4800); + tcsetattr(fd, TCSAFLUSH, &termios); + + tcdrain(fd); + tcflush(fd, TCIFLUSH); + return fd; +} + +#include + +static const struct option options[] = { + { .name = "tty", .has_arg = 1, .val = 'T' }, + { 0, 0, 0, 0}, +}; + +static void usage(char *program) +{ + fprintf(stderr, "usage: %s [--tty ]\n", program); + exit(1); +} + +int +main (int argc, char **argv) +{ + char *tty = "/dev/ttyUSB0"; + int c; + + while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) { + switch (c) { + case 'T': + tty = optarg; + break; + default: + usage(argv[0]); + break; + } + } + ao_gps_fd = ao_gps_open(tty); + if (ao_gps_fd < 0) { + perror (tty); + exit (1); + } + ao_gps_file = fdopen(ao_gps_fd, "r"); + ao_gps(); + return 0; +} -- cgit v1.2.3 From c88aa32b979f379e3cf316dcb651e264c32a5283 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 13 May 2013 22:59:26 -0700 Subject: altos/test: ao_gps_test_ublox uses ao_gps_blox.h Signed-off-by: Keith Packard --- src/test/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/test/Makefile') diff --git a/src/test/Makefile b/src/test/Makefile index 08aa8cd5..169c1dc6 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -38,7 +38,7 @@ ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< -ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h +ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h ao_gps_ublox.h cc $(CFLAGS) -o $@ $< ao_convert_test: ao_convert_test.c ao_convert.c altitude.h -- cgit v1.2.3