X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_flight_test.c;h=57785442553b09486317b2c5649f016cd0b4ecc4;hp=70888d34448cd090029f951db7e33d1f38e1c9c7;hb=4e2c18249e16c98cf5f7dccdf8d3b84bc473863a;hpb=e339ffd8bd8b9e3f4758017ba355028000cb612e diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index 70888d34..57785442 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -21,6 +21,8 @@ #include #include #include +#include +#include #define AO_HERTZ 100 @@ -28,6 +30,10 @@ #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 AO_M_TO_HEIGHT(m) ((int16_t) (m)) +#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) +#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) + /* * One set of samples read from the A/D converter */ @@ -35,6 +41,7 @@ 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 */ int16_t temp; /* temperature sensor */ int16_t v_batt; /* battery voltage */ int16_t sense_d; /* drogue continuity sense */ @@ -47,6 +54,24 @@ struct ao_adc { #define __code #define __reentrant +#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) + +/* + * Above this height, the baro sensor doesn't work + */ +#define AO_MAX_BARO_HEIGHT 12000 +#define AO_BARO_SATURATE 13000 +#define AO_MIN_BARO_VALUE ao_altitude_to_pres(AO_BARO_SATURATE) + +/* + * Above this speed, baro measurements are unreliable + */ +#define AO_MAX_BARO_SPEED 200 + +#define ACCEL_NOSE_UP (ao_accel_2g >> 2) + enum ao_flight_state { ao_flight_startup = 0, ao_flight_idle = 1, @@ -60,8 +85,14 @@ enum ao_flight_state { ao_flight_invalid = 9 }; +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; +int ao_summary = 0; #define ao_led_on(l) #define ao_led_off(l) @@ -72,16 +103,36 @@ uint8_t ao_adc_head; #define ao_telemetry_set_interval(x) #define ao_rdf_set(rdf) #define ao_packet_slave_start() +#define ao_packet_slave_stop() enum ao_igniter { ao_igniter_drogue = 0, ao_igniter_main = 1 }; +struct ao_adc ao_adc_static; + +int drogue_height; +double drogue_time; +int main_height; +double main_time; + +int tick_offset; + +static int32_t ao_k_height; + void ao_ignite(enum ao_igniter igniter) { - printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); + double time = (double) (ao_adc_static.tick + tick_offset) / 100; + + if (igniter == ao_igniter_drogue) { + drogue_time = time; + drogue_height = ao_k_height >> 16; + } else { + main_time = time; + main_height = ao_k_height >> 16; + } } struct ao_task { @@ -98,9 +149,12 @@ struct ao_task { #define AO_FLIGHT_TEST -struct ao_adc ao_adc_static; +int ao_flight_debug; FILE *emulator_in; +char *emulator_app; +char *emulator_name; +double emulator_error_max = 4; void ao_dump_state(void); @@ -138,33 +192,115 @@ struct ao_config ao_config; #define HAS_GPS 1 #ifndef HAS_ACCEL #define HAS_ACCEL 1 -#endif #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; + +extern uint16_t ao_sample_tick; + +extern int16_t ao_sample_height; +extern int16_t ao_sample_accel; +extern int32_t ao_accel_scale; +extern int16_t ao_ground_height; +extern int16_t ao_sample_alt; + +int ao_sample_prev_tick; +uint16_t prev_tick; +#include "ao_kalman.c" +#include "ao_sample.c" #include "ao_flight.c" +#define to_double(f) ((f) / 65536.0) + +static int ao_records_read = 0; +static int ao_eof_read = 0; +static int ao_flight_ground_accel; +static int ao_flight_started = 0; +static int ao_test_max_height; +static double ao_test_max_height_time; +static int ao_test_main_height; +static double ao_test_main_height_time; + +void +ao_test_exit(void) +{ + double drogue_error; + double main_error; + + if (!ao_test_main_height_time) { + ao_test_main_height_time = ao_test_max_height_time; + ao_test_main_height = ao_test_max_height; + } + drogue_error = fabs(ao_test_max_height_time - drogue_time); + main_error = fabs(ao_test_main_height_time - main_time); + if (drogue_error > emulator_error_max || main_error > emulator_error_max) { + printf ("%s %s\n", + emulator_app, emulator_name); + printf ("\tApogee error %g\n", drogue_error); + printf ("\tMain error %g\n", main_error); + printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f\n", + ao_test_max_height, ao_test_max_height_time, + ao_test_main_height, ao_test_main_height_time); + printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f\n", + drogue_height, drogue_time, main_height, main_time); + exit (1); + } + exit(0); +} + void ao_insert(void) { + double time; + ao_adc_ring[ao_adc_head] = ao_adc_static; ao_adc_head = ao_adc_ring_next(ao_adc_head); if (ao_flight_state != ao_flight_startup) { - printf("time %g accel %d pres %d\n", - (double) ao_adc_static.tick / 100, - ao_adc_static.accel, - ao_adc_static.pres); + 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) / + (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 += 65536; + prev_tick = ao_adc_static.tick; + time = (double) (ao_adc_static.tick + tick_offset) / 100; + + if (ao_test_max_height < height) { + ao_test_max_height = height; + ao_test_max_height_time = time; + } + if (height > ao_config.main_deploy) { + ao_test_main_height_time = time; + ao_test_main_height = height; + } + + if (!ao_summary) { + printf("%7.2f height %g accel %g state %s k_height %g k_speed %g k_accel %g drogue %d main %d error %d\n", + time, + height, + accel, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_k_speed / 65536.0 / 16.0, + ao_k_accel / 65536.0 / 16.0, + drogue_height, + main_height, + ao_error_h_sq_avg); + if (ao_flight_state == ao_flight_landed) + ao_test_exit(); + } } } -static int ao_records_read = 0; -static int ao_eof_read = 0; -static int ao_flight_ground_accel; -static int ao_flight_started = 0; - void ao_sleep(void *wchan) { - ao_dump_state(); if (wchan == &ao_adc_head) { char type; uint16_t tick; @@ -186,8 +322,9 @@ ao_sleep(void *wchan) if (!fgets(line, sizeof (line), emulator_in)) { if (++ao_eof_read >= 1000) { - printf ("no more data, exiting simulation\n"); - exit(0); + if (!ao_summary) + printf ("no more data, exiting simulation\n"); + ao_test_exit(); } ao_adc_static.tick += 10; ao_insert(); @@ -205,10 +342,10 @@ ao_sleep(void *wchan) tick = strtoul(words[1], NULL, 16); a = strtoul(words[2], NULL, 16); b = strtoul(words[3], NULL, 16); - } else if (nword >= 6 && strcmp(words[0], "Accel")) { + } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) { ao_config.accel_plus_g = atoi(words[3]); ao_config.accel_minus_g = atoi(words[5]); - } else if (nword >= 4 && strcmp(words[0], "Main")) { + } else if (nword >= 4 && strcmp(words[0], "Main") == 0) { ao_config.main_deploy = atoi(words[2]); } else if (nword >= 36 && strcmp(words[0], "CALL") == 0) { tick = atoi(words[10]); @@ -241,6 +378,9 @@ ao_sleep(void *wchan) case 'A': ao_adc_static.tick = tick; ao_adc_static.accel = a; + ao_adc_static.pres_real = b; + if (b < AO_MIN_BARO_VALUE) + b = AO_MIN_BARO_VALUE; ao_adc_static.pres = b; ao_records_read++; ao_insert(); @@ -266,26 +406,56 @@ ao_sleep(void *wchan) void ao_dump_state(void) { - if (ao_flight_state == ao_flight_startup) - return; - printf ("\t\t\t\t\t%s accel %g vel %g alt %d main %d\n", - ao_state_names[ao_flight_state], - (ao_ground_accel - ao_flight_accel) / COUNTS_PER_G * GRAVITY, - (double) ao_flight_vel / 100 / COUNTS_PER_G * GRAVITY, - ao_pres_to_altitude(ao_flight_pres) - ao_pres_to_altitude(ao_ground_pres), - ao_pres_to_altitude(ao_main_pres) - ao_pres_to_altitude(ao_ground_pres)); - if (ao_flight_state == ao_flight_landed) - exit(0); +} + +static const struct option options[] = { + { .name = "summary", .has_arg = 0, .val = 's' }, + { .name = "debug", .has_arg = 0, .val = 'd' }, + { 0, 0, 0, 0}, +}; + +void run_flight_fixed(char *name, FILE *f, int summary) +{ + emulator_name = name; + emulator_in = f; + ao_summary = summary; + ao_flight_init(); + ao_flight(); } int main (int argc, char **argv) { - emulator_in = fopen (argv[1], "r"); - if (!emulator_in) { - perror(argv[1]); - exit(1); + int summary = 0; + int c; + int i; + +#if HAS_ACCEL + emulator_app="full"; +#else + emulator_app="baro"; +#endif + while ((c = getopt_long(argc, argv, "sd", options, NULL)) != -1) { + switch (c) { + case 's': + summary = 1; + break; + case 'd': + ao_flight_debug = 1; + break; + } } - ao_flight_init(); - ao_flight(); + + if (optind == argc) + run_flight_fixed("", stdin, summary); + 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); + fclose(f); + } }