X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=d3d39f2a1b721b92d9327345964bf432c15b205c;hp=314998c1bc750667bfe4a50abba06cb2f689b75b;hb=545bdc1f8046b08e237307d97d0044ddae9095b8;hpb=34d5be68ca23e8beb05db9a480faef63ecc911d0 diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 314998c1..d3d39f2a 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -34,15 +35,21 @@ #define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) #define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) +#if 0 #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)) +#endif #define AO_GPS_NEW_DATA 1 #define AO_GPS_NEW_TRACKING 2 int ao_gps_new; +#if !defined(TELEMEGA) && !defined(TELEMETRUM_V2) && !defined(EASYMINI) +#define TELEMETRUM_V1 1 +#endif + #if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 @@ -51,6 +58,7 @@ int ao_gps_new; #define HAS_HMC5883 1 #define HAS_BEEP 1 #define AO_CONFIG_MAX_SIZE 1024 +#define AO_MMA655X_INVERT 0 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -58,7 +66,38 @@ struct ao_adc { int16_t v_pbatt; int16_t temp; }; -#else +#endif + +#if TELEMETRUM_V2 +#define AO_ADC_NUM_SENSE 2 +#define HAS_MS5607 1 +#define HAS_MMA655X 1 +#define AO_MMA655X_INVERT 1 +#define HAS_BEEP 1 +#define AO_CONFIG_MAX_SIZE 1024 + +struct ao_adc { + int16_t sense_a; + int16_t sense_m; + int16_t v_batt; + int16_t temp; +}; +#endif + +#if EASYMINI +#define AO_ADC_NUM_SENSE 2 +#define HAS_MS5607 1 +#define HAS_BEEP 1 +#define AO_CONFIG_MAX_SIZE 1024 + +struct ao_adc { + int16_t sense_a; + int16_t sense_m; + int16_t v_batt; +}; +#endif + +#if TELEMETRUM_V1 /* * One set of samples read from the A/D converter */ @@ -93,6 +132,7 @@ struct ao_adc { #include #include #include +#include #if TELEMEGA int ao_gps_count; @@ -186,17 +226,6 @@ ao_gps_angle(void) #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_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) extern enum ao_flight_state ao_flight_state; @@ -234,7 +263,7 @@ double main_time; int tick_offset; -static int32_t ao_k_height; +static ao_k_t ao_k_height; int16_t ao_time(void) @@ -306,7 +335,7 @@ struct ao_cmds { #define ao_xmemcmp(d,s,c) memcmp(d,s,c) #define AO_NEED_ALTITUDE_TO_PRES 1 -#if TELEMEGA +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI #include "ao_convert_pa.c" #include struct ao_ms5607_prom ao_ms5607_prom; @@ -358,6 +387,8 @@ uint16_t prev_tick; #define AO_PYRO_2 2 #define AO_PYRO_3 3 +#define PYRO_DBG 1 + static void ao_pyro_pin_set(uint8_t pin, uint8_t value) { @@ -456,7 +487,7 @@ ao_insert(void) #else double accel = 0.0; #endif -#if TELEMEGA +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI double height; ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); @@ -540,9 +571,9 @@ ao_insert(void) ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); +#if 0 int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); -#if 0 printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n", time, ao_state_names[ao_flight_state], @@ -551,6 +582,7 @@ ao_insert(void) mag_azel.el, mag_azel.az); #endif +#if 0 printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n", time, ao_state_names[ao_flight_state], @@ -560,6 +592,7 @@ ao_insert(void) (int) floor (ao_gps_angle() + 0.5), (ao_gps_static.flags & 0xf) * 10); +#endif #if 0 printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", ao_state_names[ao_flight_state], @@ -582,9 +615,9 @@ ao_insert(void) #endif #endif -#if 0 +#if 1 printf("%7.2f height %8.2f accel %8.3f " -#if TELEMEGA +#if TELEMEGA && 1 "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 mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " #endif @@ -592,7 +625,7 @@ ao_insert(void) time, height, accel, -#if TELEMEGA +#if TELEMEGA && 1 ao_sample_orient, ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), @@ -649,6 +682,19 @@ int32(uint8_t *bytes, int off) return (int32_t) uint32(bytes, off); } +uint32_t +uint24(uint8_t *bytes, int off) +{ + return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) | + (((uint32_t) bytes[off+2]) << 16); +} + +int32_t +int24(uint8_t *bytes, int off) +{ + return (int32_t) uint24(bytes, off); +} + static int log_format; void @@ -673,11 +719,14 @@ ao_sleep(void *wchan) for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { + #if TELEMEGA ao_data_static.mpu6000 = ao_ground_mpu6000; -#else +#endif +#if TELEMETRUM_V1 ao_data_static.adc.accel = ao_flight_ground_accel; #endif + ao_insert(); return; } @@ -700,7 +749,7 @@ ao_sleep(void *wchan) break; } #if TELEMEGA - if (log_format == AO_LOG_FORMAT_TELEMEGA && nword == 30 && strlen(words[0]) == 1) { + if ((log_format == AO_LOG_FORMAT_TELEMEGA_OLD || log_format == AO_LOG_FORMAT_TELEMEGA) && nword == 30 && strlen(words[0]) == 1) { int i; struct ao_ms5607_value value; @@ -747,6 +796,8 @@ ao_sleep(void *wchan) ao_data_static.hmc5883.z = int16(bytes, 24); #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + ao_data_static.mma655x = ao_data_accel_invert(ao_data_static.mma655x); #endif ao_records_read++; ao_insert(); @@ -804,7 +855,125 @@ ao_sleep(void *wchan) } } } -#else +#endif +#if EASYMINI + if ((log_format == AO_LOG_FORMAT_EASYMINI1 || log_format == AO_LOG_FORMAT_EASYMINI2) && nword == 14 && 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_started = 1; + ao_ground_pres = uint32(bytes, 4); + ao_ground_height = ao_pa_to_altitude(ao_ground_pres); +#if 0 + printf("ground pres %d height %d\n", ao_ground_pres, ao_ground_height); + printf("sens %d off %d tcs %d tco %d tref %d tempsens %d crc %d\n", + ao_ms5607_prom.sens, + ao_ms5607_prom.off, + ao_ms5607_prom.tcs, + ao_ms5607_prom.tco, + ao_ms5607_prom.tref, + ao_ms5607_prom.tempsens, + ao_ms5607_prom.crc); +#endif + break; + case 'A': + ao_data_static.tick = tick; + ao_data_static.ms5607_raw.pres = int24(bytes, 0); + ao_data_static.ms5607_raw.temp = int24(bytes, 3); +#if 0 + printf("raw pres %d temp %d\n", + ao_data_static.ms5607_raw.pres, + ao_data_static.ms5607_raw.temp); +#endif + ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); +// printf("pres %d height %d\n", value.pres, ao_pa_to_altitude(value.pres)); + ao_records_read++; + ao_insert(); + return; + } + continue; + } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) { + if (strcmp(words[1], "reserved:") == 0) + ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "sens:") == 0) + ao_ms5607_prom.sens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "off:") == 0) + ao_ms5607_prom.off = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tcs:") == 0) + ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tco:") == 0) + ao_ms5607_prom.tco = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tref:") == 0) + ao_ms5607_prom.tref = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tempsens:") == 0) + ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "crc:") == 0) + ao_ms5607_prom.crc = strtoul(words[2], NULL, 10); + continue; + } +#endif +#if TELEMETRUM_V2 + if (log_format == AO_LOG_FORMAT_TELEMETRUM && nword == 14 && 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_static.mma655x = int16(bytes, 8); + ao_records_read++; + ao_insert(); + return; + } + continue; + } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) { + if (strcmp(words[1], "reserved:") == 0) + ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "sens:") == 0) + ao_ms5607_prom.sens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "off:") == 0) + ao_ms5607_prom.off = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tcs:") == 0) + ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tco:") == 0) + ao_ms5607_prom.tco = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tref:") == 0) + ao_ms5607_prom.tref = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tempsens:") == 0) + ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "crc:") == 0) + ao_ms5607_prom.crc = strtoul(words[2], NULL, 10); + continue; + } +#endif +#if TELEMETRUM_V1 if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { type = words[0][0]; tick = strtoul(words[1], NULL, 16); @@ -824,13 +993,15 @@ ao_sleep(void *wchan) 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 && strcmp(words[1], "lockout:") == 0) { ao_config.apogee_lockout = atoi(words[2]); + } else if (nword >= 3 && strcmp(words[0], "Pad") == 0 && + strcmp(words[1], "orientation:") == 0) { + ao_config.pad_orientation = atoi(words[2]); } else if (nword >= 36 && strcmp(words[0], "CALL") == 0) { tick = atoi(words[10]); if (!ao_flight_started) { @@ -929,7 +1100,7 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; -#if TELEMEGA +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI (void) a; (void) b; #else @@ -950,8 +1121,6 @@ ao_sleep(void *wchan) 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_data_static.adc.pres = b; ao_records_read++; ao_insert();