From 144db05f6b286a0450d486f69ce192632a2c0656 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Nov 2009 21:38:18 -0800 Subject: [PATCH] Add two-point accelerometer calibration. Calibration now uses two values, one upside right and the other upside down instead of a single horizontal value. This allows the use of other accelerometers and compensates for variations in the divider circuit to provide more accurate data. --- debian/rules | 2 +- src/ao.h | 10 +++++--- src/ao_config.c | 61 ++++++++++++++++++++++++++++++++------------ src/ao_flight.c | 46 ++++++++++++++++----------------- src/ao_flight_test.c | 7 +++-- 5 files changed, 79 insertions(+), 47 deletions(-) diff --git a/debian/rules b/debian/rules index ac594c07..260ba773 100755 --- a/debian/rules +++ b/debian/rules @@ -9,7 +9,7 @@ DEB_VERSION := $(shell git describe | tr - +) prebuild: git-dch --release --new-version=$(DEB_VERSION) git log > ChangeLog - git commit ChangeLog debian/changelog \ + git commit -n ChangeLog debian/changelog \ -m "update changelogs for Debian build" git tag debian/$(DEB_VERSION) git push --tags origin master diff --git a/src/ao.h b/src/ao.h index 4cceefe1..22e8785f 100644 --- a/src/ao.h +++ b/src/ao.h @@ -117,8 +117,9 @@ ao_panic(uint8_t reason); */ /* Our timer runs at 100Hz */ -#define AO_MS_TO_TICKS(ms) ((ms) / 10) -#define AO_SEC_TO_TICKS(s) ((s) * 100) +#define AO_HERTZ 100 +#define AO_MS_TO_TICKS(ms) ((ms) / (1000 / AO_HERTZ)) +#define AO_SEC_TO_TICKS(s) ((s) * AO_HERTZ) /* Returns the current time in ticks */ uint16_t @@ -928,16 +929,17 @@ ao_igniter_init(void); */ #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 1 +#define AO_CONFIG_MINOR 2 struct ao_config { uint8_t major; uint8_t minor; uint16_t main_deploy; - int16_t accel_zero_g; + int16_t accel_plus_g; uint8_t radio_channel; char callsign[AO_MAX_CALLSIGN + 1]; uint8_t apogee_delay; + int16_t accel_minus_g; }; extern __xdata struct ao_config ao_config; diff --git a/src/ao_config.c b/src/ao_config.c index 021fb6f6..85fcff8c 100644 --- a/src/ao_config.c +++ b/src/ao_config.c @@ -45,7 +45,8 @@ _ao_config_get(void) ao_config.minor = AO_CONFIG_MINOR; ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY; ao_config.radio_channel = AO_CONFIG_DEFAULT_RADIO_CHANNEL; - ao_config.accel_zero_g = AO_CONFIG_DEFAULT_ACCEL_ZERO_G; + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; memset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); memcpy(&ao_config.callsign, AO_CONFIG_DEFAULT_CALLSIGN, sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); @@ -53,9 +54,14 @@ _ao_config_get(void) ao_config_dirty = 1; } if (ao_config.minor < AO_CONFIG_MINOR) { - /* Fixups for major version 1 */ + /* Fixups for mior version 1 */ if (ao_config.minor < 1) ao_config.apogee_delay = AO_CONFIG_DEFAULT_APOGEE_DELAY; + /* Fixupes for minor version 2 */ + if (ao_config.minor < 2) { + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } @@ -152,23 +158,28 @@ ao_config_main_deploy_set(void) __reentrant } void -ao_config_accel_zero_g_show(void) __reentrant +ao_config_accel_calibrate_show(void) __reentrant { - printf("Accel zero g point: %d\n", - ao_config.accel_zero_g); + printf("Accel cal +1g: %d -1g: %d\n", + ao_config.accel_plus_g, ao_config.accel_minus_g); } -#define ZERO_G_SAMPLES 1000 +#define ACCEL_CALIBRATE_SAMPLES 1024 +#define ACCEL_CALIBRATE_SHIFT 10 static int16_t -ao_config_accel_zero_g_auto(void) __reentrant +ao_config_accel_calibrate_auto(char *orientation) __reentrant { uint16_t i; int32_t accel_total; uint8_t cal_adc_ring; - puts("Calibrating accelerometer..."); flush(); - i = ZERO_G_SAMPLES; + printf("Orient %s and press a key...", orientation); + flush(); + (void) getchar(); + puts("\r\n"); flush(); + puts("Calibrating..."); flush(); + i = ACCEL_CALIBRATE_SAMPLES; accel_total = 0; cal_adc_ring = ao_adc_head; while (i) { @@ -179,22 +190,38 @@ ao_config_accel_zero_g_auto(void) __reentrant i--; } } - return (int16_t) (accel_total / ZERO_G_SAMPLES); + return accel_total >> ACCEL_CALIBRATE_SHIFT; } + void -ao_config_accel_zero_g_set(void) __reentrant +ao_config_accel_calibrate_set(void) __reentrant { + int16_t up, down; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - if (ao_cmd_lex_i == 0) - ao_cmd_lex_i = ao_config_accel_zero_g_auto(); + if (ao_cmd_lex_i == 0) { + up = ao_config_accel_calibrate_auto("antenna up"); + down = ao_config_accel_calibrate_auto("antenna down"); + } else { + up = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + down = ao_cmd_lex_i; + } + if (up >= down) { + printf("Invalid accel calibration: antenna up (%d) should be less than antenna down (%d)\n", + up, down); + return; + } ao_mutex_get(&ao_config_mutex); _ao_config_get(); - ao_config.accel_zero_g = ao_cmd_lex_i; + ao_config.accel_plus_g = up; + ao_config.accel_minus_g = down; ao_config_dirty = 1; ao_mutex_put(&ao_config_mutex); - ao_config_accel_zero_g_show(); + ao_config_accel_calibrate_show(); } void @@ -237,8 +264,8 @@ ao_config_write(void) __reentrant; __code struct ao_config_var ao_config_vars[] = { { 'm', ao_config_main_deploy_set, ao_config_main_deploy_show, "m Set height above launch for main deploy (in meters)" }, - { 'a', ao_config_accel_zero_g_set, ao_config_accel_zero_g_show, - "a Set accelerometer zero g point (0 for auto)" }, + { 'a', ao_config_accel_calibrate_set, ao_config_accel_calibrate_show, + "a <+g> <-g> Set accelerometer calibration (0 for auto)" }, { 'r', ao_config_radio_channel_set, ao_config_radio_channel_show, "r Set radio channel (freq = 434.550 + channel * .1)" }, { 'c', ao_config_callsign_set, ao_config_callsign_show, diff --git a/src/ao_flight.c b/src/ao_flight.c index e91a5daa..92c955fb 100644 --- a/src/ao_flight.c +++ b/src/ao_flight.c @@ -48,6 +48,7 @@ __pdata int16_t ao_interval_max_pres; __data uint8_t ao_flight_adc; __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres; +__pdata int16_t ao_accel_2g; /* Accelerometer calibration * @@ -72,19 +73,18 @@ __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres; #define GRAVITY 9.80665 /* convert m/s to velocity count */ -#define VEL_MPS_TO_COUNT(mps) ((int32_t) (((mps) / GRAVITY) * ACCEL_G * 100)) +#define VEL_MPS_TO_COUNT(mps) (((int32_t) (((mps) / GRAVITY) * (AO_HERTZ/2))) * (int32_t) ao_accel_2g) #define ACCEL_G 265 -#define ACCEL_ZERO_G 16000 -#define ACCEL_NOSE_UP (ACCEL_G * 2 /3) -#define ACCEL_BOOST ACCEL_G * 2 +#define ACCEL_NOSE_UP (ao_accel_2g / 4) +#define ACCEL_BOOST ao_accel_2g #define ACCEL_INT_LAND (ACCEL_G / 10) -#define ACCEL_VEL_LAND VEL_MPS_TO_COUNT(10) #define ACCEL_VEL_MACH VEL_MPS_TO_COUNT(200) -#define ACCEL_VEL_APOGEE VEL_MPS_TO_COUNT(2) -#define ACCEL_VEL_MAIN VEL_MPS_TO_COUNT(100) #define ACCEL_VEL_BOOST VEL_MPS_TO_COUNT(5) +int32_t accel_vel_mach; +int32_t accel_vel_boost; + /* * Barometer calibration * @@ -170,14 +170,14 @@ ao_flight(void) * so subtract instead of add. */ ticks = ao_flight_tick - ao_flight_prev_tick; - ao_vel_change = (((ao_raw_accel >> 1) + (ao_raw_accel_prev >> 1)) - ao_ground_accel); + ao_vel_change = ao_ground_accel - (((ao_raw_accel + 1) >> 1) + ((ao_raw_accel_prev + 1) >> 1)); ao_raw_accel_prev = ao_raw_accel; /* one is a common interval */ if (ticks == 1) - ao_flight_vel -= (int32_t) ao_vel_change; + ao_flight_vel += (int32_t) ao_vel_change; else - ao_flight_vel -= (int32_t) ao_vel_change * (int32_t) ticks; + ao_flight_vel += (int32_t) ao_vel_change * (int32_t) ticks; ao_flight_adc = ao_adc_ring_next(ao_flight_adc); } @@ -211,6 +211,9 @@ ao_flight(void) ao_min_pres = ao_ground_pres; ao_config_get(); ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy); + ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; + accel_vel_mach = ACCEL_VEL_MACH; + accel_vel_boost = ACCEL_VEL_BOOST; ao_flight_vel = 0; ao_min_vel = 0; ao_old_vel = ao_flight_vel; @@ -218,8 +221,9 @@ ao_flight(void) /* Go to pad state if the nose is pointing up */ ao_config_get(); - if (ao_flight_accel < ao_config.accel_zero_g - ACCEL_NOSE_UP) { - + if (ao_config.accel_plus_g != 0 && ao_config.accel_minus_g != 0 && + ao_flight_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP) + { /* Disable the USB controller in flight mode * to save power */ @@ -337,19 +341,15 @@ ao_flight(void) /* apogee detect: coast to drogue deploy: * - * accelerometer: abs(velocity) > min_velocity + 2m/s - * OR * barometer: fall at least 10m * - * If the barometer saturates because the flight - * goes over its measuring range (about 53k'), - * requiring a 10m fall will avoid prematurely - * detecting apogee; the accelerometer will take - * over in that case and the integrated velocity - * measurement should suffice to find apogee + * It would be nice to use the accelerometer + * to detect apogee as well, but tests have + * shown that flights far from vertical would + * grossly mis-detect apogee. So, for now, + * we'll trust to a single sensor for this test */ - if (/* abs(ao_flight_vel) > ao_min_vel + ACCEL_VEL_APOGEE || */ - ao_flight_pres > ao_min_pres + BARO_APOGEE) + if (ao_flight_pres > ao_min_pres + BARO_APOGEE) { /* ignite the drogue charge */ ao_ignite(ao_igniter_drogue); @@ -462,7 +462,7 @@ ao_flight_status(void) __reentrant { printf("STATE: %7s accel: %d speed: %d altitude: %d main: %d\n", ao_state_names[ao_flight_state], - AO_ACCEL_COUNT_TO_MSS(ACCEL_ZERO_G - ao_flight_accel), + AO_ACCEL_COUNT_TO_MSS( - ao_flight_accel), AO_VEL_COUNT_TO_MS(ao_flight_vel), ao_pres_to_altitude(ao_flight_pres), ao_pres_to_altitude(ao_main_pres)); diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index 83c63016..61f48cb6 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -22,6 +22,8 @@ #include #include +#define AO_HERTZ 100 + #define AO_ADC_RING 64 #define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) #define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) @@ -144,12 +146,13 @@ ao_altitude_to_pres(int16_t alt) __reentrant struct ao_config { uint16_t main_deploy; - int16_t accel_zero_g; + int16_t accel_plus_g; + int16_t accel_minus_g; }; #define ao_config_get() -struct ao_config ao_config = { 250, 16000 }; +struct ao_config ao_config = { 250, 15937, 16467 }; #include "ao_flight.c" -- 2.30.2