X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=804f1f94bcc6beba2a029ec8398ba1e27f036fde;hb=196ccb59f53eaa9a10752498f040961e5cddf908;hp=298848d6a46b4a7921bd8fddb9ba4605020e3e25;hpb=6e4c104c28952f2010c7b1cbc9ce59e148ab2d8f;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 298848d6..804f1f94 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -25,6 +25,7 @@ #include #include #include +#define log ao_log_data #define GRAVITY 9.80665 @@ -46,7 +47,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 @@ -118,17 +119,41 @@ struct ao_adc { #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 +255,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 +265,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 +328,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 +342,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,10 +355,6 @@ 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" @@ -361,24 +378,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; @@ -390,6 +408,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 @@ -437,10 +456,6 @@ static uint16_t pyros_fired; static struct ao_mpu6000_sample ao_ground_mpu6000; #endif -#if HAS_ACCEL -int ao_error_h_sq_avg; -#endif - void ao_test_exit(void) { @@ -503,7 +518,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; @@ -514,13 +529,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 @@ -536,7 +558,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) { @@ -652,17 +675,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, @@ -682,7 +706,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 @@ -754,15 +779,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 @@ -930,6 +961,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); @@ -998,7 +1086,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; @@ -1006,6 +1094,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;