From e3bdcf1423fef9c2e77b410357ad150d60a85b44 Mon Sep 17 00:00:00 2001 From: "Jan \"Yenya\" Kasprzak" Date: Fri, 30 Nov 2012 02:23:45 +0100 Subject: [PATCH] pwmled.c: rework Probing and time-averaging works, but several corner cases need to be polished. --- firmware/pwmled.c | 306 ++++++++++++++++------------------------------ 1 file changed, 106 insertions(+), 200 deletions(-) diff --git a/firmware/pwmled.c b/firmware/pwmled.c index d5d947b..23231c9 100644 --- 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? - } -} -- 2.39.3