X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=952a811af2f27e24facc8f5776e40fde0900dfdc;hb=5d9e715d570b24ac124c30772b11923bd26ed670;hp=7180f02d2cd4b5dba6ea2df607049e7a71fd404d;hpb=055f3232decc07e064d596469b81cf9869411c2d;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 7180f02d..952a811a 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -20,10 +20,13 @@ #include #include #include +#include #include #include #include +#define GRAVITY 9.80665 + #define AO_HERTZ 100 #define HAS_ADC 1 @@ -35,7 +38,12 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) -#if MEGAMETRUM +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +int ao_gps_new; + +#if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 #define HAS_MPU6000 1 @@ -120,6 +128,7 @@ int ao_summary = 0; #define ao_rdf_set(rdf) #define ao_packet_slave_start() #define ao_packet_slave_stop() +#define flush() enum ao_igniter { ao_igniter_drogue = 0, @@ -137,6 +146,18 @@ int tick_offset; static int32_t ao_k_height; +int16_t +ao_time(void) +{ + return ao_data_static.tick; +} + +void +ao_delay(int16_t interval) +{ + return; +} + void ao_ignite(enum ao_igniter igniter) { @@ -195,11 +216,13 @@ struct ao_cmds { #define ao_xmemcmp(d,s,c) memcmp(d,s,c) #define AO_NEED_ALTITUDE_TO_PRES 1 -#if MEGAMETRUM +#if TELEMEGA #include "ao_convert_pa.c" #include struct ao_ms5607_prom ms5607_prom; #include "ao_ms5607_convert.c" +#define AO_PYRO_NUM 4 +#include #else #include "ao_convert.c" #endif @@ -210,6 +233,12 @@ struct ao_config { int16_t accel_minus_g; uint8_t pad_orientation; uint16_t apogee_lockout; +#if TELEMEGA + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ + int16_t accel_zero_along; + int16_t accel_zero_across; + int16_t accel_zero_through; +#endif }; #define AO_PAD_ORIENTATION_ANTENNA_UP 0 @@ -222,7 +251,6 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; @@ -236,12 +264,32 @@ extern int32_t ao_accel_scale; extern alt_t ao_ground_height; extern alt_t ao_sample_alt; +double ao_sample_qangle; + int ao_sample_prev_tick; uint16_t prev_tick; + #include "ao_kalman.c" +#include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" +#if TELEMEGA +#define AO_PYRO_NUM 4 + +#define AO_PYRO_0 0 +#define AO_PYRO_1 1 +#define AO_PYRO_2 2 +#define AO_PYRO_3 3 + +static void +ao_pyro_pin_set(uint8_t pin, uint8_t value) +{ + printf ("set pyro %d %d\n", pin, value); +} + +#include "ao_pyro.c" +#endif #define to_double(f) ((f) / 65536.0) @@ -301,20 +349,6 @@ ao_test_exit(void) exit(0); } -#if HAS_MPU6000 -static double -ao_mpu6000_accel(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; -} - -static double -ao_mpu6000_gyro(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; -} -#endif - void ao_insert(void) { @@ -329,7 +363,7 @@ ao_insert(void) #else double accel = 0.0; #endif -#if MEGAMETRUM +#if TELEMEGA double height; ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); @@ -369,20 +403,24 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " -#if MEGAMETRUM - "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " +#if TELEMEGA + "angle %5d " +/* "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */ #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", time, height, accel, -#if MEGAMETRUM +#if TELEMEGA + ao_sample_orient, +/* ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), +*/ #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -562,10 +600,14 @@ ao_sleep(void *wchan) char *words[64]; int nword; +#if TELEMEGA + if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed) + ao_pyro_check(); +#endif for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { -#if MEGAMETRUM +#if TELEMEGA ao_data_static.mpu6000 = ao_ground_mpu6000; #else ao_data_static.adc.accel = ao_flight_ground_accel; @@ -591,8 +633,8 @@ ao_sleep(void *wchan) if (words[nword] == NULL) break; } -#if MEGAMETRUM - if (log_format == AO_LOG_FORMAT_MEGAMETRUM && nword == 30 && strlen(words[0]) == 1) { +#if TELEMEGA + if (log_format == AO_LOG_FORMAT_TELEMEGA && nword == 30 && strlen(words[0]) == 1) { int i; struct ao_ms5607_value value; @@ -625,17 +667,6 @@ ao_sleep(void *wchan) #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif - if (ao_records_read == 0) - ao_ground_mpu6000 = ao_data_static.mpu6000; - else if (ao_records_read < 10) { -#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) - f(accel_x); - f(accel_y); - f(accel_z); - f(gyro_x); - f(gyro_y); - f(gyro_z); - } ao_records_read++; ao_insert(); return; @@ -659,9 +690,26 @@ ao_sleep(void *wchan) else if (strcmp(words[1], "crc:") == 0) ms5607_prom.crc = strtoul(words[2], NULL, 10); continue; + } else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) { + int p = strtoul(words[1], NULL, 10); + int i, j; + struct ao_pyro *pyro = &ao_config.pyro[p]; + + for (i = 2; i < nword; i++) { + for (j = 0; j < NUM_PYRO_VALUES; j++) + if (!strcmp (words[2], ao_pyro_values[j].name)) + break; + if (j == NUM_PYRO_VALUES) + continue; + pyro->flags |= ao_pyro_values[j].flag; + if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) { + int16_t val = strtoul(words[++i], NULL, 10); + *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val; + } + } } #else - if (nword == 4 && log_format != AO_LOG_FORMAT_MEGAMETRUM) { + if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { type = words[0][0]; tick = strtoul(words[1], NULL, 16); a = strtoul(words[2], NULL, 16); @@ -675,6 +723,13 @@ ao_sleep(void *wchan) } 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]); +#ifdef TELEMEGA + } else if (nword >= 8 && strcmp(words[0], "IMU") == 0) { + ao_config.accel_zero_along = atoi(words[3]); + ao_config.accel_zero_across = atoi(words[5]); + ao_config.accel_zero_through = atoi(words[7]); + printf ("%d %d %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, ao_config.accel_zero_through); +#endif } 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 && @@ -778,7 +833,9 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; -#if MEGAMETRUM +#if TELEMEGA + (void) a; + (void) b; #else switch (type) { case 'F':