You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 

181 lines
4.0 KiB

  1. #include "config.h"
  2. #include "calibrate.h"
  3. #include "util.h"
  4. #include "adc.h"
  5. #include "dac.h"
  6. #include "led.h"
  7. #include "timer.h"
  8. #include "uart.h"
  9. //#define DEBUG_CALIBRATION
  10. float g_scale; /* delta(DAC) / delta(ADC) */
  11. /* Initialize. Assume some relatively-safe scaling if no calibration is run */
  12. void calibrate_init(void)
  13. {
  14. g_scale = (DAC_RANGE / 65536.0); /* 1 count at 16-bit DAC means 1 counts at ADC */
  15. }
  16. /* Given the current DAC and ADC values d1 and a1,
  17. compute a new DAC value d2 to give the desired ADC value a2 */
  18. uint16_t adc_to_dac(uint16_t d1, int16_t a1, int16_t a2, float scale)
  19. {
  20. int32_t delta;
  21. int32_t d2;
  22. delta = (int32_t)((a2 - a1) * scale + 0.5);
  23. d2 = d1 + delta;
  24. return clamp(DAC_MIN, d2, DAC_MAX);
  25. }
  26. /* Calculate a new scale factor given two DAC and ADC points */
  27. float calculate_scale(uint16_t d1, float a1, uint16_t d2, float a2)
  28. {
  29. float scale;
  30. float a = a2 - a1;
  31. /* Correct for known errors */
  32. d1 = dac_get_actual_float(d1);
  33. d2 = dac_get_actual_float(d2);
  34. if (a < 0.1 && a > -0.1)
  35. scale = 1.0;
  36. else {
  37. scale = ((float)d2 - d1) / a;
  38. if (scale < 0.01)
  39. scale = 0.01;
  40. if (scale > 20)
  41. scale = 20;
  42. }
  43. return scale;
  44. }
  45. /* Seek with the DAC to reach a specific ADC value. Uses g_scale as
  46. an initial guess for scaling factor, but adjusts it dynamically. */
  47. uint16_t seek(uint16_t starting_dac, int16_t desired_adc)
  48. {
  49. uint16_t old_dac, dac;
  50. int16_t old_adc, adc;
  51. float scale = g_scale;
  52. int steps = 0;
  53. dac = starting_dac;
  54. /* goto current location */
  55. dac_write(dac);
  56. msleep(1);
  57. adc = adc_get();
  58. while (1)
  59. {
  60. /* give up if we're not making progress */
  61. if (steps++ > SEEK_MAX_STEPS) {
  62. // 2 flashes, delay, repeat
  63. led_pattern(0b00101000);
  64. break;
  65. }
  66. old_dac = dac;
  67. old_adc = adc;
  68. /* jump to the desired value */
  69. dac = adc_to_dac(old_dac, old_adc, desired_adc, scale);
  70. /* write it out */
  71. dac_write(dac);
  72. msleep(1);
  73. adc = adc_get();
  74. #ifdef DEBUG_CALIBRATION
  75. uart1_put_hex16(dac);
  76. uart1_put(' ');
  77. uart1_put_hex16(adc);
  78. uart1_put(' ');
  79. uart1_put_hex16(desired_adc);
  80. uart1_put(' ');
  81. uart1_put_hex32(*(uint32_t *)&scale);
  82. uart1_crlf();
  83. #endif
  84. /* if we're close, accept it */
  85. if (abs(adc - desired_adc) <= SEEK_FUZZ_ADC) {
  86. led_pattern(0b11111110);
  87. break;
  88. }
  89. /* otherwise, if we were within ADC clamp limits, and
  90. the DAC changed a non-trivial amount, readjust
  91. scale factor */
  92. if (adc > ADC_CLAMP_MIN && old_adc > ADC_CLAMP_MIN &&
  93. adc < ADC_CLAMP_MAX && old_adc < ADC_CLAMP_MAX &&
  94. abs((int32_t)dac - old_dac) >= SEEK_FUZZ_DAC) {
  95. scale = calculate_scale(old_dac, old_adc, dac, adc);
  96. }
  97. /* if we totally overshot the window, cut the scale in half */
  98. if ((adc < ADC_CLAMP_MIN && old_adc > ADC_CLAMP_MAX) ||
  99. (adc > ADC_CLAMP_MAX && old_adc < ADC_CLAMP_MIN))
  100. {
  101. scale *= 0.5;
  102. }
  103. }
  104. return dac;
  105. }
  106. /* Perform calibration */
  107. uint16_t do_calibrate(void)
  108. {
  109. uint16_t daczero, x1, x2;
  110. float y1, y2;
  111. /* Zero ADC */
  112. daczero = seek((DAC_MIN + DAC_MAX) / 2, CALIBRATE_ADC_ZERO);
  113. /* Go down take an accurate sample */
  114. x1 = seek(daczero, CALIBRATE_ADC_LOW);
  115. y1 = oversample(x1);
  116. /* Go up and take an accurate sample */
  117. x2 = seek(daczero, CALIBRATE_ADC_HIGH);
  118. y2 = oversample(x2);
  119. /* Calculate scale */
  120. g_scale = calculate_scale(x1, y1, x2, y2);
  121. #ifdef DEBUG_CALIBRATION
  122. uart1_put_string("calibrate x1=");
  123. uart1_put_dec(x1);
  124. uart1_put_string(" y1=");
  125. uart1_put_float(y1);
  126. uart1_put_string(" x2=");
  127. uart1_put_dec(x2);
  128. uart1_put_string(" y2=");
  129. uart1_put_float(y2);
  130. uart1_put_string(" scale=");
  131. uart1_put_float(g_scale);
  132. uart1_crlf();
  133. #endif
  134. /* Return to zero position */
  135. dac_write(daczero);
  136. return daczero;
  137. }
  138. /* Oversample to get a nice ADC value */
  139. float oversample(uint16_t dac)
  140. {
  141. int i;
  142. int32_t sum = 0;
  143. for (i = 0; i < OVERSAMPLE_COUNT; i++) {
  144. dac_write(dac);
  145. msleep(1);
  146. sum += adc_get();
  147. }
  148. return (float)sum / (float)OVERSAMPLE_COUNT;
  149. }