X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=ff480e2db9fd7ff7890ebbc46f309870c51e9c64;hp=298848d6a46b4a7921bd8fddb9ba4605020e3e25;hb=8be4e3b8faac362665641e98a1df1e1e8a681b7a;hpb=95952059bbb50b5f22460418df003a87c8bd3271 diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 298848d6..ff480e2d 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 @@ -118,17 +119,22 @@ struct ao_adc { #endif -#define __pdata -#define __data -#define __xdata -#define __code -#define __reentrant +#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 +236,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 +246,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 +309,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 +323,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 +336,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,16 +359,13 @@ 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; @@ -437,10 +432,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 +494,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,7 +505,12 @@ 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; @@ -652,17 +648,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 +679,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 @@ -998,7 +996,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 +1004,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;