X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=d12652b49617126a5d9177877f800c5886f84b4a;hp=0913e7ba96a3a99f4498f6a0c1820d215e232a1e;hb=HEAD;hpb=6023ff81f1bbd240169b9548209133d3b02d475f diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 0913e7ba..e49035b5 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -19,6 +19,7 @@ #define _GNU_SOURCE #include +#include #include #include #include @@ -27,6 +28,20 @@ #include #define log ao_log_data +#define AO_TICK_TYPE uint32_t +#define AO_TICK_SIGNED int32_t + +typedef int32_t pres_t; +#define pres_to_altitude(p) ao_pa_to_altitude(p) +#define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) +#define ao_data_pres(packet) ((packet)->ms5607_cooked.pres) +#define AO_ADC_MAX 4095 +#define AO_PYRO_BATTERY_DIV_PLUS 100 +#define AO_PYRO_BATTERY_DIV_MINUS 27 +#define AO_IGNITE_DIV_PLUS 100 +#define AO_IGNITE_DIV_MINUS 27 +#define AO_ADC_REFERENCE_DV 33 + #define GRAVITY 9.80665 #define AO_HERTZ 100 @@ -47,7 +62,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) && !defined(TELEMEGA_V4) #define TELEMETRUM_V1 1 #endif @@ -58,6 +73,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 @@ -69,12 +85,56 @@ struct ao_adc { }; #endif +#if TELEMEGA_V4 +#define AO_ADC_NUM_SENSE 6 +#define HAS_MS5607 1 +#define HAS_BMX160 1 +#define HAS_ADXL375 1 +#define AO_ADXL375_INVERT 1 +#define AO_ADXL375_AXIS x +#define HAS_BEEP 1 +#define HAS_BARO 1 +#define AO_CONFIG_MAX_SIZE 1024 + +struct ao_adc { + int16_t sense[AO_ADC_NUM_SENSE]; + int16_t v_batt; + int16_t v_pbatt; + int16_t temp; +}; + +#define ao_data_along(packet) ((packet)->bmx160.acc_x) +#define ao_data_across(packet) (-(packet)->bmx160.acc_y) +#define ao_data_through(packet) ((packet)->bmx160.acc_z) + +#define ao_data_roll(packet) ((packet)->bmx160.gyr_x) +#define ao_data_pitch(packet) (-(packet)->bmx160.gyr_y) +#define ao_data_yaw(packet) ((packet)->bmx160.gyr_z) + +#define ao_data_mag_along(packet) ((packet)->bmx160.mag_x) +#define ao_data_mag_across(packet) ((packet)->bmx160.mag_y) +#define ao_data_mag_through(packet) ((packet)->bmx160.mag_z) + +#define ao_data_set_along(packet,v) ((packet)->bmx160.acc_x = (v)) +#define ao_data_set_across(packet,v) ((packet)->bmx160.acc_y = -(v)) +#define ao_data_set_through(packet,v) ((packet)->bmx160.acc_z = (v)) + +#define ao_data_set_roll(packet,v) ((packet)->bmx160.gyr_x = (v)) +#define ao_data_set_pitch(packet,v) ((packet)->bmx160.gyr_y = -(v)) +#define ao_data_set_yaw(packet,v) ((packet)->bmx160.gyr_z = (v)) + +#define ao_data_set_mag_along(packet,v) ((packet)->bmx160.mag_x = (v)) +#define ao_data_set_mag_across(packet,v) ((packet)->bmx160.mag_y = (v)) +#define ao_data_set_mag_through(packet,v) ((packet)->bmx160.mag_z = (v)) +#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 HAS_BARO 1 #define AO_CONFIG_MAX_SIZE 1024 struct ao_adc { @@ -116,26 +176,58 @@ struct ao_adc { #define HAS_ACCEL 1 #define HAS_ACCEL_REF 0 #endif +#define HAS_BARO 1 + +#endif + +#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 __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 +AO_TICK_TYPE +ao_time(void); + +void +ao_dump_state(void); + +#define ao_tick_count (ao_time()) +#define ao_wakeup(wchan) ao_dump_state() + +enum ao_igniter_status { + ao_igniter_unknown, /* unknown status (ambiguous voltage) */ + ao_igniter_ready, /* continuity detected */ + ao_igniter_active, /* igniter firing */ + ao_igniter_open, /* open circuit detected */ +}; + #include #include #include #include -#if TELEMEGA +#if TELEMEGA || TELEMEGA_V4 int ao_gps_count; struct ao_telemetry_location ao_gps_first; struct ao_telemetry_location ao_gps_prev; @@ -231,8 +323,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; @@ -241,7 +333,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) @@ -262,12 +353,12 @@ double drogue_time; int main_height; double main_time; -int tick_offset; +uint32_t tick_offset; static ao_k_t ao_k_height; static double simple_speed; -int16_t +AO_TICK_TYPE ao_time(void) { return ao_data_static.tick; @@ -319,9 +410,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); @@ -335,17 +423,13 @@ 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 +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI || TELEMEGA_V4 #include "ao_convert_pa.c" #include struct ao_ms5607_prom ao_ms5607_prom; #include "ao_ms5607_convert.c" -#if TELEMEGA +#if TELEMEGA || TELEMEGA_V4 #define AO_PYRO_NUM 4 #include #endif @@ -362,36 +446,39 @@ struct ao_ms5607_prom ao_ms5607_prom; struct ao_config ao_config; -#define 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; -int16_t ao_flight_number; +uint16_t ao_flight_number; -extern uint16_t ao_sample_tick; +extern AO_TICK_TYPE 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; -int ao_sample_prev_tick; -uint16_t prev_tick; +AO_TICK_TYPE ao_sample_prev_tick; +AO_TICK_TYPE prev_tick; +AO_TICK_TYPE start_tick; #include "ao_kalman.c" #include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" -#if TELEMEGA +#include "ao_data.c" +#if TELEMEGA || TELEMEGA_V4 #define AO_PYRO_NUM 4 #define AO_PYRO_0 0 @@ -404,7 +491,6 @@ uint16_t prev_tick; static void ao_pyro_pin_set(uint8_t pin, uint8_t value) { - printf ("set pyro %d %d\n", pin, value); } #include "ao_pyro.c" @@ -437,6 +523,9 @@ static uint16_t pyros_fired; #if HAS_MPU6000 static struct ao_mpu6000_sample ao_ground_mpu6000; #endif +#if HAS_BMX160 +static struct ao_bmx160_sample ao_ground_bmx160; +#endif void ao_test_exit(void) @@ -475,7 +564,7 @@ ao_test_exit(void) exit(0); } -#ifdef TELEMEGA +#if TELEMEGA || TELEMEGA_V4 struct ao_azel { int az; int el; @@ -507,9 +596,9 @@ ao_insert(void) #endif (void) accel; - if (!tick_offset) - tick_offset = -ao_data_static.tick; - if ((prev_tick - ao_data_static.tick) > 0x400) + if (!start_tick) + start_tick = ao_data_static.tick; + if ((AO_TICK_SIGNED) (prev_tick - ao_data_static.tick) > 0x400) tick_offset += 65536; if (prev_tick) { int ticks = ao_data_static.tick - prev_tick; @@ -518,11 +607,13 @@ ao_insert(void) simple_speed += accel * ticks / 100.0; } prev_tick = ao_data_static.tick; - time = (double) (ao_data_static.tick + tick_offset) / 100; + time = (double) (ao_data_static.tick + tick_offset - start_tick) / 100; -#if TELEMEGA || TELEMETRUM_V2 || EASYMINI + double height = 0; +#if HAS_BARO +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI || TELEMEGA_V4 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 @@ -538,7 +629,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) { @@ -564,7 +656,7 @@ ao_insert(void) } if (!ao_summary) { -#if TELEMEGA +#if TELEMEGA || TELEMEGA_V4 static struct ao_quaternion ao_ground_mag; static int ao_ground_mag_set; @@ -655,9 +747,10 @@ ao_insert(void) #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 +#if TELEMEGA || TELEMEGA_V4 " 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 pyro %d inhibited %d" #endif "\n", time, @@ -684,7 +777,27 @@ 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, + ao_pyro_fired * 10, + ao_pyro_inhibited * 10 +#endif +#if TELEMEGA_V4 + , ao_sample_orient, + + ao_bmx160_accel(ao_data_static.bmx160.acc_x), + ao_bmx160_accel(ao_data_static.bmx160.acc_y), + ao_bmx160_accel(ao_data_static.bmx160.acc_z), + ao_bmx160_gyro(ao_data_static.bmx160.gyr_x - ao_ground_bmx160.gyr_x), + ao_bmx160_gyro(ao_data_static.bmx160.gyr_y - ao_ground_bmx160.gyr_y), + ao_bmx160_gyro(ao_data_static.bmx160.gyr_z - ao_ground_bmx160.gyr_z), + ao_data_static.bmx160.mag_x, + ao_data_static.bmx160.mag_y, + ao_data_static.bmx160.mag_z, + ao_mag_angle, + ao_coast_avg_accel / 16.0, + ao_pyro_fired * 10, + ao_pyro_inhibited * 10 #endif ); #endif @@ -742,7 +855,7 @@ void ao_sleep(void *wchan) { if (wchan == &ao_data_head) { -#if TELEMEGA +#if TELEMEGA || TELEMEGA_V4 if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed) ao_pyro_check(); #endif @@ -753,18 +866,27 @@ ao_sleep(void *wchan) #if TELEMEGA ao_data_static.mpu6000 = ao_ground_mpu6000; #endif +#if TELEMEGA_V4 + ao_data_static.bmx160 = ao_ground_bmx160; +#endif #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 || TELEMEGA_V4 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 @@ -859,6 +981,102 @@ ao_sleep(void *wchan) } break; #endif +#if TELEMEGA_V4 + 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; + 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_bmx160.acc_x = ao_ground_accel_along; + ao_ground_bmx160.acc_y = -ao_ground_accel_across; + ao_ground_bmx160.acc_z = ao_ground_accel_through; + ao_ground_bmx160.gyr_x = ao_ground_roll >> 9; + ao_ground_bmx160.gyr_y = -(ao_ground_pitch >> 9); + ao_ground_bmx160.gyr_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_set_along(&ao_data_static, log_mega->u.sensor.accel_along); + ao_data_set_across(&ao_data_static, log_mega->u.sensor.accel_across); + ao_data_set_through(&ao_data_static, log_mega->u.sensor.accel_through); + ao_data_set_roll(&ao_data_static, log_mega->u.sensor.gyro_roll); + ao_data_set_pitch(&ao_data_static, log_mega->u.sensor.gyro_pitch); + ao_data_set_yaw(&ao_data_static, log_mega->u.sensor.gyro_yaw); + ao_data_set_mag_along(&ao_data_static, log_mega->u.sensor.mag_along); + ao_data_set_mag_across(&ao_data_static, log_mega->u.sensor.mag_across); + ao_data_set_mag_through(&ao_data_static, log_mega->u.sensor.mag_through); +#if AO_ADXL375_INVERT + ao_data_static.adxl375.AO_ADXL375_AXIS = ao_data_accel_invert(log_mega->u.sensor.accel); +#else + ao_data_static.adxl375.AO_ADXL375_AXIS = log_mega->u.sensor.accel; +#endif + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + ao_data_static.adxl375.AO_ADXL375_AXIS = ao_data_accel_invert(ao_data_static.adxl375.AO_ADXL375_AXIS); + 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; +#endif +#ifdef foo_TELEMEGA_V4 + 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; +#endif #if TELEMETRUM_V2 case AO_LOG_FORMAT_TELEMETRUM: log_metrum = (struct ao_log_metrum *) &eeprom->data[eeprom_offset]; @@ -932,6 +1150,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);