Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

[fw/altos] / src / ao_flight.c

diff --git a/src/ao_flight.c b/src/ao_flight.c
index 9eb9a0145ac54614afd519f89a88cd143061694a..81aecad3e26e0053059b65a90b7273a9504d627a 100644 (file)
--- a/src/ao_flight.c
+++ b/src/ao_flight.c
@@ -151,12 +151,98 @@ ao_flight(void)
                while (ao_flight_adc != ao_adc_head) {
                        __pdata uint8_t ticks;
                        __pdata int16_t ao_vel_change;
+                       __xdata struct ao_adc *ao_adc;
                        ao_flight_prev_tick = ao_flight_tick;
 
                        /* Capture a sample */
-                       ao_raw_accel = ao_adc_ring[ao_flight_adc].accel;
-                       ao_raw_pres = ao_adc_ring[ao_flight_adc].pres;
-                       ao_flight_tick = ao_adc_ring[ao_flight_adc].tick;
+                       ao_adc = &ao_adc_ring[ao_flight_adc];
+                       ao_flight_tick = ao_adc->tick;
+                       ao_raw_accel = ao_adc->accel;
+#if HAS_ACCEL_REF
+                       /*
+                        * Ok, the math here is a bit tricky.
+                        *
+                        * ao_raw_accel:  ADC output for acceleration
+                        * ao_accel_ref:  ADC output for the 5V reference.
+                        * ao_cook_accel: Corrected acceleration value
+                        * Vcc:           3.3V supply to the CC1111
+                        * Vac:           5V supply to the accelerometer
+                        * accel:         input voltage to accelerometer ADC pin
+                        * ref:           input voltage to 5V reference ADC pin
+                        *
+                        *
+                        * Measured acceleration is ratiometric to Vcc:
+                        *
+                        *     ao_raw_accel   accel
+                        *     ------------ = -----
+                        *        32767        Vcc
+                        *
+                        * Measured 5v reference is also ratiometric to Vcc:
+                        *
+                        *     ao_accel_ref    ref
+                        *     ------------ = -----
+                        *        32767        Vcc
+                        *
+                        *
+                        *      ao_accel_ref = 32767 * (ref / Vcc)
+                        *
+                        * Acceleration is measured ratiometric to the 5V supply,
+                        * so what we want is:
+                        *
+                        *      ao_cook_accel    accel
+                        *      ------------- =  -----
+                        *          32767         ref
+                        *
+                        *
+                        *                      accel    Vcc
+                        *                    = ----- *  ---
+                        *                       Vcc     ref
+                        *
+                        *                      ao_raw_accel       32767
+                        *                    = ------------ *  ------------
+                        *                         32737        ao_accel_ref
+                        *
+                        * Multiply through by 32767:
+                        *
+                        *                      ao_raw_accel * 32767
+                        *      ao_cook_accel = --------------------
+                        *                          ao_accel_ref
+                        *
+                        * Now, the tricky part. Getting this to compile efficiently
+                        * and keeping all of the values in-range.
+                        *
+                        * First off, we need to use a shift of 16 instead of * 32767 as SDCC
+                        * does the obvious optimizations for byte-granularity shifts:
+                        *
+                        *      ao_cook_accel = (ao_raw_accel << 16) / ao_accel_ref
+                        *
+                        * Next, lets check our input ranges:
+                        *
+                        *      0 <= ao_raw_accel <= 0x7fff             (singled ended ADC conversion)
+                        *      0x7000 <= ao_accel_ref <= 0x7fff        (the 5V ref value is close to 0x7fff)
+                        *
+                        * Plugging in our input ranges, we get an output range of 0 - 0x12490,
+                        * which is 17 bits. That won't work. If we take the accel ref and shift
+                        * by a bit, we'll change its range:
+                        *
+                        *      0xe000 <= ao_accel_ref<<1 <= 0xfffe
+                        *
+                        *      ao_cook_accel = (ao_raw_accel << 16) / (ao_accel_ref << 1)
+                        *
+                        * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It
+                        * is, however, one bit too large for our signed computations. So, we
+                        * take the result and shift that by a bit:
+                        *
+                        *      ao_cook_accel = ((ao_raw_accel << 16) / (ao_accel_ref << 1)) >> 1
+                        *
+                        * This finally creates an output range of 0 - 0x4924. As the ADC only
+                        * provides 11 bits of data, we haven't actually lost any precision,
+                        * just dropped a bit of noise off the low end.
+                        */
+                       ao_raw_accel = (uint16_t) ((((uint32_t) ao_raw_accel << 16) / (ao_accel_ref[ao_flight_adc] << 1))) >> 1;
+                       ao_adc->accel = ao_raw_accel;
+#endif
+                       ao_raw_pres = ao_adc->pres;
 
                        ao_flight_accel -= ao_flight_accel >> 4;
                        ao_flight_accel += ao_raw_accel >> 4;