From f99461512e886d0c3051188d041a97cea76b971e Mon Sep 17 00:00:00 2001 From: jim Date: Tue, 21 Jul 2009 17:44:15 +0000 Subject: [PATCH] calibration work, need to commit to test-compile in windows git-svn-id: https://bucket.mit.edu/svn/nilm/zoom@7691 ddd99763-3ecb-0310-9145-efcb8ce7c51f --- firmware/calibrate.c | 138 ++++++++++++++++++++++++++++++------------ firmware/calibrate.h | 37 +++++++++-- firmware/mode_debug.c | 11 +++- firmware/util.h | 13 ++++ 4 files changed, 152 insertions(+), 47 deletions(-) diff --git a/firmware/calibrate.c b/firmware/calibrate.c index ad9d0f0..21ea8a6 100644 --- a/firmware/calibrate.c +++ b/firmware/calibrate.c @@ -3,75 +3,133 @@ #include "util.h" #include "adc.h" #include "dac.h" +#include "led.h" -static float scale; /* delta(DAC) / delta(ADC) */ +float g_scale; /* delta(DAC) / delta(ADC) */ /* Initialize. Assume some relatively-safe scaling if no calibration is run */ void calibrate_init(void) { - scale = 0; + g_scale = 0.1; /* 1 count at DAC means 10 counts at ADC */ } -/* Seek with the DAC to reach a specific ADC value, adjusting - * calibration if necessary */ -uint16_t seek(int16_t desired) +/* Given the current DAC and ADC values d1 and a1, + compute a new DAC value d2 to give the desired ADC value a2 */ +uint16_t adc_to_dac(uint16_t d1, int16_t a1, int16_t a2, float scale) { - int32_t dac = 32768; - int16_t actual; - int32_t delta = 0; + int32_t delta; + int32_t d2; + + delta = (int32_t)((a2 - a1) * scale + 0.5); + d2 = d1 - delta; + return clamp(DAC_MIN, d2, DAC_MAX); +} + +/* Calculate a new scale factor given two DAC and ADC points */ +float calculate_scale(uint16_t d1, float a1, uint16_t d2, float a2) +{ + if (d1 == d2) + return 1.0; + else + return (a2 - a1) / (float)(d2 - d1); +} + +/* Seek with the DAC to reach a specific ADC value. Uses g_scale as + an initial guess for scaling factor, but adjusts it dynamically. */ +uint16_t seek(uint16_t starting_dac, int16_t desired_adc) +{ + uint16_t old_dac, dac; + int16_t old_adc, adc; + float scale = g_scale; + int bad = 0; + + dac = starting_dac; + + /* goto current location */ + dac_write(dac); + msleep(1); + adc = adc_get(); while (1) { - /* get current location */ + old_dac = dac; + old_adc = adc; + + /* jump to the desired value */ + dac = adc_to_dac(old_dac, old_adc, desired_adc, scale); + + /* if we're not making progress, give up */ + if (dac == old_dac) { + bad++; + if (bad > 5) { + led_pattern(0b00000001); + break; + } + } + + /* write it out */ dac_write(dac); msleep(1); - actual = adc_get(); - - /* adjust scaling to match the jump we just saw */ - if (delta) { - + adc = adc_get(); + + /* if we're close, accept it */ + if (abs(adc - desired_adc) <= SEEK_FUZZ_ADC) + break; + + /* otherwise, if we were within ADC clamp limits, and + the DAC changed a non-trivial amount, readjust + scale factor */ + if (adc > ADC_CLAMP_MIN && old_adc > ADC_CLAMP_MIN && + adc < ADC_CLAMP_MAX && old_adc < ADC_CLAMP_MAX && + abs((int32_t)dac - old_dac) >= SEEK_FUZZ_DAC) { + scale = calculate_scale(old_dac, old_adc, dac, adc); } - if (actual == desired) - return dac; - - /* figure out how much to change the DAC */ - delta = (desired - actual) * scale; - - - dac -= (desired - actual) * scale; - - + /* if we totally overshot the window, cut the scale in half */ + if ((adc < ADC_CLAMP_MIN && old_adc > ADC_CLAMP_MAX) || + (adc > ADC_CLAMP_MAX && old_adc < ADC_CLAMP_MIN)) + { + scale *= 0.5; + } } - uint16_t dac = 0; - dac_write(0x8000 + return dac; } /* Perform calibration */ -void do_calibrate(void) +float do_calibrate(void) { - uint16_t daczero, x; - float x1, y1, x2, y2; + uint16_t daczero, x1, x2; + float y1, y2; /* Zero ADC */ - daczero = seek(1024); + daczero = seek((DAC_MIN + DAC_MAX) / 2, CALIBRATE_ADC_ZERO); /* Go halfway down and take an accurate sample */ - x = seek(512); - oversample(x, &x1, &y1); + x1 = seek(daczero, CALIBRATE_ADC_LOW); + y1 = oversample(x1); /* Go halfway up and take an accurate sample */ - x = seek(768); - oversample(x, &x2, &y2); + x2 = seek(daczero, CALIBRATE_ADC_HIGH); + y2 = oversample(x2); + + /* Calculate scale */ + g_scale = calculate_scale(x1, y1, x2, y2); - /* Scale factor is 1/slope */ - scale = (x2 - x1) / (y2 - y1); + return g_scale; } -/* convert from (counts at ADC) to (counts at DAC) */ -int32_t adc_toc(int16_t adc) +/* Oversample to get a nice ADC value */ +float oversample(uint16_t dac) { - int32_t r = ((float) adc) * scale; - return r; + int i; + int32_t sum = 0; + + for (i = 0; i < OVERSAMPLE_COUNT; i++) { + dac_write(dac); + msleep(1); + sum += adc_get(); + } + + return (float)sum / (float)OVERSAMPLE_COUNT; } diff --git a/firmware/calibrate.h b/firmware/calibrate.h index 8964eba..4f8bf7c 100644 --- a/firmware/calibrate.h +++ b/firmware/calibrate.h @@ -1,12 +1,41 @@ #ifndef CALIBRATE_H #define CALIBRATE_H +#define ADC_CLAMP_MIN 256 +#define ADC_CLAMP_MAX 1792 + +#define DAC_MIN 0 +#define DAC_MAX 65535 + +#define OVERSAMPLE_COUNT 128 + +#define SEEK_FUZZ_ADC 20 +#define SEEK_FUZZ_DAC 5 + +#define CALIBRATE_ADC_ZERO 1024 +#define CALIBRATE_ADC_LOW 512 +#define CALIBRATE_ADC_HIGH 1536 + +/* Initialize. Assume some relatively-safe scaling if no calibration is run */ void calibrate_init(void); -/* Do calibration routine */ -void do_calibrate(void); +/* Given the current DAC and ADC values d1 and a1, + compute a new DAC value d2 to give the desired ADC value a2 */ +uint16_t adc_to_dac(uint16_t d1, int16_t a1, int16_t a2, float scale); + +/* Calculate a new scale factor given two DAC and ADC points */ +float calculate_scale(uint16_t d1, float a1, uint16_t d2, float a2); + +/* Seek with the DAC to reach a specific ADC value, + using current scaling as a starting point. */ +uint16_t seek(uint16_t starting_dac, int16_t desired_adc); + +/* Perform calibration */ +float do_calibrate(void); + +/* Oversample to get a nice ADC value */ +float oversample(uint16_t dac); -/* convert from (counts at ADC) to (counts at DAC) */ -int32_t adc_to_dac(int16_t adc); +extern float g_scale; /* delta(DAC) / delta(ADC) */ #endif diff --git a/firmware/mode_debug.c b/firmware/mode_debug.c index 07ebb79..664e5bd 100644 --- a/firmware/mode_debug.c +++ b/firmware/mode_debug.c @@ -18,14 +18,19 @@ static int zero_adc = 0; void TISR_HANDLER(7) { -#pragma warn FIXME -#if 0 int16_t v; - float dac_current, i; timer_clear_txif(7); if (zero_adc) { + v = adc_get(); + + + (1024 - v) + + dacval = dac; + + dac_current = dac_to_current(dac); v = adc_get(); diff --git a/firmware/util.h b/firmware/util.h index c86643e..46cfd93 100644 --- a/firmware/util.h +++ b/firmware/util.h @@ -21,4 +21,17 @@ int32_t hex_to_u16(char x[4]); x; \ __int_enable(); } while(0) +/* Misc */ +#define max(a,b) \ + ({ typeof (a) _a = (a); \ + typeof (b) _b = (b); \ + _a > _b ? _a : _b; }) + +#define min(a,b) \ + ({ typeof (a) _a = (a); \ + typeof (b) _b = (b); \ + _a < _b ? _a : _b; }) + +#define clamp(a,v,b) min(b,max(v,a)) + #endif