X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=3231194d519467843bff13b798761622e826bf34;hb=0ec77f5c90e0b930488ae2ab75efcbba8a3bd1d8;hp=4a8625cfaab0e44b7394fd675e1b45d8f6f515ba;hpb=6cb744e305116a738b5d71686c6748f6f08c12ea;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 4a8625cf..3231194d 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -26,9 +26,10 @@ #define AO_HERTZ 100 -#define AO_ADC_RING 64 -#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) -#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) +#define HAS_ADC 1 +#define AO_DATA_RING 64 +#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) +#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) #define AO_M_TO_HEIGHT(m) ((int16_t) (m)) #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) @@ -38,7 +39,6 @@ * One set of samples read from the A/D converter */ struct ao_adc { - uint16_t tick; /* tick when the sample was read */ int16_t accel; /* accelerometer */ int16_t pres; /* pressure sensor */ int16_t pres_real; /* unclipped */ @@ -54,6 +54,17 @@ struct ao_adc { #define __code #define __reentrant +#define HAS_FLIGHT 1 +#define HAS_IGNITE 1 +#define HAS_USB 1 +#define HAS_GPS 1 +#ifndef HAS_ACCEL +#define HAS_ACCEL 1 +#define HAS_ACCEL_REF 0 +#endif + +#include + #define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) #define from_fix(x) ((x) >> 16) @@ -90,8 +101,8 @@ extern enum ao_flight_state ao_flight_state; #define FALSE 0 #define TRUE 1 -struct ao_adc ao_adc_ring[AO_ADC_RING]; -uint8_t ao_adc_head; +volatile struct ao_data ao_data_ring[AO_DATA_RING]; +volatile uint8_t ao_data_head; int ao_summary = 0; #define ao_led_on(l) @@ -110,7 +121,7 @@ enum ao_igniter { ao_igniter_main = 1 }; -struct ao_adc ao_adc_static; +struct ao_data ao_data_static; int drogue_height; double drogue_time; @@ -124,7 +135,7 @@ static int32_t ao_k_height; void ao_ignite(enum ao_igniter igniter) { - double time = (double) (ao_adc_static.tick + tick_offset) / 100; + double time = (double) (ao_data_static.tick + tick_offset) / 100; if (igniter == ao_igniter_drogue) { drogue_time = time; @@ -139,7 +150,7 @@ struct ao_task { int dummy; }; -#define ao_add_task(t,f,n) +#define ao_add_task(t,f,n) ((void) (t)) #define ao_log_start() #define ao_log_stop() @@ -154,6 +165,7 @@ int ao_flight_debug; FILE *emulator_in; char *emulator_app; char *emulator_name; +char *emulator_info; double emulator_error_max = 4; double emulator_height_error_max = 20; /* noise in the baro sensor */ @@ -177,6 +189,7 @@ struct ao_cmds { #define ao_xmemset(d,v,c) memset(d,v,c) #define ao_xmemcmp(d,s,c) memcmp(d,s,c) +#define AO_NEED_ALTITUDE_TO_PRES 1 #include "ao_convert.c" struct ao_config { @@ -196,23 +209,17 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define HAS_FLIGHT 1 -#define HAS_ADC 1 -#define HAS_USB 1 -#define HAS_GPS 1 -#ifndef HAS_ACCEL -#define HAS_ACCEL 1 -#define HAS_ACCEL_REF 0 -#endif #define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; +typedef int16_t accel_t; + extern uint16_t ao_sample_tick; extern int16_t ao_sample_height; -extern int16_t ao_sample_accel; +extern accel_t ao_sample_accel; extern int32_t ao_accel_scale; extern int16_t ao_ground_height; extern int16_t ao_sample_alt; @@ -257,10 +264,11 @@ ao_test_exit(void) main_error = fabs(ao_test_main_height_time - main_time); landed_error = fabs(ao_test_landed_height - landed_height); landed_time_error = ao_test_landed_time - landed_time; - if (drogue_error > emulator_error_max || main_error > emulator_error_max || - landed_time_error > emulator_error_max || landed_error > emulator_height_error_max) { + if (drogue_error > emulator_error_max || main_error > emulator_error_max) { printf ("%s %s\n", emulator_app, emulator_name); + if (emulator_info) + printf ("\t%s\n", emulator_info); printf ("\tApogee error %g\n", drogue_error); printf ("\tMain error %g\n", main_error); printf ("\tLanded height error %g\n", landed_error); @@ -282,19 +290,19 @@ ao_insert(void) { double time; - ao_adc_ring[ao_adc_head] = ao_adc_static; - ao_adc_head = ao_adc_ring_next(ao_adc_head); + ao_data_ring[ao_data_head] = ao_data_static; + ao_data_head = ao_data_ring_next(ao_data_head); if (ao_flight_state != ao_flight_startup) { - double height = ao_pres_to_altitude(ao_adc_static.pres_real) - ao_ground_height; - double accel = ((ao_flight_ground_accel - ao_adc_static.accel) * GRAVITY * 2.0) / + double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; + double accel = ((ao_flight_ground_accel - ao_data_static.adc.accel) * GRAVITY * 2.0) / (ao_config.accel_minus_g - ao_config.accel_plus_g); if (!tick_offset) - tick_offset = -ao_adc_static.tick; - if ((prev_tick - ao_adc_static.tick) > 0x400) + tick_offset = -ao_data_static.tick; + if ((prev_tick - ao_data_static.tick) > 0x400) tick_offset += 65536; - prev_tick = ao_adc_static.tick; - time = (double) (ao_adc_static.tick + tick_offset) / 100; + prev_tick = ao_data_static.tick; + time = (double) (ao_data_static.tick + tick_offset) / 100; if (ao_test_max_height < height) { ao_test_max_height = height; @@ -461,7 +469,6 @@ union ao_telemetry_all { uint16_t uint16(uint8_t *bytes, int off) { - off++; return (uint16_t) bytes[off] | (((uint16_t) bytes[off+1]) << 8); } @@ -474,11 +481,10 @@ int16(uint8_t *bytes, int off) void ao_sleep(void *wchan) { - if (wchan == &ao_adc_head) { - char type; - uint16_t tick; - uint16_t a, b; - int ret; + if (wchan == &ao_data_head) { + char type = 0; + uint16_t tick = 0; + uint16_t a = 0, b = 0; uint8_t bytes[1024]; union ao_telemetry_all telem; char line[1024]; @@ -490,7 +496,7 @@ ao_sleep(void *wchan) for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { - ao_adc_static.accel = ao_flight_ground_accel; + ao_data_static.adc.accel = ao_flight_ground_accel; ao_insert(); return; } @@ -501,7 +507,7 @@ ao_sleep(void *wchan) printf ("no more data, exiting simulation\n"); ao_test_exit(); } - ao_adc_static.tick += 10; + ao_data_static.tick += 10; ao_insert(); return; } @@ -601,22 +607,22 @@ ao_sleep(void *wchan) } } else if (len == 99) { ao_flight_started = 1; - tick = uint16(bytes, 21); - ao_flight_ground_accel = int16(bytes, 7); - ao_config.accel_plus_g = int16(bytes, 17); - ao_config.accel_minus_g = int16(bytes, 19); + tick = uint16(bytes+1, 21); + ao_flight_ground_accel = int16(bytes+1, 7); + ao_config.accel_plus_g = int16(bytes+1, 17); + ao_config.accel_minus_g = int16(bytes+1, 19); type = 'A'; - a = int16(bytes, 23); - b = int16(bytes, 25); + a = int16(bytes+1, 23); + b = int16(bytes+1, 25); } else if (len == 98) { ao_flight_started = 1; - tick = uint16(bytes, 20); - ao_flight_ground_accel = int16(bytes, 6); - ao_config.accel_plus_g = int16(bytes, 16); - ao_config.accel_minus_g = int16(bytes, 18); + tick = uint16(bytes+1, 20); + ao_flight_ground_accel = int16(bytes+1, 6); + ao_config.accel_plus_g = int16(bytes+1, 16); + ao_config.accel_minus_g = int16(bytes+1, 18); type = 'A'; - a = int16(bytes, 22); - b = int16(bytes, 24); + a = int16(bytes+1, 22); + b = int16(bytes+1, 24); } else { printf("unknown len %d\n", len); continue; @@ -639,19 +645,19 @@ ao_sleep(void *wchan) case 'S': break; case 'A': - ao_adc_static.tick = tick; - ao_adc_static.accel = a; - ao_adc_static.pres_real = b; + ao_data_static.tick = tick; + ao_data_static.adc.accel = a; + ao_data_static.adc.pres_real = b; if (b < AO_MIN_BARO_VALUE) b = AO_MIN_BARO_VALUE; - ao_adc_static.pres = b; + ao_data_static.adc.pres = b; ao_records_read++; ao_insert(); return; case 'T': - ao_adc_static.tick = tick; - ao_adc_static.temp = a; - ao_adc_static.v_batt = b; + ao_data_static.tick = tick; + ao_data_static.adc.temp = a; + ao_data_static.adc.v_batt = b; break; case 'D': case 'G': @@ -674,13 +680,15 @@ 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) +void run_flight_fixed(char *name, FILE *f, int summary, char *info) { emulator_name = name; emulator_in = f; + emulator_info = info; ao_summary = summary; ao_flight_init(); ao_flight(); @@ -692,13 +700,14 @@ main (int argc, char **argv) int summary = 0; int c; int i; + char *info = NULL; #if HAS_ACCEL emulator_app="full"; #else emulator_app="baro"; #endif - while ((c = getopt_long(argc, argv, "sd", options, NULL)) != -1) { + while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) { switch (c) { case 's': summary = 1; @@ -706,11 +715,14 @@ main (int argc, char **argv) case 'd': ao_flight_debug = 1; break; + case 'i': + info = optarg; + break; } } if (optind == argc) - run_flight_fixed("", stdin, summary); + run_flight_fixed("", stdin, summary, info); else for (i = optind; i < argc; i++) { FILE *f = fopen(argv[i], "r"); @@ -718,7 +730,8 @@ main (int argc, char **argv) perror(argv[i]); continue; } - run_flight_fixed(argv[i], f, summary); + run_flight_fixed(argv[i], f, summary, info); fclose(f); } + exit(0); }