pwmled.c: save error sum between mode switches

otherwise it leads to instability when during short blink the error
sum does not manage to increase the stride.

Also, use 32-bit ints to simplify the code and save some branches.

diff --git a/firmware/pwmled.c b/firmware/pwmled.c
index 3e4e1f495b419872b897cd68800eac9351b5d208..d70013e53eb29b4e1cb3faae35f9c61e0f6085fd 100644 (file)
--- a/firmware/pwmled.c
+++ b/firmware/pwmled.c
@@ -7,6 +7,7 @@ typedef struct {
        int16_t err_sum;
        unsigned char mode, state, probe_steps;
        uint16_t mode_pwm[N_PWMLED_MODES];
+       int16_t err_sums[N_PWMLED_MODES];
 } pwmled_t;
 
 pwmled_t pwmleds[N_PWMLEDS];
@@ -81,8 +82,10 @@ void init_pwmled()
                led->state = ST_PROBING;
                led->probe_steps = 0;
 
-               for (j = 0; j < N_PWMLED_MODES; j++)
+               for (j = 0; j < N_PWMLED_MODES; j++) {
                        led->mode_pwm[j] = 0;
+                       led->err_sums[j] = 0;
+               }
        }
 }
 
@@ -93,11 +96,10 @@ void pwmled_set_mode(unsigned char n, unsigned char mode)
        if (!ST_CAN_SET_MODE(led->state))
                return;
 
-       if (led->mode) // save the previous state
+       if (led->mode) { // save the previous state
                led->mode_pwm[led->mode - 1] = led->pwm;
-
-       if (n == 1)
-               log_byte(mode);
+               led->err_sums[led->mode - 1] = led->err_sum;
+       }
 
        led->mode = mode;
 
@@ -105,7 +107,7 @@ void pwmled_set_mode(unsigned char n, unsigned char mode)
                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;
+               led->err_sum = led->err_sums[mode - 1];
                pwm_set(n, led->pwm);
        } else {
                led->state = ST_OFF;
@@ -116,34 +118,28 @@ void pwmled_set_mode(unsigned char n, unsigned char mode)
 void pwmled_adc(unsigned char n, uint16_t adcval)
 {
        pwmled_t *led = pwmleds + n;
-       int16_t sum, pwm_div;
        uint16_t old_pwm;
+       int32_t sum;
+       unsigned char shift;
 
        if (!ST_IS_ON(led->state))
                return;
 
        // FIXME: test for maximum adcval value (adc_max[n])
 
-       sum = led->err_sum += (int16_t)(led->target) - (int16_t)adcval;
-
-       // FIXME: try to work faster during probing
-       pwm_div = 64; //led->state == ST_PROBING ? (1 << 5) : (1 << 8);
        old_pwm = led->pwm;
 
-       if (sum >= pwm_div) {
-               uint16_t diff = sum/pwm_div;
-               led->pwm += diff;
-               led->err_sum -= diff * pwm_div;
+       shift = led->state == ST_PROBING ? 3 : 5;
 
-       } else if (sum <= -pwm_div) {
-               uint16_t diff = (-sum)/pwm_div;
+       sum = ((int32_t)led->pwm << shift)
+               + led->err_sum + led->target - adcval;
 
-               if (led->pwm >= diff)
-                       led->pwm -= (int16_t)diff;
-               else
-                       led->pwm = 0;
-               led->err_sum += diff * pwm_div;
-       }
+       if (sum < 0)
+               sum = 0;
+
+       led->pwm = sum >> shift;
+       sum -= led->pwm << shift;
+       led->err_sum = sum;
 
        if (led->state == ST_PROBING) {
                if (led->pwm == old_pwm) {