X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=d12652b49617126a5d9177877f800c5886f84b4a;hb=5f77ec01ebbee33f94698fcd978ee52c8676cf50;hp=8e69730ee54e1f73322de422ef677a44d9001cd7;hpb=441056b01abcf9287f61f425cf29fc4b1603c619;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 8e69730e..d12652b4 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -19,12 +19,14 @@ #define _GNU_SOURCE #include +#include #include #include #include #include #include #include +#define log ao_log_data #define GRAVITY 9.80665 @@ -46,7 +48,7 @@ int ao_gps_new; -#if !defined(TELEMEGA) && !defined(TELEMETRUM_V2) && !defined(EASYMINI) +#if !defined(TELEMEGA) && !defined(TELEMETRUM_V2) && !defined(EASYMINI) && !defined(EASYMOTOR_V_2) #define TELEMETRUM_V1 1 #endif @@ -57,6 +59,7 @@ int ao_gps_new; #define HAS_MMA655X 1 #define HAS_HMC5883 1 #define HAS_BEEP 1 +#define HAS_BARO 1 #define AO_CONFIG_MAX_SIZE 1024 #define AO_MMA655X_INVERT 0 @@ -74,6 +77,7 @@ struct ao_adc { #define HAS_MMA655X 1 #define AO_MMA655X_INVERT 0 #define HAS_BEEP 1 +#define HAS_BARO 1 #define AO_CONFIG_MAX_SIZE 1024 struct ao_adc { @@ -115,20 +119,45 @@ struct ao_adc { #define HAS_ACCEL 1 #define HAS_ACCEL_REF 0 #endif +#define HAS_BARO 1 #endif -#define __pdata -#define __data -#define __xdata -#define __code -#define __reentrant +#if EASYMOTOR_V_2 +#define AO_ADC_NUM_SENSE 0 +#define HAS_ADXL375 1 +#define HAS_BEEP 1 +#define AO_CONFIG_MAX_SIZE 1024 +#define USE_ADXL375_IMU 1 +#define AO_ADXL375_INVERT 0 +#define HAS_IMU 1 +#define AO_ADXL375_AXIS x +#define AO_ADXL375_ACROSS_AXIS y +#define AO_ADXL375_THROUGH_AXIS z + +struct ao_adc { + int16_t pressure; + int16_t v_batt; +}; + +#endif + +#define const #define HAS_FLIGHT 1 #define HAS_IGNITE 1 #define HAS_USB 1 #define HAS_GPS 1 +int16_t +ao_time(void); + +void +ao_dump_state(void); + +#define ao_tick_count (ao_time()) +#define ao_wakeup(wchan) ao_dump_state() + #include #include #include @@ -230,8 +259,8 @@ ao_gps_angle(void) extern enum ao_flight_state ao_flight_state; -#define FALSE 0 -#define TRUE 1 +#define false 0 +#define true 1 volatile struct ao_data ao_data_ring[AO_DATA_RING]; volatile uint8_t ao_data_head; @@ -240,7 +269,6 @@ int ao_summary = 0; #define ao_led_on(l) #define ao_led_off(l) #define ao_timer_set_adc_interval(i) -#define ao_wakeup(wchan) ao_dump_state() #define ao_cmd_register(c) #define ao_usb_disable() #define ao_telemetry_set_interval(x) @@ -304,7 +332,7 @@ 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; @@ -318,9 +346,6 @@ char *emulator_info; double emulator_error_max = 4; double emulator_height_error_max = 20; /* noise in the baro sensor */ -void -ao_dump_state(void); - void ao_sleep(void *wchan); @@ -334,18 +359,16 @@ 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 TELEMEGA || TELEMETRUM_V2 || EASYMINI #include "ao_convert_pa.c" #include 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 @@ -359,24 +382,25 @@ struct ao_ms5607_prom ao_ms5607_prom; struct ao_config ao_config; -#define DATA_TO_XDATA(x) (x) - - 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; -uint16_t ao_flight_number; +int16_t ao_flight_number; extern uint16_t ao_sample_tick; +#if HAS_BARO extern alt_t ao_sample_height; +#endif extern accel_t ao_sample_accel; extern int32_t ao_accel_scale; +#if HAS_BARO extern alt_t ao_ground_height; extern alt_t ao_sample_alt; +#endif double ao_sample_qangle; @@ -388,6 +412,7 @@ uint16_t prev_tick; #include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" +#include "ao_data.c" #if TELEMEGA #define AO_PYRO_NUM 4 @@ -427,15 +452,14 @@ 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; #endif -#if HAS_ACCEL -int ao_error_h_sq_avg; -#endif - void ao_test_exit(void) { @@ -498,7 +522,7 @@ ao_insert(void) ao_data_ring[ao_data_head] = ao_data_static; 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; @@ -509,13 +533,20 @@ ao_insert(void) tick_offset = -ao_data_static.tick; if ((prev_tick - ao_data_static.tick) > 0x400) tick_offset += 65536; - simple_speed += accel * (ao_data_static.tick - prev_tick) / 100.0; + 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; + double height = 0; +#if HAS_BARO #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; + 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 @@ -531,7 +562,8 @@ ao_insert(void) } } #else - double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; + height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; +#endif #endif if (ao_test_max_height < height) { @@ -647,17 +679,18 @@ ao_insert(void) #if 1 printf("%7.2f height %8.2f accel %8.3f accel_speed %8.3f " - "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d" + "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 " + "avg_accel %8.3f " #endif "\n", time, height, accel, simple_speed > -100.0 ? simple_speed : -100.0, - ao_state_names[ao_flight_state], + ao_flight_state * 10, ao_k_height / 65536.0, ao_k_speed / 65536.0 / 16.0, ao_k_accel / 65536.0 / 16.0, @@ -677,7 +710,8 @@ ao_insert(void) ao_data_static.hmc5883.x, ao_data_static.hmc5883.y, ao_data_static.hmc5883.z, - ao_mag_angle + ao_mag_angle, + ao_coast_avg_accel / 16.0 #endif ); #endif @@ -749,15 +783,21 @@ ao_sleep(void *wchan) #if TELEMETRUM_V1 ao_data_static.adc.accel = ao_flight_ground_accel; #endif +#if EASYMOTOR_V_2 + ao_data_static.adxl375.AO_ADXL375_AXIS = ao_flight_ground_accel; +#endif ao_insert(); return; } if (eeprom) { -#if TELEMEGA +#if TELEMEGA || EASYMOTOR_V_2 struct ao_log_mega *log_mega; #endif +#if EASYMOTOR_V_2 + struct ao_log_motor *log_motor; +#endif #if TELEMETRUM_V2 struct ao_log_metrum *log_metrum; #endif @@ -825,6 +865,10 @@ ao_sleep(void *wchan) 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; @@ -921,6 +965,63 @@ ao_sleep(void *wchan) break; } break; +#endif +#if EASYMOTOR_V_2 + case AO_LOG_FORMAT_TELEMEGA_3: + 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; + break; + case AO_LOG_SENSOR: + ao_data_static.tick = log_mega->tick; + ao_data_static.adxl375.AO_ADXL375_AXIS = -log_mega->u.sensor.accel; + ao_records_read++; + ao_insert(); + return; + } + break; + case AO_LOG_FORMAT_TELEMEGA_4: + 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; + break; + case AO_LOG_SENSOR: + ao_data_static.tick = log_mega->tick; + ao_data_static.adxl375.AO_ADXL375_AXIS = log_mega->u.sensor.accel; + ao_records_read++; + ao_insert(); + return; + } + break; + case AO_LOG_FORMAT_EASYMOTOR: + log_motor = (struct ao_log_motor *) &eeprom->data[eeprom_offset]; + eeprom_offset += sizeof (*log_motor); + switch (log_motor->type) { + case AO_LOG_FLIGHT: + ao_flight_number = log_motor->u.flight.flight; + ao_flight_ground_accel = log_motor->u.flight.ground_accel; + ao_flight_started = 1; + break; + case AO_LOG_SENSOR: + ao_data_static.tick = log_motor->tick; + ao_data_static.adc.pressure = log_motor->u.sensor.pressure; + ao_data_static.adc.v_batt = log_motor->u.sensor.v_batt; + ao_data_static.adxl375.AO_ADXL375_AXIS = log_motor->u.sensor.accel_along; + ao_data_static.adxl375.AO_ADXL375_ACROSS_AXIS = log_motor->u.sensor.accel_across; + ao_data_static.adxl375.AO_ADXL375_THROUGH_AXIS = log_motor->u.sensor.accel_through; + ao_records_read++; + ao_insert(); + return; + } + break; #endif default: printf ("invalid log format %d\n", log_format); @@ -989,7 +1090,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; @@ -997,6 +1098,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;