From: Jan "Yenya" Kasprzak <kas@fi.muni.cz>
Date: Fri, 7 Dec 2012 21:03:49 +0000 (+0100)
Subject: pwmled.c: save error sum between mode switches
X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?p=bike-lights.git;a=commitdiff_plain;h=b075b225aa743ebb6ce09ca2d1be42e81a6c9ad8

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 3e4e1f4..d70013e 100644
--- 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) {