X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=0913e7ba96a3a99f4498f6a0c1820d215e232a1e;hp=acdf4d926a3ead68460fd6b284d2e656a30ebf8f;hb=6023ff81f1bbd240169b9548209133d3b02d475f;hpb=6fa1ec0dbf2a4eda8d061c67b3779b83b88f29f0 diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index acdf4d92..0913e7ba 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 @@ -20,9 +21,13 @@ #include #include #include +#include #include #include #include +#define log ao_log_data + +#define GRAVITY 9.80665 #define AO_HERTZ 100 @@ -31,25 +36,69 @@ #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 -#if MEGAMETRUM +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 #define HAS_MPU6000 1 #define HAS_MMA655X 1 +#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]; int16_t v_batt; int16_t v_pbatt; - int16_t accel_ref; - int16_t accel; 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 0 +#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 */ @@ -83,22 +132,101 @@ struct ao_adc { #include #include +#include +#include + +#if TELEMEGA +int ao_gps_count; +struct ao_telemetry_location ao_gps_first; +struct ao_telemetry_location ao_gps_prev; +struct ao_telemetry_location ao_gps_static; + +struct ao_telemetry_satellite ao_gps_tracking; + +static inline double sqr(double a) { return a * a; } + +void +cc_great_circle (double start_lat, double start_lon, + double end_lat, double end_lon, + double *dist, double *bearing) +{ + const double rad = M_PI / 180; + const double earth_radius = 6371.2 * 1000; /* in meters */ + double lat1 = rad * start_lat; + double lon1 = rad * -start_lon; + double lat2 = rad * end_lat; + double lon2 = rad * -end_lon; + +// double d_lat = lat2 - lat1; + double d_lon = lon2 - lon1; + + /* From http://en.wikipedia.org/wiki/Great-circle_distance */ + double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) + + sqr(cos(lat1) * sin(lat2) - + sin(lat1) * cos(lat2) * cos(d_lon))); + double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon); + double d = atan2(vdn,vdd); + double course; + + if (cos(lat1) < 1e-20) { + if (lat1 > 0) + course = M_PI; + else + course = -M_PI; + } else { + if (d < 1e-10) + course = 0; + else + course = acos((sin(lat2)-sin(lat1)*cos(d)) / + (sin(d)*cos(lat1))); + if (sin(lon2-lon1) > 0) + course = 2 * M_PI-course; + } + *dist = d * earth_radius; + *bearing = course * 180/M_PI; +} + +double +ao_distance_from_pad(void) +{ + double dist, bearing; + if (!ao_gps_count) + return 0; + + cc_great_circle(ao_gps_first.latitude / 1e7, + ao_gps_first.longitude / 1e7, + ao_gps_static.latitude / 1e7, + ao_gps_static.longitude / 1e7, + &dist, &bearing); + return dist; +} + +double +ao_gps_angle(void) +{ + double dist, bearing; + double height; + double angle; + + if (ao_gps_count < 2) + return 0; + + cc_great_circle(ao_gps_prev.latitude / 1e7, + ao_gps_prev.longitude / 1e7, + ao_gps_static.latitude / 1e7, + ao_gps_static.longitude / 1e7, + &dist, &bearing); + height = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_static) - AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_prev); + + angle = atan2(dist, height); + return angle * 180/M_PI; +} +#endif #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_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; @@ -120,6 +248,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, @@ -135,7 +264,20 @@ double main_time; int tick_offset; -static int32_t ao_k_height; +static ao_k_t ao_k_height; +static double simple_speed; + +int16_t +ao_time(void) +{ + return ao_data_static.tick; +} + +void +ao_delay(int16_t interval) +{ + return; +} void ao_ignite(enum ao_igniter igniter) @@ -163,10 +305,13 @@ struct ao_task { #define AO_MS_TO_TICKS(ms) ((ms) / 10) #define AO_SEC_TO_TICKS(s) ((s) * 100) -#define AO_FLIGHT_TEST +#define AO_FLIGHT_TEST 1 int ao_flight_debug; +struct ao_eeprom *eeprom; +uint32_t eeprom_offset; + FILE *emulator_in; char *emulator_app; char *emulator_name; @@ -180,7 +325,7 @@ ao_dump_state(void); void ao_sleep(void *wchan); -const char const * const ao_state_names[] = { +const char * const ao_state_names[] = { "startup", "idle", "pad", "boost", "fast", "coast", "drogue", "main", "landed", "invalid" }; @@ -195,39 +340,39 @@ struct ao_cmds { #define ao_xmemcmp(d,s,c) memcmp(d,s,c) #define AO_NEED_ALTITUDE_TO_PRES 1 -#if MEGAMETRUM +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI #include "ao_convert_pa.c" #include -struct ao_ms5607_prom ms5607_prom; +struct ao_ms5607_prom ao_ms5607_prom; #include "ao_ms5607_convert.c" +#if TELEMEGA +#define AO_PYRO_NUM 4 +#include +#endif #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 -#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 +#include +#include +#include +#include #define ao_config_get() struct ao_config ao_config; -#define DATA_TO_XDATA(x) (x) +#define x (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; typedef int16_t accel_t; +uint16_t ao_serial_number; +int16_t ao_flight_number; + extern uint16_t ao_sample_tick; extern alt_t ao_sample_height; @@ -236,18 +381,44 @@ 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 + +#define PYRO_DBG 1 + +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 +#include "ao_eeprom_read.c" +#include "ao_eeprom_read_old.c" #define to_double(f) ((f) / 65536.0) static int ao_records_read = 0; static int ao_eof_read = 0; +#if !EASYMINI static int ao_flight_ground_accel; +#endif static int ao_flight_started = 0; static int ao_test_max_height; static double ao_test_max_height_time; @@ -259,6 +430,9 @@ static double ao_test_landed_time; static int landed_set; static double landed_time; static double landed_height; +#if AO_PYRO_NUM +static uint16_t pyros_fired; +#endif #if HAS_MPU6000 static struct ao_mpu6000_sample ao_ground_mpu6000; @@ -301,17 +475,20 @@ 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; -} +#ifdef TELEMEGA +struct ao_azel { + int az; + int el; +}; -static double -ao_mpu6000_gyro(int16_t sensor) +static void +azel (struct ao_azel *r, struct ao_quaternion *q) { - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; + double v; + + r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5); + v = sqrt (q->x*q->x + q->y*q->y); + r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5); } #endif @@ -321,30 +498,49 @@ ao_insert(void) double time; 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) { #if HAS_ACCEL - double accel = ((ao_flight_ground_accel - ao_data_accel_cook(&ao_data_static)) * GRAVITY * 2.0) / + double accel = ((ao_flight_ground_accel - ao_data_accel(&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 + (void) accel; if (!tick_offset) tick_offset = -ao_data_static.tick; if ((prev_tick - ao_data_static.tick) > 0x400) tick_offset += 65536; + if (prev_tick) { + int ticks = ao_data_static.tick - prev_tick; + if (ticks < 0) + ticks += 65536; + simple_speed += accel * ticks / 100.0; + } prev_tick = ao_data_static.tick; time = (double) (ao_data_static.tick + tick_offset) / 100; +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI + ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); + double height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height; + + /* Hack to skip baro spike at accidental drogue charge + * firing in 2015-09-26-serial-2093-flight-0012.eeprom + * so we can test the kalman filter with this data. Just + * keep reporting the same baro value across the pressure spike + */ + { + static struct ao_ms5607_sample save; + if (ao_serial_number == 2093 && ao_flight_number == 12 && 32.5 < time && time < 33.7) { + ao_data_ring[ao_data_head].ms5607_raw = save; + } else { + save = ao_data_static.ms5607_raw; + } + } +#else + double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; +#endif + if (ao_test_max_height < height) { ao_test_max_height = height; ao_test_max_height_time = time; @@ -368,156 +564,138 @@ 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 + static struct ao_quaternion ao_ground_mag; + static int ao_ground_mag_set; + + if (!ao_ground_mag_set) { + ao_quaternion_init_vector (&ao_ground_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag); + ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation); + ao_ground_mag_set = 1; + } + + struct ao_quaternion ao_mag, ao_mag_rot; + + ao_quaternion_init_vector(&ao_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + + ao_quaternion_normalize(&ao_mag, &ao_mag); + ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation); + + float ao_dot; + int ao_mag_angle; + + ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag); + + struct ao_azel ground_azel, mag_azel, rot_azel; + + azel(&ground_azel, &ao_ground_mag); + azel(&mag_azel, &ao_mag); + azel(&rot_azel, &ao_mag_rot); + + ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5); + + (void) ao_mag_angle; + + static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 }; + struct ao_quaternion ao_out; + + ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); + +#if 0 + int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); + + 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], + ao_k_height / 65536.0, + ao_sample_orient, out, + 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], + ao_k_height / 65536.0, + ao_sample_orient, out, + ao_distance_from_pad(), + (int) floor (ao_gps_angle() + 0.5), + (ao_gps_static.flags & 0xf) * 10); + #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", +#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], + ground_azel.az, ground_azel.el, + mag_azel.az, mag_azel.el, + rot_azel.az, rot_azel.el, + ground_azel.el - rot_azel.el, + ground_azel.az - rot_azel.az, + ao_mag_angle, + ao_sample_orient, + ao_ground_mag.x, + ao_ground_mag.y, + ao_ground_mag.z, + ao_mag.x, + ao_mag.y, + ao_mag.z, + ao_mag_rot.x, + ao_mag_rot.y, + ao_mag_rot.z); +#endif +#endif + +#if 1 + printf("%7.2f height %8.2f accel %8.3f accel_speed %8.3f " + "state %d k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d" +#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 mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " +#endif + "\n", time, height, accel, -#if MEGAMETRUM + simple_speed > -100.0 ? simple_speed : -100.0, + ao_flight_state * 10, + ao_k_height / 65536.0, + ao_k_speed / 65536.0 / 16.0, + ao_k_accel / 65536.0 / 16.0, + ao_avg_height, + drogue_height, + main_height, + ao_error_h_sq_avg +#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), + ao_data_static.hmc5883.x, + ao_data_static.hmc5883.y, + ao_data_static.hmc5883.z, + ao_mag_angle #endif - ao_state_names[ao_flight_state], - ao_k_height / 65536.0, - ao_k_speed / 65536.0 / 16.0, - ao_k_accel / 65536.0 / 16.0, - ao_avg_height, - drogue_height, - main_height, - ao_error_h_sq_avg); - + ); +#endif + // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); } } + ao_data_head = ao_data_ring_next(ao_data_head); } -#define AO_MAX_CALLSIGN 8 -#define AO_MAX_VERSION 8 -#define AO_MAX_TELEMETRY 128 - -struct ao_telemetry_generic { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t payload[27]; /* 5 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 -#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 -#define AO_TELEMETRY_SENSOR_TELENANO 0x03 - -struct ao_telemetry_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 accelerometer (TM only) */ - int16_t pres; /* 8 pressure sensor */ - int16_t temp; /* 10 temperature sensor */ - int16_t v_batt; /* 12 battery voltage */ - int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ - int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int16_t ground_pres; /* 24 average pres on pad */ - int16_t ground_accel; /* 26 average accel on pad */ - int16_t accel_plus_g; /* 28 accel calibration at +1g */ - int16_t accel_minus_g; /* 30 accel calibration at -1g */ - /* 32 */ -}; - -#define AO_TELEMETRY_CONFIGURATION 0x04 - -struct ao_telemetry_configuration { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t device; /* 5 device type */ - uint16_t flight; /* 6 flight number */ - uint8_t config_major; /* 8 Config major version */ - uint8_t config_minor; /* 9 Config minor version */ - uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ - uint16_t main_deploy; /* 12 Main deploy alt in meters */ - uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ - char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ - char version[AO_MAX_VERSION]; /* 24 Software version */ - /* 32 */ -}; - -#define AO_TELEMETRY_LOCATION 0x05 - -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' - -struct ao_telemetry_location { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ - int32_t latitude; /* 8 latitude (degrees * 10⁷) */ - int32_t longitude; /* 12 longitude (degrees * 10⁷) */ - uint8_t year; /* 16 (- 2000) */ - uint8_t month; /* 17 (1-12) */ - uint8_t day; /* 18 (1-31) */ - uint8_t hour; /* 19 (0-23) */ - uint8_t minute; /* 20 (0-59) */ - uint8_t second; /* 21 (0-59) */ - uint8_t pdop; /* 22 (m * 5) */ - uint8_t hdop; /* 23 (m * 5) */ - uint8_t vdop; /* 24 (m * 5) */ - uint8_t mode; /* 25 */ - uint16_t ground_speed; /* 26 cm/s */ - int16_t climb_rate; /* 28 cm/s */ - uint8_t course; /* 30 degrees / 2 */ - uint8_t unused[1]; /* 31 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SATELLITE 0x06 - -struct ao_telemetry_satellite_info { - uint8_t svid; - uint8_t c_n_1; -}; - -struct ao_telemetry_satellite { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t channels; /* 5 number of reported sats */ - - struct ao_telemetry_satellite_info sats[12]; /* 6 */ - uint8_t unused[2]; /* 30 */ - /* 32 */ -}; - -union ao_telemetry_all { - struct ao_telemetry_generic generic; - struct ao_telemetry_sensor sensor; - struct ao_telemetry_configuration configuration; - struct ao_telemetry_location location; - struct ao_telemetry_satellite satellite; -}; uint16_t uint16(uint8_t *bytes, int off) @@ -545,490 +723,221 @@ int32(uint8_t *bytes, int off) return (int32_t) uint32(bytes, off); } -static int log_format; - -#if MEGAMETRUM - -static double -ao_vec_norm(double x, double y, double z) -{ - return x*x + y*y + z*z; -} - -static void -ao_vec_normalize(double *x, double *y, double *z) -{ - double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); - - *x *= scale; - *y *= scale; - *z *= scale; -} - -struct ao_quat { - double q0, q1, q2, q3; -}; - -static void -ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) -{ - r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; - r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; - r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; - r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; -} - -#if 0 -static void -ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) -{ - r->q0 = a->q0 * s; - r->q1 = a->q1 * s; - r->q2 = a->q2 * s; - r->q3 = a->q3 * s; -} -#endif - -static void -ao_quat_conj(struct ao_quat *r, struct ao_quat *a) -{ - r->q0 = a->q0; - r->q1 = -a->q1; - r->q2 = -a->q2; - r->q3 = -a->q3; -} - -static void -ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) -{ - struct ao_quat t; - struct ao_quat c; - ao_quat_mul(&t, q, a); - ao_quat_conj(&c, q); - ao_quat_mul(r, &t, &c); -} - -static void -ao_quat_from_angle(struct ao_quat *r, - double x_rad, - double y_rad, - double z_rad) -{ - double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); - double s = sin(angle/2); - double c = cos(angle/2); - - r->q0 = c; - r->q1 = x_rad * s / angle; - r->q2 = y_rad * s / angle; - r->q3 = z_rad * s / angle; -} - -static void -ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) -{ - ao_vec_normalize(&x, &y, &z); - double x_rad = atan2(z, y); - double y_rad = atan2(x, z); - double z_rad = atan2(y, x); - - ao_quat_from_angle(r, x_rad, y_rad, z_rad); -} - -static double -ao_quat_norm(struct ao_quat *a) -{ - return (a->q0 * a->q0 + - a->q1 * a->q1 + - a->q2 * a->q2 + - a->q3 * a->q3); -} - -static void -ao_quat_normalize(struct ao_quat *a) +uint32_t +uint24(uint8_t *bytes, int off) { - double norm = ao_quat_norm(a); - - if (norm) { - double m = 1/sqrt(norm); - - a->q0 *= m; - a->q1 *= m; - a->q2 *= m; - a->q3 *= m; - } + return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) | + (((uint32_t) bytes[off+2]) << 16); } -static struct ao_quat ao_up, ao_current; -static struct ao_quat ao_orient; -static int ao_orient_tick; - -void -set_orientation(double x, double y, double z, int tick) +int32_t +int24(uint8_t *bytes, int off) { - struct ao_quat t; - - printf ("set_orientation %g %g %g\n", x, y, z); - ao_quat_from_vector(&ao_orient, x, y, z); - ao_up.q1 = ao_up.q2 = 0; - ao_up.q0 = ao_up.q3 = sqrt(2)/2; - ao_orient_tick = tick; - - ao_orient.q0 = 1; - ao_orient.q1 = 0; - ao_orient.q2 = 0; - ao_orient.q3 = 0; - - printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_up.q0, - ao_up.q1, - ao_up.q2, - ao_up.q3); - - ao_quat_rot(&t, &ao_up, &ao_orient); - printf ("pad orient (%g) %g %g %g\n", - t.q0, - t.q1, - t.q2, - t.q3); - + return (int32_t) uint24(bytes, off); } -void -update_orientation (double rate_x, double rate_y, double rate_z, int tick) -{ - struct ao_quat q_dot; - double lambda; - double dt = (tick - ao_orient_tick) / 100.0; - - ao_orient_tick = tick; - -// lambda = 1 - ao_quat_norm(&ao_orient); - lambda = 0; - - q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; - q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; - q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2; - q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3; - - printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt); - printf ("q_dot (%g) %g %g %g\n", - q_dot.q0, - q_dot.q1, - q_dot.q2, - q_dot.q3); - - ao_orient.q0 += q_dot.q0 * dt; - ao_orient.q1 += q_dot.q1 * dt; - ao_orient.q2 += q_dot.q2 * dt; - ao_orient.q3 += q_dot.q3 * dt; - - ao_quat_normalize(&ao_orient); - - ao_quat_rot(&ao_current, &ao_up, &ao_orient); - - printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_current.q0, - ao_current.q1, - ao_current.q2, - ao_current.q3); -} -#endif +static int log_format; void ao_sleep(void *wchan) { 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]; - char *saveptr; - char *l; - 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 +#endif +#if TELEMETRUM_V1 ao_data_static.adc.accel = ao_flight_ground_accel; #endif + ao_insert(); return; } - if (!fgets(line, sizeof (line), emulator_in)) { - if (++ao_eof_read >= 1000) { - if (!ao_summary) - printf ("no more data, exiting simulation\n"); + if (eeprom) { +#if TELEMEGA + struct ao_log_mega *log_mega; +#endif +#if TELEMETRUM_V2 + struct ao_log_metrum *log_metrum; +#endif +#if EASYMINI + struct ao_log_mini *log_mini; +#endif +#if TELEMETRUM_V1 + struct ao_log_record *log_record; +#endif + + if (eeprom_offset >= eeprom->len) { + if (++ao_eof_read >= 1000) + if (!ao_summary) + printf ("no more data, exiting simulation\n"); ao_test_exit(); + ao_data_static.tick += 10; + ao_insert(); + return; } - ao_data_static.tick += 10; - ao_insert(); - return; - } - l = line; - for (nword = 0; nword < 64; nword++) { - words[nword] = strtok_r(l, " \t\n", &saveptr); - l = NULL; - if (words[nword] == NULL) + switch (eeprom->log_format) { +#if TELEMEGA + case AO_LOG_FORMAT_TELEMEGA_OLD: + case AO_LOG_FORMAT_TELEMEGA: + log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset]; + eeprom_offset += sizeof (*log_mega); + switch (log_mega->type) { + case AO_LOG_FLIGHT: + ao_flight_number = log_mega->u.flight.flight; + ao_flight_ground_accel = log_mega->u.flight.ground_accel; + ao_flight_started = 1; + ao_ground_pres = log_mega->u.flight.ground_pres; + ao_ground_height = ao_pa_to_altitude(ao_ground_pres); + ao_ground_accel_along = log_mega->u.flight.ground_accel_along; + ao_ground_accel_across = log_mega->u.flight.ground_accel_across; + ao_ground_accel_through = log_mega->u.flight.ground_accel_through; + ao_ground_roll = log_mega->u.flight.ground_roll; + ao_ground_pitch = log_mega->u.flight.ground_pitch; + ao_ground_yaw = log_mega->u.flight.ground_yaw; + ao_ground_mpu6000.accel_x = ao_ground_accel_across; + ao_ground_mpu6000.accel_y = ao_ground_accel_along; + ao_ground_mpu6000.accel_z = ao_ground_accel_through; + ao_ground_mpu6000.gyro_x = ao_ground_pitch >> 9; + ao_ground_mpu6000.gyro_y = ao_ground_roll >> 9; + ao_ground_mpu6000.gyro_z = ao_ground_yaw >> 9; + break; + case AO_LOG_STATE: + break; + case AO_LOG_SENSOR: + ao_data_static.tick = log_mega->tick; + ao_data_static.ms5607_raw.pres = log_mega->u.sensor.pres; + ao_data_static.ms5607_raw.temp = log_mega->u.sensor.temp; + ao_data_static.mpu6000.accel_x = log_mega->u.sensor.accel_x; + ao_data_static.mpu6000.accel_y = log_mega->u.sensor.accel_y; + ao_data_static.mpu6000.accel_z = log_mega->u.sensor.accel_z; + ao_data_static.mpu6000.gyro_x = log_mega->u.sensor.gyro_x; + ao_data_static.mpu6000.gyro_y = log_mega->u.sensor.gyro_y; + ao_data_static.mpu6000.gyro_z = log_mega->u.sensor.gyro_z; + ao_data_static.hmc5883.x = log_mega->u.sensor.mag_x; + ao_data_static.hmc5883.y = log_mega->u.sensor.mag_y; + ao_data_static.hmc5883.z = log_mega->u.sensor.mag_z; + ao_data_static.mma655x = log_mega->u.sensor.accel; + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + ao_data_static.mma655x = ao_data_accel_invert(ao_data_static.mma655x); + ao_records_read++; + ao_insert(); + return; + case AO_LOG_TEMP_VOLT: + if (pyros_fired != log_mega->u.volt.pyro) { + printf("pyro changed %x -> %x\n", pyros_fired, log_mega->u.volt.pyro); + pyros_fired = log_mega->u.volt.pyro; + } + break; + case AO_LOG_GPS_TIME: + ao_gps_prev = ao_gps_static; + ao_gps_static.tick = log_mega->tick; + ao_gps_static.latitude = log_mega->u.gps.latitude; + ao_gps_static.longitude = log_mega->u.gps.longitude; + { + int16_t altitude_low = log_mega->u.gps.altitude_low; + int16_t altitude_high = log_mega->u.gps.altitude_high; + int32_t altitude = altitude_low | ((int32_t) altitude_high << 16); + + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude); + } + ao_gps_static.flags = log_mega->u.gps.flags; + if (!ao_gps_count) + ao_gps_first = ao_gps_static; + ao_gps_count++; + break; + case AO_LOG_GPS_SAT: + break; + } break; - } -#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); +#endif +#if TELEMETRUM_V2 + case AO_LOG_FORMAT_TELEMETRUM: + log_metrum = (struct ao_log_metrum *) &eeprom->data[eeprom_offset]; + eeprom_offset += sizeof (*log_metrum); + switch (log_metrum->type) { + case AO_LOG_FLIGHT: + ao_flight_started = 1; + ao_flight_number = log_metrum->u.flight.flight; + ao_flight_ground_accel = log_metrum->u.flight.ground_accel; + ao_ground_pres = log_metrum->u.flight.ground_pres; + ao_ground_height = ao_pa_to_altitude(ao_ground_pres); + break; + case AO_LOG_SENSOR: + ao_data_static.tick = log_metrum->tick; + ao_data_static.ms5607_raw.pres = log_metrum->u.sensor.pres; + ao_data_static.ms5607_raw.temp = log_metrum->u.sensor.temp; + ao_data_static.mma655x = log_metrum->u.sensor.accel; + ao_records_read++; + ao_insert(); + return; + } 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.mpu6000.accel_x = int16(bytes, 8); - ao_data_static.mpu6000.accel_y = -int16(bytes, 10); - ao_data_static.mpu6000.accel_z = int16(bytes, 12); - ao_data_static.mpu6000.gyro_x = int16(bytes, 14); - ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); - ao_data_static.mpu6000.gyro_z = int16(bytes, 18); -#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); - - double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); - double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); - double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); - - /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes - * Z is normal to the ground, the MPU y axis - */ - set_orientation(accel_x, accel_z, accel_y, tick); - } else { - double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); - double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y); - double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z); - - update_orientation(rate_x, rate_z, rate_y, tick); +#if EASYMINI + case AO_LOG_FORMAT_EASYMINI1: + case AO_LOG_FORMAT_EASYMINI2: + case AO_LOG_FORMAT_TELEMINI3: + log_mini = (struct ao_log_mini *) &eeprom->data[eeprom_offset]; + eeprom_offset += sizeof (*log_mini); + switch (log_mini->type) { + case AO_LOG_FLIGHT: + ao_flight_started = 1; + ao_flight_number = log_mini->u.flight.flight; + ao_ground_pres = log_mini->u.flight.ground_pres; + ao_ground_height = ao_pa_to_altitude(ao_ground_pres); + break; + case AO_LOG_SENSOR: + ao_data_static.tick = log_mini->tick; + ao_data_static.ms5607_raw.pres = int24(log_mini->u.sensor.pres, 0); + ao_data_static.ms5607_raw.temp = int24(log_mini->u.sensor.temp, 0); + ao_records_read++; + ao_insert(); + return; } - 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'; - } + break; #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) { - type = 'F'; - a = atoi(words[26]); - ao_flight_started = 1; - } else { - type = 'A'; - a = atoi(words[12]); - b = atoi(words[14]); - } - } else if (nword == 3 && strcmp(words[0], "BARO") == 0) { - tick = strtol(words[1], NULL, 16); - a = 16384 - 328; - b = strtol(words[2], NULL, 10); - type = 'A'; - if (!ao_flight_started) { - ao_flight_ground_accel = 16384 - 328; - ao_config.accel_plus_g = 16384 - 328; - ao_config.accel_minus_g = 16384 + 328; - ao_flight_started = 1; - } - } else if (nword == 2 && strcmp(words[0], "TELEM") == 0) { - __xdata char *hex = words[1]; - char elt[3]; - int i, len; - uint8_t sum; - - len = strlen(hex); - if (len > sizeof (bytes) * 2) { - len = sizeof (bytes)*2; - hex[len] = '\0'; - } - for (i = 0; i < len; i += 2) { - elt[0] = hex[i]; - elt[1] = hex[i+1]; - elt[2] = '\0'; - bytes[i/2] = (uint8_t) strtol(elt, NULL, 16); - } - len = i/2; - if (bytes[0] != len - 2) { - printf ("bad length %d != %d\n", bytes[0], len - 2); - continue; - } - sum = 0x5a; - for (i = 1; i < len-1; i++) - sum += bytes[i]; - if (sum != bytes[len-1]) { - printf ("bad checksum\n"); - continue; - } - if ((bytes[len-2] & 0x80) == 0) { - continue; - } - if (len == 36) { - ao_xmemcpy(&telem, bytes + 1, 32); - tick = telem.generic.tick; - switch (telem.generic.type) { - case AO_TELEMETRY_SENSOR_TELEMETRUM: - case AO_TELEMETRY_SENSOR_TELEMINI: - case AO_TELEMETRY_SENSOR_TELENANO: - if (!ao_flight_started) { - ao_flight_ground_accel = telem.sensor.ground_accel; - ao_config.accel_plus_g = telem.sensor.accel_plus_g; - ao_config.accel_minus_g = telem.sensor.accel_minus_g; - ao_flight_started = 1; - } - type = 'A'; - a = telem.sensor.accel; - b = telem.sensor.pres; +#if TELEMETRUM_V1 + case AO_LOG_FORMAT_FULL: + case AO_LOG_FORMAT_TINY: + log_record = (struct ao_log_record *) &eeprom->data[eeprom_offset]; + eeprom_offset += sizeof (*log_record); + switch (log_record->type) { + case AO_LOG_FLIGHT: + ao_flight_started = 1; + ao_flight_ground_accel = log_record->u.flight.ground_accel; + ao_flight_number = log_record->u.flight.flight; + break; + case AO_LOG_SENSOR: + case 'P': /* ancient telemini */ + ao_data_static.tick = log_record->tick; + ao_data_static.adc.accel = log_record->u.sensor.accel; + ao_data_static.adc.pres_real = log_record->u.sensor.pres; + ao_data_static.adc.pres = log_record->u.sensor.pres; + ao_records_read++; + ao_insert(); + return; + case AO_LOG_TEMP_VOLT: + ao_data_static.tick = log_record->tick;; + ao_data_static.adc.temp = log_record->u.temp_volt.temp; + ao_data_static.adc.v_batt = log_record->u.temp_volt.v_batt; break; } - } else if (len == 99) { - ao_flight_started = 1; - 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+1, 23); - b = int16(bytes+1, 25); - } else if (len == 98) { - ao_flight_started = 1; - 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+1, 22); - b = int16(bytes+1, 24); - } else { - printf("unknown len %d\n", len); - continue; - } - } - if (type != 'F' && !ao_flight_started) - continue; - -#if MEGAMETRUM - (void) a; - (void) b; -#else - 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; + break; +#endif + default: + printf ("invalid log format %d\n", log_format); + ao_test_exit(); } - if (ao_config.main_deploy == 0) - ao_config.main_deploy = 250; - ao_flight_started = 1; - break; - case 'S': - break; - case 'A': - 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(); - return; - case 'T': - ao_data_static.tick = tick; - ao_data_static.adc.temp = a; - ao_data_static.adc.v_batt = b; - break; - case 'D': - case 'G': - case 'N': - case 'W': - case 'H': - break; } -#endif } } @@ -1053,6 +962,27 @@ void run_flight_fixed(char *name, FILE *f, int summary, char *info) emulator_in = f; emulator_info = info; ao_summary = summary; + + if (strstr(name, ".eeprom") != NULL) { + char c; + + c = getc(f); + ungetc(c, f); + if (c == '{') + eeprom = ao_eeprom_read(f); + else + eeprom = ao_eeprom_read_old(f); + + if (eeprom) { +#if HAS_MS5607 + ao_ms5607_prom = eeprom->ms5607_prom; +#endif + ao_config = eeprom->config; + ao_serial_number = eeprom->serial_number; + log_format = eeprom->log_format; + } + } + ao_flight_init(); ao_flight(); } @@ -1070,7 +1000,7 @@ main (int argc, char **argv) #else emulator_app="baro"; #endif - while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) { + while ((c = getopt_long(argc, argv, "sdpi:", options, NULL)) != -1) { switch (c) { case 's': summary = 1; @@ -1078,6 +1008,11 @@ main (int argc, char **argv) case 'd': ao_flight_debug = 1; break; + case 'p': +#if PYRO_DBG + pyro_dbg = 1; +#endif + break; case 'i': info = optarg; break;