pwmled.c: factored out probing to a separate function

[bike-lights.git] / firmware / pwmled.c

diff --git a/firmware/pwmled.c b/firmware/pwmled.c
index 23231c9858cf80abfe6a2f777e59cc04b1a1e8ea..076d032c7b4a713e4a9db98e79ddf6381ed835ab 100644 (file)
--- a/firmware/pwmled.c
+++ b/firmware/pwmled.c
@@ -5,8 +5,9 @@
 typedef struct {
        uint16_t target, pwm;
        int16_t err_sum;
-       unsigned char mode, state, probe_steps;
+       unsigned char mode, state, probe_steady;
        uint16_t mode_pwm[N_PWMLED_MODES];
+       int16_t err_sums[N_PWMLED_MODES];
 } pwmled_t;
 
 pwmled_t pwmleds[N_PWMLEDS];
@@ -68,7 +69,7 @@ static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES] = {
 #define ST_IS_ON(s)    ((s) & 0x02)
 #define ST_CAN_SET_MODE(s)     ((s) & 0x01)
 
-void pwmled_init()
+void init_pwmled()
 {
        unsigned char i, j;
 
@@ -79,10 +80,12 @@ void pwmled_init()
                led->pwm = 0;
                led->mode = 1;
                led->state = ST_PROBING;
-               led->probe_steps = 0;
+               led->probe_steady = 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;
@@ -113,85 +115,107 @@ void pwmled_set_mode(unsigned char n, unsigned char mode)
        }
 }
 
-void pwmled_adc(unsigned char n, uint16_t adcval)
+#define PWMLED_PROBE_STEADY_COUNT 10
+
+static inline unsigned char pwmled_probed_ok(unsigned char n, uint16_t old_pwm)
 {
        pwmled_t *led = pwmleds + n;
-       int16_t sum, pwm_div;
-       uint16_t old_pwm;
 
-       if (!ST_IS_ON(led->state))
-               return;
+       if (led->pwm == old_pwm) {
+               if (led->probe_steady < PWMLED_PROBE_STEADY_COUNT)
+                       led->probe_steady++;
+       } else {
+               led->probe_steady = 0;
+       }
 
-       // FIXME: test for maximum adcval value (adc_max[n])
+       if (led->probe_steady < PWMLED_PROBE_STEADY_COUNT
+               && old_pwm <= led->pwm)
+               return 0;
 
-       sum = led->err_sum += (int16_t)(led->target) - (int16_t)adcval;
+       // probed OK
+       led->mode_pwm[led->mode - 1] = led->pwm;
+       led->err_sums[led->mode - 1] = 0;
 
-       // FIXME: try to work faster during probing
-       pwm_div = 64; //led->state == ST_PROBING ? (1 << 5) : (1 << 8);
-       old_pwm = led->pwm;
+       // next mode to probe?
+       if (led->mode < N_PWMLED_MODES) {
+               led->probe_steady = 0;
+               led->err_sum = 0;
+
+               led->mode++;
+               led->target = adc_vals[n*N_PWMLED_MODES+led->mode-1];
+
+               return 0;
+       } else {
+               unsigned char i;
+
+               led->state = ST_OFF;
+               pwm_off(n);
+
+               log_byte(0xF0);
+               log_byte(n);
+               log_word(jiffies);
+
+               for (i = 0; i < N_PWMLED_MODES; i++)
+                       log_word(led->mode_pwm[i]);
 
-       if (sum >= pwm_div) {
-               uint16_t diff = sum/pwm_div;
-               led->pwm += diff;
-               led->err_sum -= diff * pwm_div;
+               log_flush();
 
-       } else if (sum <= -pwm_div) {
-               uint16_t diff = (-sum)/pwm_div;
+               pattern_reload();
 
-               if (led->pwm >= diff)
-                       led->pwm -= (int16_t)diff;
-               else
-                       led->pwm = 0;
-               led->err_sum += diff * pwm_div;
+               return 1;
        }
+}
+
+static inline void pwmled_err(unsigned char n)
+{
+       pwmleds[n].state = ST_DISABLED;
+       pwm_off(n);
+
+       log_byte(0xF1);
+       log_byte(n);
+       log_word(jiffies);
+       log_flush();
+}
 
-       if (led->state == ST_PROBING) {
-               if (led->pwm == old_pwm) {
-                       if (led->probe_steps < 10)
-                               led->probe_steps++;
-               } else {
-                       led->probe_steps = 0;
-               }
 
-               if (led->probe_steps >= 10 || old_pwm > led->pwm) {
+void pwmled_adc(unsigned char n, uint16_t adcval)
+{
+       pwmled_t *led = pwmleds + n;
+       uint16_t old_pwm;
+       int32_t sum;
+       unsigned char shift;
+
+       if (!ST_IS_ON(led->state))
+               return;
 
-                       led->mode_pwm[led->mode - 1] = led->pwm;
+       // FIXME: test for maximum adcval value (adc_max[n])
 
-                       if (led->mode < N_PWMLED_MODES) {
-                               led->probe_steps = 0;
-                               led->err_sum = 0;
+       old_pwm = led->pwm;
 
-                               led->mode++;
-                               led->target = adc_vals[n*N_PWMLED_MODES+led->mode-1];
-                       } else {
-                               unsigned char i;
+       shift = led->state == ST_PROBING ? 3 : 5;
 
-                               led->state = ST_OFF;
+       sum = ((int32_t)led->pwm << shift)
+               + led->err_sum + led->target - adcval;
 
-                               log_byte(0xF0);
-                               log_byte(n);
-                               log_word(jiffies);
+       if (sum < 0)
+               sum = 0;
 
-                               for (i = 0; i < N_PWMLED_MODES; i++)
-                                       log_word(led->mode_pwm[i]);
+       led->pwm = sum >> shift;
+       sum -= led->pwm << shift;
+       led->err_sum = sum;
 
-                               log_flush();
-                       }
-               }
+       if (led->pwm > (PWM_MAX << PWM_STEP_SHIFT)-5) {
+               pwmled_err(n);
+               return;
        }
 
+       if (led->state == ST_PROBING)
+               if (pwmled_probed_ok(n, old_pwm))
+                       return;
+
        if (led->pwm == old_pwm)
                return;
 
-       if (led->pwm > (PWM_MAX << PWM_STEP_SHIFT)/2) {
-               pwm_off(n);
-               led->state = ST_DISABLED;
-               log_byte(0xF1);
-               log_byte(n);
-               log_word(jiffies);
-               log_flush();
-       }
-
        pwm_set(n, led->pwm);
 }