Add two-point accelerometer calibration.
authorKeith Packard <keithp@keithp.com>
Tue, 3 Nov 2009 05:38:18 +0000 (21:38 -0800)
committerKeith Packard <keithp@keithp.com>
Tue, 3 Nov 2009 07:51:41 +0000 (23:51 -0800)
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
src/ao.h
src/ao_config.c
src/ao_flight.c
src/ao_flight_test.c

index ac594c0..260ba77 100755 (executable)
@@ -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
index 4cceefe..22e8785 100644 (file)
--- 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;
index 021fb6f..85fcff8 100644 (file)
@@ -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 <meters>  Set height above launch for main deploy (in meters)" },
-       { 'a',  ao_config_accel_zero_g_set,     ao_config_accel_zero_g_show,
-               "a <value>   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 <channel> Set radio channel (freq = 434.550 + channel * .1)" },
        { 'c',  ao_config_callsign_set,         ao_config_callsign_show,
index e91a5da..92c955f 100644 (file)
@@ -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));
index 83c6301..61f48cb 100644 (file)
@@ -22,6 +22,8 @@
 #include <stdlib.h>
 #include <string.h>
 
+#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"