|
|
@@ -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;
|
|
|
|
}
|