X-Git-Url: https://git.gag.com/?a=blobdiff_plain;ds=sidebyside;f=src%2Ftest%2Fao_flight_test.c;h=452f5b75fea982517bc494d93fce7c74b66708c5;hb=bdd6244d8b4a55c9aa4fb79b0cb1a0727afbc2ac;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..452f5b75 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -20,10 +20,13 @@ #include #include #include +#include #include #include #include +#define GRAVITY 9.80665 + #define AO_HERTZ 100 #define HAS_ADC 1 @@ -35,11 +38,17 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) -#if MEGAMETRUM +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +int ao_gps_new; + +#if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 #define HAS_MPU6000 1 #define HAS_MMA655X 1 +#define HAS_HMC5883 1 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -120,6 +129,7 @@ int ao_summary = 0; #define ao_rdf_set(rdf) #define ao_packet_slave_start() #define ao_packet_slave_stop() +#define flush() enum ao_igniter { ao_igniter_drogue = 0, @@ -137,6 +147,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 +217,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 +234,12 @@ 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 */ + int16_t accel_zero_along; + int16_t accel_zero_across; + int16_t accel_zero_through; +#endif }; #define AO_PAD_ORIENTATION_ANTENNA_UP 0 @@ -222,7 +252,6 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; @@ -236,12 +265,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) @@ -301,17 +350,20 @@ ao_test_exit(void) exit(0); } -#if HAS_MPU6000 -static double -ao_mpu6000_accel(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; -} +#ifdef TELEMEGA +struct ao_azel { + int az; + int el; +}; -static double -ao_mpu6000_gyro(int16_t sensor) +static void +azel (struct ao_azel *r, struct ao_quaternion *q) { - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; + double v; + + r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5); + v = sqrt (q->x*q->x + q->y*q->y); + r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5); } #endif @@ -329,7 +381,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); @@ -368,21 +420,104 @@ ao_insert(void) } if (!ao_summary) { +#if TELEMEGA + static struct ao_quaternion ao_ground_mag; + static int ao_ground_mag_set; + + if (!ao_ground_mag_set) { + ao_quaternion_init_vector (&ao_ground_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag); + ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation); + ao_ground_mag_set = 1; + } + + struct ao_quaternion ao_mag, ao_mag_rot; + + ao_quaternion_init_vector(&ao_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + + ao_quaternion_normalize(&ao_mag, &ao_mag); + ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation); + + float ao_dot; + int ao_mag_angle; + + ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag); + + struct ao_azel ground_azel, mag_azel, rot_azel; + + azel(&ground_azel, &ao_ground_mag); + azel(&mag_azel, &ao_mag); + azel(&rot_azel, &ao_mag_rot); + + ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5); + + static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 }; + struct ao_quaternion ao_out; + + ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); + + int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); + + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + mag_azel.el, + mag_azel.az); + + +#if 0 + printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", + ao_state_names[ao_flight_state], + ground_azel.az, ground_azel.el, + mag_azel.az, mag_azel.el, + rot_azel.az, rot_azel.el, + ground_azel.el - rot_azel.el, + ground_azel.az - rot_azel.az, + ao_mag_angle, + ao_sample_orient, + ao_ground_mag.x, + ao_ground_mag.y, + ao_ground_mag.z, + ao_mag.x, + ao_mag.y, + ao_mag.z, + ao_mag_rot.x, + ao_mag_rot.y, + ao_mag_rot.z); +#endif +#endif + +#if 0 printf("%7.2f height %8.2f accel %8.3f " -#if MEGAMETRUM - "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " +#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 " #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_sample_orient, + 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), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), + ao_data_static.hmc5883.x, + ao_data_static.hmc5883.y, + ao_data_static.hmc5883.z, + ao_mag_angle, #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -392,6 +527,7 @@ ao_insert(void) drogue_height, main_height, ao_error_h_sq_avg); +#endif // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); @@ -547,202 +683,6 @@ int32(uint8_t *bytes, int off) static int log_format; -#if MEGAMETRUM - -static double -ao_vec_norm(double x, double y, double z) -{ - return x*x + y*y + z*z; -} - -static void -ao_vec_normalize(double *x, double *y, double *z) -{ - double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); - - *x *= scale; - *y *= scale; - *z *= scale; -} - -struct ao_quat { - double q0, q1, q2, q3; -}; - -static void -ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) -{ - r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; - r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; - r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; - r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; -} - -#if 0 -static void -ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) -{ - r->q0 = a->q0 * s; - r->q1 = a->q1 * s; - r->q2 = a->q2 * s; - r->q3 = a->q3 * s; -} -#endif - -static void -ao_quat_conj(struct ao_quat *r, struct ao_quat *a) -{ - r->q0 = a->q0; - r->q1 = -a->q1; - r->q2 = -a->q2; - r->q3 = -a->q3; -} - -static void -ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) -{ - struct ao_quat t; - struct ao_quat c; - ao_quat_mul(&t, q, a); - ao_quat_conj(&c, q); - ao_quat_mul(r, &t, &c); -} - -static void -ao_quat_from_angle(struct ao_quat *r, - double x_rad, - double y_rad, - double z_rad) -{ - double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); - double s = sin(angle/2); - double c = cos(angle/2); - - r->q0 = c; - r->q1 = x_rad * s / angle; - r->q2 = y_rad * s / angle; - r->q3 = z_rad * s / angle; -} - -static void -ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) -{ - ao_vec_normalize(&x, &y, &z); - double x_rad = atan2(z, y); - double y_rad = atan2(x, z); - double z_rad = atan2(y, x); - - ao_quat_from_angle(r, x_rad, y_rad, z_rad); -} - -static double -ao_quat_norm(struct ao_quat *a) -{ - return (a->q0 * a->q0 + - a->q1 * a->q1 + - a->q2 * a->q2 + - a->q3 * a->q3); -} - -static void -ao_quat_normalize(struct ao_quat *a) -{ - double norm = ao_quat_norm(a); - - if (norm) { - double m = 1/sqrt(norm); - - a->q0 *= m; - a->q1 *= m; - a->q2 *= m; - a->q3 *= m; - } -} - -static struct ao_quat ao_up, ao_current; -static struct ao_quat ao_orient; -static int ao_orient_tick; - -void -set_orientation(double x, double y, double z, int tick) -{ - struct ao_quat t; - - printf ("set_orientation %g %g %g\n", x, y, z); - ao_quat_from_vector(&ao_orient, x, y, z); - ao_up.q1 = ao_up.q2 = 0; - ao_up.q0 = ao_up.q3 = sqrt(2)/2; - ao_orient_tick = tick; - - ao_orient.q0 = 1; - ao_orient.q1 = 0; - ao_orient.q2 = 0; - ao_orient.q3 = 0; - - printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_up.q0, - ao_up.q1, - ao_up.q2, - ao_up.q3); - - ao_quat_rot(&t, &ao_up, &ao_orient); - printf ("pad orient (%g) %g %g %g\n", - t.q0, - t.q1, - t.q2, - t.q3); - -} - -void -update_orientation (double rate_x, double rate_y, double rate_z, int tick) -{ - struct ao_quat q_dot; - double lambda; - double dt = (tick - ao_orient_tick) / 100.0; - - ao_orient_tick = tick; - -// lambda = 1 - ao_quat_norm(&ao_orient); - lambda = 0; - - q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; - q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; - 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; - - 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); - - ao_orient.q0 += q_dot.q0 * dt; - ao_orient.q1 += q_dot.q1 * dt; - ao_orient.q2 += q_dot.q2 * dt; - ao_orient.q3 += q_dot.q3 * dt; - - ao_quat_normalize(&ao_orient); - - ao_quat_rot(&ao_current, &ao_up, &ao_orient); - - printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_current.q0, - ao_current.q1, - ao_current.q2, - ao_current.q3); -} -#endif - void ao_sleep(void *wchan) { @@ -758,10 +698,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 +731,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; @@ -813,40 +757,17 @@ ao_sleep(void *wchan) ao_data_static.ms5607_raw.temp = int32(bytes, 4); ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); ao_data_static.mpu6000.accel_x = int16(bytes, 8); - ao_data_static.mpu6000.accel_y = -int16(bytes, 10); + ao_data_static.mpu6000.accel_y = int16(bytes, 10); ao_data_static.mpu6000.accel_z = int16(bytes, 12); ao_data_static.mpu6000.gyro_x = int16(bytes, 14); - ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); + ao_data_static.mpu6000.gyro_y = int16(bytes, 16); ao_data_static.mpu6000.gyro_z = int16(bytes, 18); + ao_data_static.hmc5883.x = int16(bytes, 20); + ao_data_static.hmc5883.y = int16(bytes, 22); + ao_data_static.hmc5883.z = int16(bytes, 24); #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif - if (ao_records_read == 0) - ao_ground_mpu6000 = ao_data_static.mpu6000; - else if (ao_records_read < 10) { -#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) - f(accel_x); - f(accel_y); - f(accel_z); - f(gyro_x); - f(gyro_y); - f(gyro_z); - - double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); - double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); - double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); - - /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes - * Z is normal to the ground, the MPU y axis - */ - set_orientation(accel_x, accel_z, accel_y, tick); - } else { - double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); - 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); - } ao_records_read++; ao_insert(); return; @@ -870,9 +791,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); @@ -886,6 +824,13 @@ ao_sleep(void *wchan) } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) { ao_config.accel_plus_g = atoi(words[3]); ao_config.accel_minus_g = atoi(words[5]); +#ifdef TELEMEGA + } else if (nword >= 8 && strcmp(words[0], "IMU") == 0) { + ao_config.accel_zero_along = atoi(words[3]); + ao_config.accel_zero_across = atoi(words[5]); + ao_config.accel_zero_through = atoi(words[7]); + printf ("%d %d %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, ao_config.accel_zero_through); +#endif } else if (nword >= 4 && strcmp(words[0], "Main") == 0) { ao_config.main_deploy = atoi(words[2]); } else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 && @@ -989,7 +934,7 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; -#if MEGAMETRUM +#if TELEMEGA (void) a; (void) b; #else