diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/drivers/ao_gps_skytraq.c | 6 | ||||
| -rw-r--r-- | src/test/Makefile | 2 | ||||
| -rw-r--r-- | src/test/ao_flight_test.c | 213 |
3 files changed, 220 insertions, 1 deletions
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index b7dc0a86..d637a602 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -515,7 +515,13 @@ gps_update(void) __reentrant ao_timer_set_adc_interval(0); #endif ao_skytraq_sendstruct(ao_gps_115200); + ao_delay(AO_MS_TO_TICKS(500)); + ao_gps_set_speed(AO_SERIAL_SPEED_4800); + ao_delay(AO_MS_TO_TICKS(500)); + ao_skytraq_sendstruct(ao_gps_115200); + ao_delay(AO_MS_TO_TICKS(500)); ao_gps_set_speed(AO_SERIAL_SPEED_115200); + ao_delay(AO_MS_TO_TICKS(500)); /* It's a binary protocol; abandon attempts to escape */ for (;;) diff --git a/src/test/Makefile b/src/test/Makefile index 44cee904..0dcdc949 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -29,7 +29,7 @@ ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kal cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) - cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< + cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< -lm ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 7180f02d..acdf4d92 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -547,6 +547,202 @@ 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) { @@ -635,6 +831,21 @@ ao_sleep(void *wchan) 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(); @@ -779,6 +990,8 @@ ao_sleep(void *wchan) continue; #if MEGAMETRUM + (void) a; + (void) b; #else switch (type) { case 'F': |