From 5f6b3790667d9b92370b4fe0dad5626929fea2ba Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 30 Nov 2012 20:51:47 -0800 Subject: [PATCH] altos: Make skytraq reflashing code try both 9600 and 4800 baud This lets it communicate with the ROM code which boots at 4800 baud instead of 9600 baud. Signed-off-by: Keith Packard --- ao-tools/Makefile.am | 2 +- ao-tools/lib/Makefile.am | 1 + ao-tools/lib/cc.h | 116 +++++++++++++++++++ configure.ac | 1 + src/drivers/ao_gps_skytraq.c | 6 + src/test/Makefile | 2 +- src/test/ao_flight_test.c | 213 +++++++++++++++++++++++++++++++++++ 7 files changed, 339 insertions(+), 2 deletions(-) diff --git a/ao-tools/Makefile.am b/ao-tools/Makefile.am index 871b8205..4cf1f382 100644 --- a/ao-tools/Makefile.am +++ b/ao-tools/Makefile.am @@ -1 +1 @@ -SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash +SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash ao-mega diff --git a/ao-tools/lib/Makefile.am b/ao-tools/lib/Makefile.am index 1f8f2e42..a664d083 100644 --- a/ao-tools/lib/Makefile.am +++ b/ao-tools/lib/Makefile.am @@ -34,6 +34,7 @@ libao_tools_a_SOURCES = \ cc-telem.c \ cc-telemetry.c \ cc-telemetry.h \ + cc-mega.c \ cp-usb-async.c \ cp-usb-async.h \ i0.c \ diff --git a/ao-tools/lib/cc.h b/ao-tools/lib/cc.h index 6257ee44..625540bb 100644 --- a/ao-tools/lib/cc.h +++ b/ao-tools/lib/cc.h @@ -269,6 +269,122 @@ struct cc_telem { int cc_telem_parse(const char *input_line, struct cc_telem *telem); +struct ao_log_mega { + char type; /* 0 */ + uint8_t is_config; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + } flight; /* 12 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; + uint16_t reason; + } state; + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel_x; /* 12 */ + int16_t accel_y; /* 14 */ + int16_t accel_z; /* 16 */ + int16_t gyro_x; /* 18 */ + int16_t gyro_y; /* 20 */ + int16_t gyro_z; /* 22 */ + int16_t mag_x; /* 24 */ + int16_t mag_y; /* 26 */ + int16_t mag_z; /* 28 */ + int16_t accel; /* 30 */ + } sensor; /* 32 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t v_pbatt; /* 6 */ + int16_t n_sense; /* 8 */ + int16_t sense[10]; /* 10 */ + } volt; /* 30 */ + /* AO_LOG_GPS_TIME */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + uint8_t hour; /* 14 */ + uint8_t minute; /* 15 */ + uint8_t second; /* 16 */ + uint8_t flags; /* 17 */ + uint8_t year; /* 18 */ + uint8_t month; /* 19 */ + uint8_t day; /* 20 */ + uint8_t pad; /* 21 */ + } gps; /* 22 */ + /* AO_LOG_GPS_SAT */ + struct { + uint16_t channels; /* 4 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[12]; /* 6 */ + } gps_sat; /* 30 */ + + struct { + uint32_t kind; + int32_t data[6]; + } config_int; + + struct { + uint32_t kind; + char string[24]; + } config_str; + + /* Raw bytes */ + uint8_t bytes[28]; + } u; +}; + +#define AO_CONFIG_CONFIG 1 +#define AO_CONFIG_MAIN 2 +#define AO_CONFIG_APOGEE 3 +#define AO_CONFIG_LOCKOUT 4 +#define AO_CONFIG_FREQUENCY 5 +#define AO_CONFIG_RADIO_ENABLE 6 +#define AO_CONFIG_ACCEL_CAL 7 +#define AO_CONFIG_RADIO_CAL 8 +#define AO_CONFIG_MAX_LOG 9 +#define AO_CONFIG_IGNITE_MODE 10 +#define AO_CONFIG_PAD_ORIENTATION 11 +#define AO_CONFIG_SERIAL_NUMBER 12 +#define AO_CONFIG_LOG_FORMAT 13 +#define AO_CONFIG_MS5607_RESERVED 14 +#define AO_CONFIG_MS5607_SENS 15 +#define AO_CONFIG_MS5607_OFF 16 +#define AO_CONFIG_MS5607_TCS 17 +#define AO_CONFIG_MS5607_TCO 18 +#define AO_CONFIG_MS5607_TREF 19 +#define AO_CONFIG_MS5607_TEMPSENS 20 +#define AO_CONFIG_MS5607_CRC 21 + + +#define AO_LOG_FLIGHT 'F' +#define AO_LOG_SENSOR 'A' +#define AO_LOG_TEMP_VOLT 'T' +#define AO_LOG_DEPLOY 'D' +#define AO_LOG_STATE 'S' +#define AO_LOG_GPS_TIME 'G' +#define AO_LOG_GPS_LAT 'N' +#define AO_LOG_GPS_LON 'W' +#define AO_LOG_GPS_ALT 'H' +#define AO_LOG_GPS_SAT 'V' +#define AO_LOG_GPS_DATE 'Y' + +#define AO_LOG_CONFIG 'c' + +int +cc_mega_parse(const char *input_line, struct ao_log_mega *l); + #ifndef TRUE #define TRUE 1 #define FALSE 0 diff --git a/configure.ac b/configure.ac index 0fcd97e2..ad10ac3c 100644 --- a/configure.ac +++ b/configure.ac @@ -164,6 +164,7 @@ ao-tools/ao-telem/Makefile ao-tools/ao-stmload/Makefile ao-tools/ao-send-telem/Makefile ao-tools/ao-sky-flash/Makefile +ao-tools/ao-mega/Makefile ao-utils/Makefile src/Version ]) 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': -- 2.30.2