X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?p=bike-lights.git;a=blobdiff_plain;f=firmware%2Fpwmled.c;h=03d1adb5957df2711b6c7bedf11a9c696ccdec57;hp=b76f219fb32b1c597358832f333be26c621d50f4;hb=3e13c7f4abd65b939c1645d77cf6ed5e0124808e;hpb=e971b58770e16921fab0d94fcca8be4ff0a057f5 diff --git a/firmware/pwmled.c b/firmware/pwmled.c index b76f219..03d1adb 100644 --- a/firmware/pwmled.c +++ b/firmware/pwmled.c @@ -2,288 +2,318 @@ #include "lights.h" -static uint16_t pwm_vals[N_PWMLEDS*N_PWMLED_MODES]; -static uint16_t pwm_max[N_PWMLEDS] = { - PWM_MAX/2, - PWM_MAX - (PWM_MAX >> 4), // step-up - PWM_MAX/2 -}; +typedef struct { + uint16_t target, pwm; + int16_t err_sum; + unsigned char mode, state; + union { + unsigned char probe_steady, mode_changed; + }; + uint16_t mode_pwm[N_PWMLED_MODES]; + int16_t err_sums[N_PWMLED_MODES]; + unsigned char modes_not_yet_stable; +} pwmled_t; + +pwmled_t pwmleds[N_PWMLEDS]; + +/* + * Mode stabilization: + * when changing brightness via pwmled_set_brightness() below, + * we want to converge to the target value as fast as possible. Also, + * we would like to somehow initialize the mode 3, which is used as + * "mode 2 + other PWMLED on". So after the brightness is set, + * we also set pwmleds[n].modes_not_yet_stable to MODE_STABILIZATION_TIME. + * When modes_not_yet_stable is non-zero, we allow only mode 2 to be set + * regardless of what is fed to pwmled_set_mode. We will then converge + * to the target value of mode 2 only, and after MODE_STABILIZATION_TIME + * ADC measurements, we copy the mode_pwm value to all other modes. + * Only then it is allowed to set the other modes. + */ +#define MODE_STABILIZATION_TIME (2*16) // two seconds worth of measurements #define PWMLED2_TESTING_WITH_350MA_LED #define SENSE_MOHM 33 /* 0.033 Ohm */ -#define MA_MOHM_GAIN_TO_ADC(ma, mohm, gain) (\ - ((unsigned long)(ma))*(mohm) /* voltage at sensing resistor in uV */ \ - /(1100000UL/gain/1024UL) /* voltage of ADC reading == 1 */ \ -) +/* + * Voltage in uV at ADC reading == 1 is 1100/gain/1024 + * ADC module returns sum of 1 << PWMLED_ADC_SHIFT measurements + * Voltage in uV measured is current in mA * sense resistance in mOhm + */ +#define MA_GAIN_TO_ADC(ma, gain) ((uint16_t) \ + ((uint32_t)(ma) \ + * (SENSE_MOHM) \ + * (1 << (PWMLED_ADC_SHIFT)) \ + * 1024 \ + / (1100000/(gain)))) + 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), -#ifdef PWMLED2_TESTING_WITH_350MA_LED - MA_MOHM_GAIN_TO_ADC( 400, SENSE_MOHM, 1) +#ifdef TESTING_FW + MA_GAIN_TO_ADC( 400, 20), + MA_GAIN_TO_ADC( 30, 20), + MA_GAIN_TO_ADC( 800, 1) #else - MA_MOHM_GAIN_TO_ADC(2500, SENSE_MOHM, 1) + MA_GAIN_TO_ADC( 900, 20), + MA_GAIN_TO_ADC( 30, 20), + MA_GAIN_TO_ADC(2500, 1) #endif }; -static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES] = { - /* pwmled0 */ - MA_MOHM_GAIN_TO_ADC( 20, SENSE_MOHM, 20), - MA_MOHM_GAIN_TO_ADC( 50, SENSE_MOHM, 20), - MA_MOHM_GAIN_TO_ADC( 100, SENSE_MOHM, 20), - MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM, 20), - /* pwmled1 */ - 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), - /* pwmled2 */ -#ifdef PWMLED2_TESTING_WITH_350MA_LED - MA_MOHM_GAIN_TO_ADC( 100, SENSE_MOHM, 1), - MA_MOHM_GAIN_TO_ADC( 140, SENSE_MOHM, 1), - MA_MOHM_GAIN_TO_ADC( 250, SENSE_MOHM, 1), - MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM, 1), + +static uint16_t adc_targets_0[] = { + MA_GAIN_TO_ADC( 50, 20), + MA_GAIN_TO_ADC( 80, 20), + MA_GAIN_TO_ADC( 160, 20), + MA_GAIN_TO_ADC( 350, 20), +}; + +static uint16_t adc_targets_1[] = { + MA_GAIN_TO_ADC( 5, 20), + MA_GAIN_TO_ADC( 10, 20), + MA_GAIN_TO_ADC( 20, 20), +}; + +static uint16_t adc_targets_2[] = { +#ifdef TESTING_FW + MA_GAIN_TO_ADC( 120, 1), + MA_GAIN_TO_ADC( 160, 1), + MA_GAIN_TO_ADC( 240, 1), + MA_GAIN_TO_ADC( 320, 1), + MA_GAIN_TO_ADC( 460, 1), #else - MA_MOHM_GAIN_TO_ADC( 150, SENSE_MOHM, 1), - MA_MOHM_GAIN_TO_ADC( 350, SENSE_MOHM, 1), - MA_MOHM_GAIN_TO_ADC( 700, SENSE_MOHM, 1), - MA_MOHM_GAIN_TO_ADC(2400, SENSE_MOHM, 1), + MA_GAIN_TO_ADC( 150, 1), + MA_GAIN_TO_ADC( 300, 1), + MA_GAIN_TO_ADC( 500, 1), + MA_GAIN_TO_ADC( 700, 1), + MA_GAIN_TO_ADC(1500, 1), + MA_GAIN_TO_ADC(2500, 1), #endif }; -// TODO: maybe convert this to bitmask to simplify pwmled_needs_adc() ? -static unsigned char pwmled_state[N_PWMLEDS]; +static uint16_t adc_vals[N_PWMLEDS*N_PWMLED_MODES]; + #define ST_DISABLED 0 -#define ST_PROBING 1 -#define ST_OFF 2 +#define ST_OFF 1 +#define ST_PROBING 2 #define ST_ON 3 +// The above are constructed so that the following work: +#define ST_IS_ON(s) ((s) & 0x02) +#define ST_CAN_SET_MODE(s) ((s) & 0x01) -static unsigned char pwmled_mode[N_PWMLEDS]; -static unsigned char pwmled_mode_set[N_PWMLEDS]; +void init_pwmled() +{ + unsigned char i, j; -static uint16_t pwm_probes[N_PWMLEDS]; -static int16_t differences[N_PWMLEDS]; + for (i = 0; i < N_PWMLEDS; i++) { + pwmled_t *led = pwmleds + i; + led->err_sum = 0; + led->target = adc_vals[i*N_PWMLED_MODES]; + led->mode = 1; + led->probe_steady = 0; + led->state = ST_OFF; + led->pwm = 1; + pwm_set(i, led->pwm); + + for (j = 0; j < N_PWMLED_MODES; j++) { + led->mode_pwm[j] = 0; + led->err_sums[j] = 0; + } + } -static void start_probing(unsigned char n) -{ - pwmled_state[n] = ST_PROBING; - pwm_set(n, 0); - pwm_probes[n] = 0; + pwmled_set_brightness(PWMLED_BRIGHTNESS(0, 2, 1, 0, 2)); } -void pwmled_init() +void pwmled_set_mode(unsigned char n, unsigned char mode) { - unsigned char i; + pwmled_t *led = pwmleds + n; - for (i = 0; i < N_PWMLEDS*N_PWMLED_MODES; i++) { - pwm_vals[i] = 0; - pwmled_mode[i] = 0; - pwmled_mode_set[i] = 0; + if (!ST_CAN_SET_MODE(led->state)) + return; + + if (led->mode) { // save the previous state + led->mode_pwm[led->mode - 1] = led->pwm; + led->err_sums[led->mode - 1] = led->err_sum; } - for (i = 0; i < N_PWMLEDS; i++) { - differences[i] = 0; - start_probing(i); + led->mode = mode; + + if (mode > 0 && mode <= N_PWMLED_MODES) { + if (led->modes_not_yet_stable) // only mode 2 when !stable + mode = 2; + led->target = adc_vals[n*N_PWMLED_MODES + mode - 1]; + led->state = ST_ON; + led->pwm = led->mode_pwm[mode - 1]; + led->err_sum = led->err_sums[mode - 1]; + led->mode_changed = 1; + pwm_set(n, led->pwm); + } else { + led->state = ST_OFF; + pwm_off(n); } } -unsigned char pwmled_needs_adc(unsigned char n) -{ - unsigned char st = pwmled_state[n]; - if (st == ST_PROBING || st == ST_ON) - return 1; - else - return 0; -} +#define CHECK_BRIGHTNESS(var, expr, array) \ + do { \ + (var) = (expr); \ + if ((var) >= sizeof(array)/sizeof(array[0])) \ + (var) = sizeof(array)/sizeof(array[0]) - 1; \ + } while (0) -unsigned char pwmled_enabled(unsigned char n) +void pwmled_set_brightness(uint16_t brightness) { - unsigned char st = pwmled_state[n]; - if (st == ST_OFF || st == ST_ON) - return 1; - else - return 0; + unsigned char i; + + CHECK_BRIGHTNESS(i, brightness & 0x7, adc_targets_0); + adc_vals[0] = adc_targets_0[i]; + CHECK_BRIGHTNESS(i, (brightness >> 3) & 0x7, adc_targets_0); + if (adc_vals[1] != adc_targets_0[i]) { + adc_vals[1] = adc_targets_0[i]; + pwmleds[0].modes_not_yet_stable = MODE_STABILIZATION_TIME; + } + adc_vals[2] = adc_vals[1]; + + CHECK_BRIGHTNESS(i, (brightness >> 6) & 0x7, adc_targets_1); + // we use only one mode, so no modes_not_yet_stable handling here + adc_vals[3] = adc_targets_1[i]; + adc_vals[4] = adc_vals[3]; + adc_vals[5] = adc_vals[3]; + + CHECK_BRIGHTNESS(i, (brightness >> 9) & 0x7, adc_targets_2); + adc_vals[6] = adc_targets_2[i]; + CHECK_BRIGHTNESS(i, (brightness >> 12) & 0x7, adc_targets_2); + if (adc_vals[7] != adc_targets_2[i]) { + adc_vals[7] = adc_targets_2[i]; + pwmleds[2].modes_not_yet_stable = MODE_STABILIZATION_TIME; + } + adc_vals[8] = adc_vals[7]; + + for (i = 0; i < N_PWMLEDS; i++) { + pwmleds[i].err_sum = 0; + pwmled_set_mode(i, pwmleds[i].mode); + } } -void pwmled_set_mode(unsigned char n, unsigned char mode) +#define PWMLED_PROBE_STEADY_COUNT 10 + +static inline unsigned char pwmled_probed_ok(unsigned char n, uint16_t old_pwm) { - if (!pwmled_enabled(n)) - return; + pwmled_t *led = pwmleds + n; -#if 0 - log_byte(0xF8); - log_byte(n); - log_byte(mode); -#endif - 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; + if (led->pwm == old_pwm) { + if (led->probe_steady < PWMLED_PROBE_STEADY_COUNT) + led->probe_steady++; } else { - pwm_off(n); - pwmled_state[n] = ST_OFF; + led->probe_steady = 0; } -} -static void inline probing_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]; + if (led->probe_steady < PWMLED_PROBE_STEADY_COUNT + && old_pwm <= led->pwm) + return 0; -#if 0 - log_byte(0xF4); - log_byte(n); - log_word(adcval); -#endif + // probed OK + led->mode_pwm[led->mode - 1] = led->pwm; + led->err_sums[led->mode - 1] = 0; + + // 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; - if (adcval > adc_max[n] // Too high - || (pwm == 0 && adcval > 0) // non-zero voltage with zero PWM - ) { + led->state = ST_OFF; pwm_off(n); - pwmled_state[n] = ST_DISABLED; + log_byte(0xF0); log_byte(n); - log_word(adcval); - return; - } + log_word(jiffies); - 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; - } - } + for (i = 0; i < N_PWMLED_MODES; i++) + log_word(led->mode_pwm[i]); -#if 0 - if ((n == 1 && pwm > 0x35) || adcval != 0) { - log_byte(n); - log_byte(0xF3); - log_byte(pwm); - log_word(adcval); - } -#endif + log_flush(); - if (!need_bigger) { // successfully probed - pwm_off(n); - // pwm_set(n, 0); - pwmled_state[n] = ST_OFF; - log_byte(0xF1); - log_byte(n); + pattern_reload(); - return; + return 1; } +} - 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; - } +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(); - // try to increase - pwm++; - pwm_probes[n] = pwm; - pwm_set(n, pwm); + switch (n) { + case 0: err_flags.err_pwmled0 = 1; break; + case 1: err_flags.err_pwmled1 = 1; break; + case 2: err_flags.err_pwmled2 = 1; break; + } } -// Feedback loop -static void inline on_adc(unsigned char n, uint16_t adcval) + +void pwmled_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 0 - log_byte(0xF5); - log_byte(n); - log_word(adcval); -#endif + pwmled_t *led = pwmleds + n; + uint16_t old_pwm; + int32_t sum; + unsigned char shift; + if (!ST_IS_ON(led->state)) + return; - if (pwmled_mode_set[n]) { // ignore the first reading - pwmled_mode_set[n] = 0; + if (led->state == ST_ON && led->mode_changed) { + led->mode_changed--; return; } + // FIXME: test for maximum adcval value (adc_max[n]) - differences[n] += adcval; - differences[n] -= adc_exp; + old_pwm = led->pwm; - 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; + // shift = led->state == ST_PROBING ? 3 : 8; + shift = 3; - if (new_pwm == old_pwm) - return; + sum = ((int32_t)led->pwm << shift) + + led->err_sum + led->target - adcval; - differences[n] = 0; + if (sum < 0) + sum = 0; - if (new_pwm > (int16_t)pwm_max[n]) { - // FIXME: disconnected? - new_pwm = pwm_max[n]; - } + led->pwm = sum >> shift; + sum -= led->pwm << shift; + led->err_sum = sum; - if (new_pwm < 1) { - // FIXME: short-circuit? - new_pwm = 1; + if (led->pwm >= PWM_MAX + || (n == 1 && led->pwm > PWM_MAX/2 && adcval < 0x08)) { + pwmled_err(n); + return; } - *pwm_p = new_pwm; - pwm_set(n, new_pwm); - - if (jiffies > 500 && n == 1) { - log_byte(adcval & 0xFF); - log_byte(new_pwm); + if (led->state == ST_PROBING) + if (pwmled_probed_ok(n, old_pwm)) + return; + + if (led->modes_not_yet_stable) { + if (!--led->modes_not_yet_stable) { + // reached stability, copy mode 2 to mode 3 (-1) + led->mode_pwm[0] = led->pwm; + led->mode_pwm[2] = led->pwm; + led->err_sums[0] = 0; + led->err_sums[2] = 0; + } } -} -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) { - 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); - } -#endif - - return; - case ST_ON: - on_adc(n, adcval); + if (led->pwm == old_pwm) return; - // WTF am I doing in this function then? Maybe recently switched off? - } + + pwm_set(n, led->pwm); } +