X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=9bb03d682f55c3c97fe2de9da639fdee5aecdb2c;hb=1ae3f467a1d7be2fc3b1a45ba12568a3a25a0099;hp=56733c891a9dd3dc8020452d7c2dccda625aa821;hpb=128bbfa150f88c09f7adde2434b7bf0b5a9ed556;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 56733c89..9bb03d68 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,8 @@ struct ao_adc { #define __code #define __reentrant +#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 +92,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 +112,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 +126,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 +141,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 +156,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 */ @@ -173,6 +176,10 @@ struct ao_cmds { const char *help; }; +#define ao_xmemcpy(d,s,c) memcpy(d,s,c) +#define ao_xmemset(d,v,c) memset(d,v,c) +#define ao_xmemcmp(d,s,c) memcmp(d,s,c) + #include "ao_convert.c" struct ao_config { @@ -180,6 +187,7 @@ struct ao_config { int16_t accel_plus_g; int16_t accel_minus_g; uint8_t pad_orientation; + uint16_t apogee_lockout; }; #define AO_PAD_ORIENTATION_ANTENNA_UP 0 @@ -192,6 +200,7 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) #define HAS_FLIGHT 1 +#define HAS_IGNITE 1 #define HAS_ADC 1 #define HAS_USB 1 #define HAS_GPS 1 @@ -252,10 +261,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); @@ -277,19 +287,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; @@ -469,11 +479,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]; @@ -485,7 +494,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; } @@ -496,7 +505,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; } @@ -519,6 +528,9 @@ ao_sleep(void *wchan) ao_config.accel_minus_g = atoi(words[5]); } else if (nword >= 4 && strcmp(words[0], "Main") == 0) { ao_config.main_deploy = atoi(words[2]); + } else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 && + strcmp(words[1], "lockout:") == 0) { + ao_config.apogee_lockout = atoi(words[2]); } else if (nword >= 36 && strcmp(words[0], "CALL") == 0) { tick = atoi(words[10]); if (!ao_flight_started) { @@ -542,7 +554,7 @@ ao_sleep(void *wchan) ao_flight_started = 1; } } else if (nword == 2 && strcmp(words[0], "TELEM") == 0) { - char *hex = words[1]; + __xdata char *hex = words[1]; char elt[3]; int i, len; uint8_t sum; @@ -574,7 +586,7 @@ ao_sleep(void *wchan) continue; } if (len == 36) { - memcpy(&telem, bytes + 1, 32); + ao_xmemcpy(&telem, bytes + 1, 32); tick = telem.generic.tick; switch (telem.generic.type) { case AO_TELEMETRY_SENSOR_TELEMETRUM: @@ -631,19 +643,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': @@ -666,13 +678,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(); @@ -684,13 +698,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; @@ -698,11 +713,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"); @@ -710,7 +728,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); }