X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_flight_test.c;h=57785442553b09486317b2c5649f016cd0b4ecc4;hp=51a5965c9c3302d6d6d1b965868b149ed5df4023;hb=4e2c18249e16c98cf5f7dccdf8d3b84bc473863a;hpb=7d7b476564a16eda81ab3406f70a21995e1b464e diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index 51a5965c..57785442 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -41,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 */ @@ -53,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, @@ -66,6 +85,11 @@ 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; @@ -130,7 +154,7 @@ int ao_flight_debug; FILE *emulator_in; char *emulator_app; char *emulator_name; -double emulator_error_max = 10; +double emulator_error_max = 4; void ao_dump_state(void); @@ -171,15 +195,27 @@ struct ao_config ao_config; #define HAS_ACCEL_REF 0 #endif -#include "ao_flight.c" - -#define to_double(f) ((f) / 65536.0) - #define GRAVITY 9.80665 -extern int16_t ao_ground_accel, ao_raw_accel; +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; @@ -189,6 +225,33 @@ 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) { @@ -197,7 +260,7 @@ ao_insert(void) 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) { - double height = ao_pres_to_altitude(ao_raw_pres) - ao_ground_height; + 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); @@ -207,6 +270,16 @@ ao_insert(void) 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, @@ -219,46 +292,12 @@ ao_insert(void) drogue_height, main_height, ao_error_h_sq_avg); - } - - 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_flight_state == ao_flight_landed) + ao_test_exit(); } } } -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_sleep(void *wchan) { @@ -339,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();