X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_flight_test.c;h=1616764451be187b0a3e1c407509a000e5fe0034;hp=f4731aa8718a9ef326134e379cda363f8fbfa08c;hb=20427ae4965f756aac0cedc5179a1c45b9a781f2;hpb=210dbaa23cdacf3a6f2d6e23493e96ee2ac9bca7 diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index f4731aa8..16167644 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -21,6 +21,9 @@ #include #include #include +#include + +#define AO_HERTZ 100 #define AO_ADC_RING 64 #define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) @@ -48,10 +51,10 @@ struct ao_adc { enum ao_flight_state { ao_flight_startup = 0, ao_flight_idle = 1, - ao_flight_launchpad = 2, + ao_flight_pad = 2, ao_flight_boost = 3, - ao_flight_coast = 4, - ao_flight_apogee = 5, + ao_flight_fast = 4, + ao_flight_coast = 5, ao_flight_drogue = 6, ao_flight_main = 7, ao_flight_landed = 8, @@ -60,6 +63,7 @@ enum ao_flight_state { 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) @@ -68,16 +72,22 @@ uint8_t ao_adc_head; #define ao_cmd_register(c) #define ao_usb_disable() #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; + void ao_ignite(enum ao_igniter igniter) { - printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); + printf ("ignite %s at %7.2f\n", igniter == ao_igniter_drogue ? "drogue" : "main", + (double) ao_adc_static.tick / 100.0); } struct ao_task { @@ -94,8 +104,6 @@ struct ao_task { #define AO_FLIGHT_TEST -struct ao_adc ao_adc_static; - FILE *emulator_in; void @@ -105,62 +113,67 @@ void ao_sleep(void *wchan); const char const * const ao_state_names[] = { - "startup", "idle", "pad", "boost", "coast", - "apogee", "drogue", "main", "landed", "invalid" + "startup", "idle", "pad", "boost", "fast", + "coast", "drogue", "main", "landed", "invalid" }; struct ao_cmds { - char cmd; void (*func)(void); const char *help; }; - -static int16_t altitude_table[2048] = { -#include "altitude.h" -}; - -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant -{ - pres = pres >> 4; - if (pres < 0) pres = 0; - if (pres > 2047) pres = 2047; - return altitude_table[pres]; -} - -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant -{ - int16_t pres; - - for (pres = 0; pres < 2047; pres++) - if (altitude_table[pres] <= alt) - break; - return pres << 4; -} +#include "ao_convert.c" struct ao_config { uint16_t main_deploy; - int16_t accel_zero_g; + int16_t accel_plus_g; + int16_t accel_minus_g; }; #define ao_config_get() -struct ao_config ao_config = { 250, 16000 }; +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 +#define USE_KALMAN 0 +#else +#define USE_KALMAN 1 +#endif #include "ao_flight.c" +#define to_double(f) ((f) / 65536.0) + void ao_insert(void) { ao_adc_ring[ao_adc_head] = ao_adc_static; ao_adc_head = ao_adc_ring_next(ao_adc_head); + if (ao_summary) + return; if (ao_flight_state != ao_flight_startup) { +#if USE_KALMAN + printf("time %7.2f accel %d pres %d k_height %8.2f k_speed %8.5f k_accel %8.5f\n", + (double) ao_adc_static.tick / 100, + ao_adc_static.accel, + ao_adc_static.pres, + to_double(ao_k_height), + to_double(ao_k_speed), + to_double(ao_k_accel)); +#else printf("time %g accel %d pres %d\n", (double) ao_adc_static.tick / 100, ao_adc_static.accel, ao_adc_static.pres); +#endif } } @@ -173,7 +186,7 @@ void ao_sleep(void *wchan) { ao_dump_state(); - if (wchan == &ao_adc_ring) { + if (wchan == &ao_adc_head) { char type; uint16_t tick; uint16_t a, b; @@ -212,7 +225,12 @@ ao_sleep(void *wchan) type = words[0][0]; tick = strtoul(words[1], NULL, 16); a = strtoul(words[2], NULL, 16); - b = strtoul(words[2], NULL, 16); + b = strtoul(words[3], NULL, 16); + } else if (nword >= 6 && strcmp(words[0], "Accel")) { + ao_config.accel_plus_g = atoi(words[3]); + ao_config.accel_minus_g = atoi(words[5]); + } else if (nword >= 4 && strcmp(words[0], "Main")) { + ao_config.main_deploy = atoi(words[2]); } else if (nword >= 36 && strcmp(words[0], "CALL") == 0) { tick = atoi(words[10]); if (!ao_flight_started) { @@ -231,6 +249,12 @@ ao_sleep(void *wchan) switch (type) { case 'F': ao_flight_ground_accel = a; + if (ao_config.accel_plus_g == 0) { + ao_config.accel_plus_g = a; + ao_config.accel_minus_g = a + 530; + } + if (ao_config.main_deploy == 0) + ao_config.main_deploy = 250; ao_flight_started = 1; break; case 'S': @@ -265,24 +289,63 @@ ao_dump_state(void) { if (ao_flight_state == ao_flight_startup) return; + if (ao_summary) + return; +#if HAS_ACCEL 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)); +#else + printf ("\t\t\t\t\t%s alt %d main %d\n", + ao_state_names[ao_flight_state], + 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)); +#endif if (ao_flight_state == ao_flight_landed) exit(0); } +static const struct option options[] = { + { .name = "summary", .has_arg = 0, .val = 's' }, + { 0, 0, 0, 0}, +}; + +void run_flight_fixed(char *name, FILE *f, int summary) +{ + 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; + + while ((c = getopt_long(argc, argv, "s", options, NULL)) != -1) { + switch (c) { + case 's': + summary = 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); + } }