From 351d53836e201834a2d89773a08ab7c2dab2b2f4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 25 Oct 2013 04:34:16 -0700 Subject: [PATCH] altos: Calibrate IMU accelerometers too Average the IMU accelerometer values pointing up and down so that we have a zero-g offset for all three axes. This can then be used to compute which direction the rocket is pointing while sitting on the pad. Signed-off-by: Keith Packard --- src/core/ao.h | 7 ++++- src/core/ao_config.c | 61 +++++++++++++++++++++++++++++++++++++++ src/core/ao_sample.c | 15 ++++------ src/test/ao_flight_test.c | 10 +++++++ 4 files changed, 83 insertions(+), 10 deletions(-) diff --git a/src/core/ao.h b/src/core/ao.h index ea37885e..d12f13a0 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -739,7 +739,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 14 +#define AO_CONFIG_MINOR 15 #define AO_AES_LEN 16 @@ -773,6 +773,11 @@ struct ao_config { #if HAS_RADIO_AMP uint8_t radio_amp; /* minor version 14 */ #endif +#if HAS_GYRO + uint16_t accel_zero_along; /* minor version 15 */ + uint16_t accel_zero_across; /* minor version 15 */ + uint16_t accel_zero_through; /* minor version 15 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/core/ao_config.c b/src/core/ao_config.c index b480e14c..82faf32b 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -155,6 +155,19 @@ _ao_config_get(void) #if HAS_RADIO_AMP if (minor < 14) ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; +#endif +#if HAS_GYRO + if (minor < 15) { + ao_config.accel_zero_along = 0; + ao_config.accel_zero_across = 0; + ao_config.accel_zero_through = 0; + + /* Reset the main accel offsets to force + * re-calibration + */ + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } #endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; @@ -275,17 +288,34 @@ ao_config_accel_calibrate_show(void) __reentrant { printf("Accel cal +1g: %d -1g: %d\n", ao_config.accel_plus_g, ao_config.accel_minus_g); +#if HAS_GYRO + printf ("IMU cal along %d across %d through %d\n", + ao_config.accel_zero_along, + ao_config.accel_zero_across, + ao_config.accel_zero_through); +#endif } #define ACCEL_CALIBRATE_SAMPLES 1024 #define ACCEL_CALIBRATE_SHIFT 10 +#if HAS_GYRO +static int16_t accel_cal_along; +static int16_t accel_cal_across; +static int16_t accel_cal_through; +#endif + static int16_t ao_config_accel_calibrate_auto(char *orientation) __reentrant { uint16_t i; int32_t accel_total; uint8_t cal_data_ring; +#if HAS_GYRO + int32_t accel_along_total = 0; + int32_t accel_across_total = 0; + int32_t accel_through_total = 0; +#endif printf("Orient antenna %s and press a key...", orientation); flush(); @@ -299,10 +329,20 @@ ao_config_accel_calibrate_auto(char *orientation) __reentrant ao_sleep(DATA_TO_XDATA(&ao_sample_data)); while (i && cal_data_ring != ao_sample_data) { accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); +#if HAS_GYRO + accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); + accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); + accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); +#endif cal_data_ring = ao_data_ring_next(cal_data_ring); i--; } } +#if HAS_GYRO + accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; +#endif return accel_total >> ACCEL_CALIBRATE_SHIFT; } @@ -310,12 +350,28 @@ void ao_config_accel_calibrate_set(void) __reentrant { int16_t up, down; +#if HAS_GYRO + int16_t accel_along_up, accel_along_down; + int16_t accel_across_up, accel_across_down; + int16_t accel_through_up, accel_through_down; +#endif + ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; if (ao_cmd_lex_i == 0) { up = ao_config_accel_calibrate_auto("up"); +#if HAS_GYRO + accel_along_up = accel_cal_along; + accel_across_up = accel_cal_across; + accel_through_up = accel_cal_through; +#endif down = ao_config_accel_calibrate_auto("down"); +#if HAS_GYRO + accel_along_down = accel_cal_along; + accel_across_down = accel_cal_across; + accel_through_down = accel_cal_through; +#endif } else { up = ao_cmd_lex_i; ao_cmd_decimal(); @@ -331,6 +387,11 @@ ao_config_accel_calibrate_set(void) __reentrant _ao_config_edit_start(); ao_config.accel_plus_g = up; ao_config.accel_minus_g = down; +#if HAS_GYRO + ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; + ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; + ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; +#endif _ao_config_edit_finish(); } #endif /* HAS_ACCEL */ diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 676e0ffd..a9d50cb2 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -139,19 +139,16 @@ ao_sample_preflight_set(void) /* No rotation yet */ ao_quaternion_init_zero_rotation(&ao_rotation); - /* XXX Assume we're pointing straight up for now */ + /* Take the pad IMU acceleration values and compute our current direction + */ ao_quaternion_init_vector(&ao_pad_orientation, - ao_ground_accel_across, - ao_ground_accel_through, - -ao_ground_accel_along); + ao_ground_accel_across - ao_config.accel_zero_across, + ao_ground_accel_through - ao_config.accel_zero_through, + -ao_ground_accel_along - ao_config.accel_zero_along); + ao_quaternion_normalize(&ao_pad_orientation, &ao_pad_orientation); - printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n", - ao_pad_orientation.r, - ao_pad_orientation.x, - ao_pad_orientation.y, - ao_pad_orientation.z); #endif nsamples = 0; } diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index e2f63e34..7f18c80e 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -234,6 +234,9 @@ struct ao_config { 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 }; @@ -719,6 +722,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 && -- 2.30.2