X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?a=blobdiff_plain;f=firmware%2Fpwmled.c;h=d85087edcf81bc35d3765f419e8babfd24217fe2;hb=7f7665a5234d4d5cbd94fed9be55862772f3dc12;hp=3e4e1f495b419872b897cd68800eac9351b5d208;hpb=0d98bdd496538991b106bc64c45df8e89f23ea32;p=bike-lights.git

diff --git a/firmware/pwmled.c b/firmware/pwmled.c
index 3e4e1f4..d85087e 100644
--- 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];
@@ -79,10 +80,12 @@ void init_pwmled()
 		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) {
+		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);
 }