X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=6c0bb4da944f2c8565c98051ff7df8e02bb1ffee;hb=321d0f68c04a5a9c6ea7874081e6245d44c48bb4;hp=56733c891a9dd3dc8020452d7c2dccda625aa821;hpb=9513be7f9d3d0b0ec29f6487fa9dc8f1ac24d0de;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 56733c89..6c0bb4da 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -26,19 +26,34 @@ #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)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#if MEGAMETRUM +#define AO_ADC_NUM_SENSE 6 +#define HAS_MS5607 1 +#define HAS_MPU6000 1 +#define HAS_MMA655X 0 + +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; +}; +#else /* * 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 */ @@ -48,12 +63,27 @@ struct ao_adc { int16_t sense_m; /* main continuity sense */ }; +#ifndef HAS_ACCEL +#define HAS_ACCEL 1 +#define HAS_ACCEL_REF 0 +#endif + +#endif + #define __pdata #define __data #define __xdata #define __code #define __reentrant +#define HAS_FLIGHT 1 +#define HAS_IGNITE 1 +#define HAS_USB 1 +#define HAS_GPS 1 + +#include +#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) @@ -61,7 +91,6 @@ struct ao_adc { /* * 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) @@ -72,26 +101,13 @@ struct ao_adc { #define ACCEL_NOSE_UP (ao_accel_2g >> 2) -enum ao_flight_state { - ao_flight_startup = 0, - ao_flight_idle = 1, - ao_flight_pad = 2, - ao_flight_boost = 3, - ao_flight_fast = 4, - ao_flight_coast = 5, - ao_flight_drogue = 6, - ao_flight_main = 7, - ao_flight_landed = 8, - 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; +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 +126,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 +140,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 +155,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 +170,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,13 +190,26 @@ 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) + +#define AO_NEED_ALTITUDE_TO_PRES 1 +#if MEGAMETRUM +#include "ao_convert_pa.c" +#include +struct ao_ms5607_prom ms5607_prom; +#include "ao_ms5607_convert.c" +#else #include "ao_convert.c" +#endif struct ao_config { uint16_t main_deploy; 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 @@ -191,26 +221,20 @@ 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 alt_t ao_sample_height; +extern accel_t ao_sample_accel; extern int32_t ao_accel_scale; -extern int16_t ao_ground_height; -extern int16_t ao_sample_alt; +extern alt_t ao_ground_height; +extern alt_t ao_sample_alt; int ao_sample_prev_tick; uint16_t prev_tick; @@ -252,10 +276,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 +302,30 @@ 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) / +#if HAS_ACCEL + double accel = ((ao_flight_ground_accel - ao_data_accel_cook(&ao_data_static)) * GRAVITY * 2.0) / (ao_config.accel_minus_g - ao_config.accel_plus_g); +#else + double accel = 0.0; +#endif +#if MEGAMETRUM + double height; + + ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); + height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height; +#else + double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; +#endif 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; @@ -456,7 +492,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); } @@ -466,14 +501,29 @@ int16(uint8_t *bytes, int off) return (int16_t) uint16(bytes, off); } +uint32_t +uint32(uint8_t *bytes, int off) +{ + return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) | + (((uint32_t) bytes[off+2]) << 16) | + (((uint32_t) bytes[off+3]) << 24); +} + +int32_t +int32(uint8_t *bytes, int off) +{ + return (int32_t) uint32(bytes, off); +} + +static int log_format; + 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 +535,10 @@ ao_sleep(void *wchan) for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { - ao_adc_static.accel = ao_flight_ground_accel; +#if MEGAMETRUM +#else + ao_data_static.adc.accel = ao_flight_ground_accel; +#endif ao_insert(); return; } @@ -496,7 +549,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; } @@ -507,18 +560,77 @@ ao_sleep(void *wchan) if (words[nword] == NULL) break; } - if (nword == 4) { +#if MEGAMETRUM + if (log_format == AO_LOG_FORMAT_MEGAMETRUM && nword == 30 && strlen(words[0]) == 1) { + int i; + struct ao_ms5607_value value; + + type = words[0][0]; + tick = strtoul(words[1], NULL, 16); +// printf ("%c %04x", type, tick); + for (i = 2; i < nword; i++) { + bytes[i - 2] = strtoul(words[i], NULL, 16); +// printf(" %02x", bytes[i-2]); + } +// printf ("\n"); + switch (type) { + case 'F': + ao_flight_ground_accel = int16(bytes, 2); + ao_flight_started = 1; + ao_ground_pres = int32(bytes, 4); + ao_ground_height = ao_pa_to_altitude(ao_ground_pres); + break; + case 'A': + ao_data_static.tick = tick; + 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_records_read++; + ao_insert(); + return; + } + continue; + } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) { + if (strcmp(words[1], "reserved:") == 0) + ms5607_prom.reserved = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "sens:") == 0) + ms5607_prom.sens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "off:") == 0) + ms5607_prom.off = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tcs:") == 0) + ms5607_prom.tcs = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tco:") == 0) + ms5607_prom.tco = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tref:") == 0) + ms5607_prom.tref = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tempsens:") == 0) + ms5607_prom.tempsens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "crc:") == 0) + ms5607_prom.crc = strtoul(words[2], NULL, 10); + continue; + } +#else + if (nword == 4 && log_format != AO_LOG_FORMAT_MEGAMETRUM) { type = words[0][0]; tick = strtoul(words[1], NULL, 16); a = strtoul(words[2], NULL, 16); b = strtoul(words[3], NULL, 16); if (type == 'P') type = 'A'; + } +#endif + else if (nword == 2 && strcmp(words[0], "log-format") == 0) { + log_format = strtoul(words[1], NULL, 10); } 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") == 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 +654,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 +686,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: @@ -593,22 +705,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; @@ -617,6 +729,8 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; +#if MEGAMETRUM +#else switch (type) { case 'F': ao_flight_ground_accel = a; @@ -631,19 +745,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': @@ -652,6 +766,7 @@ ao_sleep(void *wchan) case 'H': break; } +#endif } } @@ -666,13 +781,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 +801,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 +816,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 +831,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); }