pwmled.c: rework

Probing and time-averaging works, but several corner cases need to be
polished.

diff --git a/firmware/pwmled.c b/firmware/pwmled.c
index d5d947be2d011a396353b11cc7f88f93824a5ae9..23231c9858cf80abfe6a2f777e59cc04b1a1e8ea 100644 (file)
--- a/firmware/pwmled.c
+++ b/firmware/pwmled.c
@@ -2,12 +2,14 @@
 
 #include "lights.h"
 
-static uint16_t pwm_vals[N_PWMLEDS*N_PWMLED_MODES];
-static uint16_t pwm_max[N_PWMLEDS] = {
-       2*PWM_MAX/3,
-       PWM_MAX - (PWM_MAX >> 4), // step-up
-       2*PWM_MAX/3
-};
+typedef struct {
+       uint16_t target, pwm;
+       int16_t err_sum;
+       unsigned char mode, state, probe_steps;
+       uint16_t mode_pwm[N_PWMLED_MODES];
+} pwmled_t;
+
+pwmled_t pwmleds[N_PWMLEDS];
 
 #define PWMLED2_TESTING_WITH_350MA_LED
 
@@ -16,6 +18,7 @@ static uint16_t pwm_max[N_PWMLEDS] = {
        ((unsigned long)(ma))*(mohm) /* voltage at sensing resistor in uV */ \
        /(1100000UL/gain/1024UL)     /* voltage of ADC reading == 1 */ \
 )
+
 static uint16_t adc_max[N_PWMLEDS] = {
        MA_MOHM_GAIN_TO_ADC( 400, SENSE_MOHM, 20),
        MA_MOHM_GAIN_TO_ADC(  30, SENSE_MOHM, 20),
@@ -25,6 +28,7 @@ static uint16_t adc_max[N_PWMLEDS] = {
        MA_MOHM_GAIN_TO_ADC(2500, SENSE_MOHM,  1)
 #endif
 };
+
 static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES] = {
        /* pwmled0 */
        MA_MOHM_GAIN_TO_ADC(  20, SENSE_MOHM, 20),
@@ -32,11 +36,16 @@ static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES] = {
        MA_MOHM_GAIN_TO_ADC( 100, SENSE_MOHM, 20),
        MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM, 20),
        /* pwmled1 */
+       16, 32, 64, 112,
+#if 0
        MA_MOHM_GAIN_TO_ADC(   5, SENSE_MOHM, 20),
        MA_MOHM_GAIN_TO_ADC(  12, SENSE_MOHM, 20),
        MA_MOHM_GAIN_TO_ADC(  16, SENSE_MOHM, 20),
        MA_MOHM_GAIN_TO_ADC(  20, SENSE_MOHM, 20),
+#endif
        /* pwmled2 */
+       24, 32, 40, 48
+#if 0
 #ifdef PWMLED2_TESTING_WITH_350MA_LED
        MA_MOHM_GAIN_TO_ADC( 100, SENSE_MOHM,  1),
        MA_MOHM_GAIN_TO_ADC( 140, SENSE_MOHM,  1),
@@ -48,244 +57,141 @@ static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES] = {
        MA_MOHM_GAIN_TO_ADC( 700, SENSE_MOHM,  1),
        MA_MOHM_GAIN_TO_ADC(2400, SENSE_MOHM,  1),
 #endif
+#endif
 };
 
-// TODO: maybe convert this to bitmask to simplify pwmled_needs_adc() ?
-static unsigned char pwmled_state[N_PWMLEDS];
 #define ST_DISABLED 0
-#define ST_PROBING  1
-#define ST_OFF      2
+#define ST_OFF      1
+#define ST_PROBING  2
 #define ST_ON       3
-
-static unsigned char pwmled_mode[N_PWMLEDS];
-static unsigned char pwmled_mode_set[N_PWMLEDS];
-
-static uint16_t pwm_probes[N_PWMLEDS];
-static int16_t differences[N_PWMLEDS];
-
-static void start_probing(unsigned char n)
-{
-       pwmled_state[n] = ST_PROBING;
-       pwm_set(n, 0);
-       pwm_probes[n] = 0;
-}
+// The above are constructed so that the following work:
+#define ST_IS_ON(s)    ((s) & 0x02)
+#define ST_CAN_SET_MODE(s)     ((s) & 0x01)
 
 void pwmled_init()
 {
-       unsigned char i;
-
-       for (i = 0; i < N_PWMLEDS*N_PWMLED_MODES; i++) {
-               pwm_vals[i] = 0;
-               pwmled_mode[i] = 0;
-               pwmled_mode_set[i] = 0;
-       }
+       unsigned char i, j;
 
        for (i = 0; i < N_PWMLEDS; i++) {
-               differences[i] = 0;
-               start_probing(i);
+               pwmled_t *led = pwmleds + i;
+               led->err_sum = 0;
+               led->target = adc_vals[i*N_PWMLED_MODES];
+               led->pwm = 0;
+               led->mode = 1;
+               led->state = ST_PROBING;
+               led->probe_steps = 0;
+
+               for (j = 0; j < N_PWMLED_MODES; j++)
+                       led->mode_pwm[j] = 0;
        }
 }
 
-unsigned char pwmled_enabled(unsigned char n)
-{
-       unsigned char st = pwmled_state[n];
-       if (st == ST_OFF || st == ST_ON)
-               return 1;
-       else
-               return 0;
-}
-
 void pwmled_set_mode(unsigned char n, unsigned char mode)
 {
-       if (!pwmled_enabled(n))
+       pwmled_t *led = pwmleds + n;
+
+       if (!ST_CAN_SET_MODE(led->state))
                return;
 
-#if 0
-       log_byte(0xF8);
-       log_byte(n);
-       log_byte(mode);
-#endif
+       if (led->mode) // save the previous state
+               led->mode_pwm[led->mode - 1] = led->pwm;
+
+       if (n == 1)
+               log_byte(mode);
+
+       led->mode = mode;
+
        if (mode > 0 && mode <= N_PWMLED_MODES) {
-               uint16_t pwmval;
-               mode--;
-               pwmval = pwm_vals[n*N_PWMLED_MODES+mode];
-               pwm_set(n, pwmval);
-#if 0
-               log_byte(pwmval);
-#endif
-               pwmled_state[n] = ST_ON;
-               pwmled_mode[n] = mode;
-               pwmled_mode_set[n] = 1;
-               differences[n] = 0;
+               led->target = adc_vals[n*N_PWMLED_MODES + mode - 1];
+               led->state = ST_ON;
+               led->pwm = led->mode_pwm[mode - 1];
+               led->err_sum = 0;
+               pwm_set(n, led->pwm);
        } else {
+               led->state = ST_OFF;
                pwm_off(n);
-               pwmled_state[n] = ST_OFF;
        }
 }
 
-static void inline probing_adc(unsigned char n, uint16_t adcval)
+void pwmled_adc(unsigned char n, uint16_t adcval)
 {
-       unsigned char need_bigger = 0, i;
-       uint16_t *pwm_p = &pwm_vals[n*N_PWMLED_MODES];
-       uint16_t *adc_p = &adc_vals[n*N_PWMLED_MODES];
-       uint16_t pwm = pwm_probes[n];
+       pwmled_t *led = pwmleds + n;
+       int16_t sum, pwm_div;
+       uint16_t old_pwm;
 
-#if 0
-       log_byte(0xF4);
-       log_byte(n);
-       log_word(adcval);
-#endif
-
-       if (adcval > adc_max[n] // Too high
-               || (pwm == 0 && adcval > 2) // non-zero voltage with zero PWM
-               ) {
-               pwm_off(n);
-               pwmled_state[n] = ST_DISABLED;
-               log_byte(0xF0);
-               log_byte(n);
-               log_word(adcval);
+       if (!ST_IS_ON(led->state))
                return;
-       }
 
-       for (i = 0; i < N_PWMLED_MODES; i++, pwm_p++, adc_p++) {
-               uint16_t adc = *adc_p;
-               if (adc >= adcval) {
-                       *pwm_p = pwm;
-                       need_bigger = 1;
-               }
-       }
+       // FIXME: test for maximum adcval value (adc_max[n])
 
-#if 0
-       if ((n == 1 && pwm > 0x35) || adcval != 0) {
-               log_byte(n);
-               log_byte(0xF3);
-               log_byte(pwm);
-               log_word(adcval);
-       }
-#endif
+       sum = led->err_sum += (int16_t)(led->target) - (int16_t)adcval;
 
-       if (!need_bigger) { // successfully probed
-               pwm_off(n);
-               // pwm_set(n, 0);
-               pwmled_state[n] = ST_OFF;
-               log_byte(0xF1);
-               log_byte(n);
+       // FIXME: try to work faster during probing
+       pwm_div = 64; //led->state == ST_PROBING ? (1 << 5) : (1 << 8);
+       old_pwm = led->pwm;
 
-               return;
-       }
+       if (sum >= pwm_div) {
+               uint16_t diff = sum/pwm_div;
+               led->pwm += diff;
+               led->err_sum -= diff * pwm_div;
 
-       if (pwm >= pwm_max[n]) { // over the maximum!
-               pwm_off(n);
-               pwmled_state[n] = ST_DISABLED;
-               log_byte(0xF2);
-               log_byte(n);
-               // pwm_set(n, 0);
-               return;
+       } else if (sum <= -pwm_div) {
+               uint16_t diff = (-sum)/pwm_div;
+
+               if (led->pwm >= diff)
+                       led->pwm -= (int16_t)diff;
+               else
+                       led->pwm = 0;
+               led->err_sum += diff * pwm_div;
        }
 
-       // try to increase
-       pwm++;
-       pwm_probes[n] = pwm;
-       pwm_set(n, pwm);
-}
+       if (led->state == ST_PROBING) {
+               if (led->pwm == old_pwm) {
+                       if (led->probe_steps < 10)
+                               led->probe_steps++;
+               } else {
+                       led->probe_steps = 0;
+               }
 
-// Feedback loop
-static void inline on_adc(unsigned char n, uint16_t adcval)
-{
-       unsigned char mode = pwmled_mode[n];
-       uint16_t adc_exp   =  adc_vals[n*N_PWMLED_MODES+mode];
-       uint16_t *pwm_p    = &pwm_vals[n*N_PWMLED_MODES+mode];
-       int16_t old_pwm    = *pwm_p;
-       int16_t new_pwm    = old_pwm;
+               if (led->probe_steps >= 10 || old_pwm > led->pwm) {
 
-#if 0
-       log_byte(0xF5);
-       log_byte(n);
-       log_word(adcval);
-#endif
+                       led->mode_pwm[led->mode - 1] = led->pwm;
 
+                       if (led->mode < N_PWMLED_MODES) {
+                               led->probe_steps = 0;
+                               led->err_sum = 0;
 
-       if (pwmled_mode_set[n]) { // ignore the first reading
-               pwmled_mode_set[n] = 0;
-               return;
-       }
+                               led->mode++;
+                               led->target = adc_vals[n*N_PWMLED_MODES+led->mode-1];
+                       } else {
+                               unsigned char i;
 
-       differences[n] += adcval;
-       differences[n] -= adc_exp;
+                               led->state = ST_OFF;
 
-       if (differences[n] > 16)
-               new_pwm -= 2;
-       else if (differences[n] > 4)
-               new_pwm--;
-       else if (differences[n] < -16)
-               new_pwm += 2;
-       else if (differences[n] < -4)
-               new_pwm++;
-       // new_pwm -= differences[n] >> 3;
+                               log_byte(0xF0);
+                               log_byte(n);
+                               log_word(jiffies);
 
-       if (new_pwm == old_pwm)
-               return;
+                               for (i = 0; i < N_PWMLED_MODES; i++)
+                                       log_word(led->mode_pwm[i]);
+
+                               log_flush();
+                       }
+               }
+       }
 
-       differences[n] = 0;
+       if (led->pwm == old_pwm)
+               return;
 
-       if (new_pwm > (int16_t)pwm_max[n]) {
-               // FIXME: disconnected?
-               log_byte(0xE1);
+       if (led->pwm > (PWM_MAX << PWM_STEP_SHIFT)/2) {
+               pwm_off(n);
+               led->state = ST_DISABLED;
+               log_byte(0xF1);
                log_byte(n);
-               log_word(new_pwm);
-               log_word(adcval);
                log_word(jiffies);
-               pwmled_state[n] = ST_DISABLED;
-               pwm_off(n);
-               return;
+               log_flush();
        }
 
-       if (new_pwm < 1) {
-               // FIXME: short-circuit?
-               new_pwm = 1;
-       }
-
-       *pwm_p = new_pwm;
-       pwm_set(n, new_pwm);
-
-       if (jiffies > 500 && n == 1) {
-               log_byte(adcval & 0xFF);
-               log_byte(new_pwm);
-       }
+       pwm_set(n, led->pwm);
 }
 
-void pwmled_adc(unsigned char n, uint16_t adcval)
-{
-       unsigned char i, probing;
-
-       switch (pwmled_state[n]) {
-       case ST_PROBING:
-               probing_adc(n, adcval);
-#if 1
-               probing = 0;
-               for (i = 0; i < N_PWMLEDS; i++)
-                       if (pwmled_state[i] == ST_PROBING)
-                               probing = 1;
-
-               if (!probing) {
-                       log_word(0x5555);
-                       log_word(jiffies);
-                       for (i = 0; i < N_PWMLEDS; i++)
-                               log_byte(pwmled_state[i]);
-                               
-                       for (i = 0; i < N_PWMLEDS*N_PWMLED_MODES; i++)
-                               log_word(pwm_vals[i]);
-                       log_flush();
-                       log_set_state(4);
-                       for (i = 0; i < N_PWMLEDS; i++)
-                               led_set_pattern(i, mode1_pattern);
-               }
-#endif
-               
-               return;
-       case ST_ON:
-               on_adc(n, adcval);
-               return;
-       // WTF am I doing in this function then? Maybe recently switched off?
-       }
-}