X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftest%2Fao_flight_test.c;h=faf31aa75c0a18290c0920527b4d13784bbbde96;hb=224a1e01bacb7db0076129906ed58e1c785e1b14;hp=acdf4d926a3ead68460fd6b284d2e656a30ebf8f;hpb=a4a841828924ee37f5201d4ff0aec38459f2d802;p=fw%2Faltos diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index acdf4d92..faf31aa7 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -35,7 +36,7 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) -#if MEGAMETRUM +#if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 #define HAS_MPU6000 1 @@ -137,6 +138,18 @@ int tick_offset; static int32_t ao_k_height; +int16_t +ao_time(void) +{ + return ao_data_static.tick; +} + +void +ao_delay(int16_t interval) +{ + return; +} + void ao_ignite(enum ao_igniter igniter) { @@ -195,11 +208,13 @@ struct ao_cmds { #define ao_xmemcmp(d,s,c) memcmp(d,s,c) #define AO_NEED_ALTITUDE_TO_PRES 1 -#if MEGAMETRUM +#if TELEMEGA #include "ao_convert_pa.c" #include struct ao_ms5607_prom ms5607_prom; #include "ao_ms5607_convert.c" +#define AO_PYRO_NUM 4 +#include #else #include "ao_convert.c" #endif @@ -210,6 +225,9 @@ struct ao_config { int16_t accel_minus_g; uint8_t pad_orientation; uint16_t apogee_lockout; +#if TELEMEGA + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ +#endif }; #define AO_PAD_ORIENTATION_ANTENNA_UP 0 @@ -236,12 +254,32 @@ 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 + +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 #define to_double(f) ((f) / 65536.0) @@ -309,7 +347,7 @@ ao_mpu6000_accel(int16_t sensor) } static double -ao_mpu6000_gyro(int16_t sensor) +ao_mpu6000_gyro(int32_t sensor) { return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; } @@ -329,7 +367,7 @@ ao_insert(void) #else double accel = 0.0; #endif -#if MEGAMETRUM +#if TELEMEGA double height; ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); @@ -369,14 +407,18 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " -#if MEGAMETRUM +#if TELEMEGA + "roll %8.3f angle %8.3f qangle %8.3f " "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " #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", time, height, accel, -#if MEGAMETRUM +#if TELEMEGA + ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, + ao_mpu6000_gyro(ao_sample_angle) / 100.0, + ao_sample_qangle, 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), @@ -547,7 +589,7 @@ int32(uint8_t *bytes, int off) static int log_format; -#if MEGAMETRUM +#if TELEMEGA static double ao_vec_norm(double x, double y, double z) @@ -715,12 +757,14 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) 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; +#if 0 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); +#endif ao_orient.q0 += q_dot.q0 * dt; ao_orient.q1 += q_dot.q1 * dt; @@ -731,6 +775,8 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) ao_quat_rot(&ao_current, &ao_up, &ao_orient); + ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2)); +#if 0 printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", ao_orient.q0, ao_orient.q1, @@ -740,6 +786,7 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) ao_current.q1, ao_current.q2, ao_current.q3); +#endif } #endif @@ -758,10 +805,14 @@ ao_sleep(void *wchan) 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 ao_data_static.adc.accel = ao_flight_ground_accel; @@ -787,8 +838,8 @@ ao_sleep(void *wchan) if (words[nword] == NULL) break; } -#if MEGAMETRUM - if (log_format == AO_LOG_FORMAT_MEGAMETRUM && nword == 30 && strlen(words[0]) == 1) { +#if TELEMEGA + if (log_format == AO_LOG_FORMAT_TELEMEGA && nword == 30 && strlen(words[0]) == 1) { int i; struct ao_ms5607_value value; @@ -845,7 +896,7 @@ ao_sleep(void *wchan) 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); + update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); } ao_records_read++; ao_insert(); @@ -870,9 +921,26 @@ ao_sleep(void *wchan) else if (strcmp(words[1], "crc:") == 0) ms5607_prom.crc = strtoul(words[2], NULL, 10); continue; + } else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) { + int p = strtoul(words[1], NULL, 10); + int i, j; + struct ao_pyro *pyro = &ao_config.pyro[p]; + + for (i = 2; i < nword; i++) { + for (j = 0; j < NUM_PYRO_VALUES; j++) + if (!strcmp (words[2], ao_pyro_values[j].name)) + break; + if (j == NUM_PYRO_VALUES) + continue; + pyro->flags |= ao_pyro_values[j].flag; + if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) { + int16_t val = strtoul(words[++i], NULL, 10); + *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val; + } + } } #else - if (nword == 4 && log_format != AO_LOG_FORMAT_MEGAMETRUM) { + if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { type = words[0][0]; tick = strtoul(words[1], NULL, 16); a = strtoul(words[2], NULL, 16); @@ -989,7 +1057,7 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; -#if MEGAMETRUM +#if TELEMEGA (void) a; (void) b; #else