From 860d0526737295c695f8e6a790d72b49eb4a686d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 30 Nov 2012 16:10:43 -0800 Subject: altos: Add support for reflashing skytraq GPS chips This simply switches the skytraq port to 115200 baud and then essentially connects it directly to the USB port by forwarding bytes in both directions. Once started, the only way out is to reboot the board. Signed-off-by: Keith Packard --- src/drivers/ao_gps_skytraq.c | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) (limited to 'src/drivers/ao_gps_skytraq.c') diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d80da97c..b7dc0a86 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -21,6 +21,7 @@ #ifndef ao_gps_getchar #define ao_gps_getchar ao_serial1_getchar +#define ao_gps_fifo ao_serial1_rx_fifo #endif #ifndef ao_gps_putchar @@ -453,6 +454,8 @@ ao_gps_nmea_parse(void) } } +static uint8_t ao_gps_updating; + void ao_gps(void) __reentrant { @@ -468,6 +471,13 @@ ao_gps(void) __reentrant if (ao_gps_getchar() == '$') { ao_gps_nmea_parse(); } +#ifndef AO_GPS_TEST + while (ao_gps_updating) { + ao_usb_putchar(ao_gps_getchar()); + if (ao_fifo_empty(ao_gps_fifo)) + flush(); + } +#endif } } @@ -492,8 +502,29 @@ gps_dump(void) __reentrant ao_mutex_put(&ao_gps_mutex); } +static __code uint8_t ao_gps_115200[] = { + SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ +}; + +static void +gps_update(void) __reentrant +{ + ao_gps_updating = 1; + ao_task_minimize_latency = 1; +#if HAS_ADC + ao_timer_set_adc_interval(0); +#endif + ao_skytraq_sendstruct(ao_gps_115200); + ao_gps_set_speed(AO_SERIAL_SPEED_115200); + + /* It's a binary protocol; abandon attempts to escape */ + for (;;) + ao_gps_putchar(ao_usb_getchar()); +} + __code struct ao_cmds ao_gps_cmds[] = { { gps_dump, "g\0Display GPS" }, + { gps_update, "U\0Update GPS firmware" }, { 0, NULL }, }; -- cgit v1.2.3 From 5f6b3790667d9b92370b4fe0dad5626929fea2ba Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 30 Nov 2012 20:51:47 -0800 Subject: 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(-) (limited to 'src/drivers/ao_gps_skytraq.c') 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': -- cgit v1.2.3 From ce12787b56f699166cafe4cdee9e2a4d8e66ebed Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 4 Dec 2012 09:45:01 -0800 Subject: altos: Break out GPS speed resetting sequence To set the GPS speed, we delay for 1/2 sec, change speed, then delay for another 1/2 sec. Signed-off-by: Keith Packard --- src/drivers/ao_gps_skytraq.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) (limited to 'src/drivers/ao_gps_skytraq.c') diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d637a602..d2f67e6b 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -506,6 +506,13 @@ static __code uint8_t ao_gps_115200[] = { SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ }; +static void +ao_gps_set_speed_delay(uint8_t speed) { + ao_delay(AO_MS_TO_TICKS(500)); + ao_gps_set_speed(speed); + ao_delay(AO_MS_TO_TICKS(500)); +} + static void gps_update(void) __reentrant { @@ -515,13 +522,9 @@ 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_gps_set_speed_delay(AO_SERIAL_SPEED_4800); 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)); + ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200); /* It's a binary protocol; abandon attempts to escape */ for (;;) -- cgit v1.2.3