From: Joseph Coffland Date: Thu, 8 Feb 2018 19:35:46 +0000 (-0800) Subject: Improved pwr firmware X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=d87aa509415f83fbfa73e90fe03b893efffd032b;p=bbctrl-firmware Improved pwr firmware --- diff --git a/src/pwr/config.h b/src/pwr/config.h index 19ccc3b..0da6d6c 100644 --- a/src/pwr/config.h +++ b/src/pwr/config.h @@ -84,7 +84,6 @@ enum { #define VOLTAGE_SETTLE_PERIOD 20 // ms #define VOLTAGE_SETTLE_TOLERANCE 0.01 #define VOLTAGE_EXP 0.01 -#define FAULT_TIMEOUT 5000 // ms #define SHUNT_WATTS 5 #define SHUNT_OHMS 10 @@ -121,7 +120,9 @@ typedef enum { enum { - UNDER_VOLTAGE_FLAG = 1 << 0, - OVER_VOLTAGE_FLAG = 1 << 1, - OVER_CURRENT_FLAG = 1 << 2, + UNDER_VOLTAGE_FLAG = 1 << 0, + OVER_VOLTAGE_FLAG = 1 << 1, + OVER_CURRENT_FLAG = 1 << 2, + MEASUREMENT_ERROR_FLAG = 1 << 3, + SHUNT_OVERLOAD_FLAG = 1 << 4, }; diff --git a/src/pwr/main.c b/src/pwr/main.c index 4c9c4d5..9a81ae2 100644 --- a/src/pwr/main.c +++ b/src/pwr/main.c @@ -44,12 +44,12 @@ static const uint8_t ch_schedule[] = { CS2_ADC, VOUT_ADC, CS3_ADC, VOUT_ADC, CS4_ADC, VOUT_ADC, - 0 }; static volatile uint16_t regs[NUM_REGS] = {0}; static volatile uint64_t time = 0; // ms +static volatile bool shunt_overload = false; static volatile float shunt_ms_power = 0; static volatile float vnom = 0; @@ -128,19 +128,11 @@ static void update_shunt() { // Remove power dissipation credit watts -= shunt_ms_power; - if (watts < 0) watts = 0; + if (watts < 0) shunt_overload = true; - // Enable shunt output when requested if allowed - if (shunt_ms_power) { - IO_DDR_SET(SHUNT_PIN); // Enable shunt output - - if (watts) IO_DDR_SET(MOTOR_PIN); // Enable motor output - else IO_DDR_CLR(MOTOR_PIN); // Disable motor output - - } else { - IO_DDR_CLR(SHUNT_PIN); // Disable output - IO_DDR_SET(MOTOR_PIN); // Enable motor output - } + // Enable shunt when requested + if (shunt_ms_power) IO_DDR_SET(SHUNT_PIN); // Enable + else IO_DDR_CLR(SHUNT_PIN); // Disable } @@ -217,7 +209,7 @@ static void adc_conversion() { static int i = 0; read_conversion(ch_schedule[i]); - if (!ch_schedule[++i]) i = 0; + if (++i == sizeof(ch_schedule)) i = 0; // Start next conversion ADMUX = (ADMUX & 0xf0) | ch_schedule[i]; @@ -256,7 +248,7 @@ static void validate_input_voltage() { static void charge_caps() { TCCR0A |= (1 << COM0A1) | (0 << COM0A0); // Clear on compare match IO_PORT_CLR(MOTOR_PIN); // Motor voltage off - IO_DDR_SET(MOTOR_PIN); // Output + IO_DDR_SET(MOTOR_PIN); // Output uint64_t now = time; for (int i = 0; i < CAP_CHARGE_TIME; i++) { @@ -321,6 +313,37 @@ void init() { } +static void shutdown(uint16_t flags) { + regs[FLAGS_REG] = flags; + + // Disable timers + TCCR0B = TCCR1B = 0; + + // Disable outputs + IO_DDR_CLR(SHUNT_PIN); + IO_DDR_CLR(MOTOR_PIN); + IO_PORT_CLR(LOAD1_PIN); + IO_PORT_CLR(LOAD2_PIN); + IO_DDR_SET(LOAD1_PIN); + IO_DDR_SET(LOAD2_PIN); + + while (true) continue; +} + + +static void validate_measurements() { + const float max_voltage = 0.99 * convert_voltage(0x3ff); + const float max_current = 0.99 * convert_current(0x3ff); + + if (max_voltage < regs[VOUT_REG] || + max_current < regs[MOTOR_REG] || + max_current < regs[LOAD1_REG] || + max_current < regs[LOAD2_REG] || + max_current < regs[VDD_REG]) + shutdown(MEASUREMENT_ERROR_FLAG); +} + + int main() { wdt_enable(WDTO_8S); @@ -328,6 +351,7 @@ int main() { adc_conversion(); // Start ADC validate_input_voltage(); charge_caps(); + validate_measurements(); while (true) { wdt_reset(); @@ -340,25 +364,11 @@ int main() { // Check fault conditions uint16_t flags = 0; if (vin < VOLTAGE_MIN) flags |= UNDER_VOLTAGE_FLAG; - if (VOLTAGE_MAX < vin) flags |= OVER_VOLTAGE_FLAG; + if (VOLTAGE_MAX < vin || VOLTAGE_MAX < vout) flags |= OVER_VOLTAGE_FLAG; if (CURRENT_MAX < get_total_current()) flags |= OVER_CURRENT_FLAG; - regs[FLAGS_REG] = flags; - - if (flags) { - // Disable - IO_PORT_CLR(MOTOR_PIN); - IO_PORT_CLR(LOAD1_PIN); - IO_PORT_CLR(LOAD2_PIN); - IO_DDR_SET(LOAD1_PIN); - IO_DDR_SET(LOAD2_PIN); - - delay_ms(FAULT_TIMEOUT); - - // Reenable - charge_caps(); - IO_DDR_CLR(LOAD1_PIN); - IO_DDR_CLR(LOAD2_PIN); - } + if (shunt_overload) flags |= SHUNT_OVERLOAD_FLAG; + + if (flags) shutdown(flags); } return 0; diff --git a/src/py/bbctrl/Pwr.py b/src/py/bbctrl/Pwr.py index d1659aa..50847b0 100644 --- a/src/py/bbctrl/Pwr.py +++ b/src/py/bbctrl/Pwr.py @@ -12,6 +12,16 @@ VOUT_REG = 2 MOTOR_REG = 3 LOAD1_REG = 4 LOAD2_REG = 5 +VDD_REG = 6 +FLAGS_REG = 7 + +UNDER_VOLTAGE_FLAG = 1 << 0 +OVER_VOLTAGE_FLAG = 1 << 1 +OVER_CURRENT_FLAG = 1 << 2 +MEASUREMENT_ERROR_FLAG = 1 << 3 +SHUNT_OVERLOAD_FLAG = 1 << 4 + +reg_names = 'temp vin vout motor load1 load2 vdd pwr_flags'.split() class Pwr(): @@ -19,7 +29,7 @@ class Pwr(): self.ctrl = ctrl self.i2c_addr = ctrl.args.pwr_addr - self.regs = [-1] * 6 + self.regs = [-1] * 8 self.lcd_page = ctrl.lcd.add_new_page() self._update() @@ -28,6 +38,20 @@ class Pwr(): def get_reg(self, i): return self.regs[i] + def error(self): + flags = self.regs[FLAGS_REG] + errors = [] + + if flags & UNDER_VOLTAGE_FLAG: errors.push('under voltage') + if flags & OVER_VOLTAGE_FLAG: errors.push('over voltage') + if flags & OVER_CURRENT_FLAG: errors.push('over current') + if flags & MEASUREMENT_ERROR_FLAG: errors.push('measurement error') + if flags & SHUNT_OVERLOAD_FLAG: errors.push('shunt overload') + + # Report errors + self.ctrl.state.set('pwr_errors', errors) + + def _update(self): update = {} @@ -38,25 +62,29 @@ class Pwr(): if i == TEMP_REG: value -= 273 else: value /= 100.0 - key = ['temp', 'vin', 'vout', 'motor', 'load1', 'load2'][i] + key = reg_names[i] self.ctrl.state.set(key, value) if self.regs[i] != value: update[key] = value self.regs[i] = value + if i == FLAGS_REG and value: self.error() + except Exception as e: log.warning('Pwr communication failed: %s' % e) self.ctrl.ioloop.call_later(1, self._update) return - self.lcd_page.text('%3dC' % self.regs[TEMP_REG], 0, 0) - self.lcd_page.text('%5.1fV In' % self.regs[VIN_REG], 0, 1) - self.lcd_page.text('%5.1fV Out' % self.regs[VOUT_REG], 0, 2) + self.lcd_page.text('%3dC Tmp' % self.regs[TEMP_REG], 0, 0) + self.lcd_page.text('%5.1fV In' % self.regs[VIN_REG], 0, 1) + self.lcd_page.text('%5.1fV Out' % self.regs[VOUT_REG], 0, 2) + self.lcd_page.text(' %02d Flg' % self.regs[FLAGS_REG], 0, 3) self.lcd_page.text('%5.1fA Mot' % self.regs[MOTOR_REG], 10, 0) self.lcd_page.text('%5.1fA Ld1' % self.regs[LOAD1_REG], 10, 1) self.lcd_page.text('%5.1fA Ld2' % self.regs[LOAD2_REG], 10, 2) + self.lcd_page.text('%5.1fA Vdd' % self.regs[VDD_REG], 10, 3) if len(update): self.ctrl.state.update(update) diff --git a/src/resources/config-template.json b/src/resources/config-template.json index 6f2d802..15f3d7e 100644 --- a/src/resources/config-template.json +++ b/src/resources/config-template.json @@ -58,7 +58,7 @@ "type": "float", "min": 0, "unit": "m/min²", - "default": 100, + "default": 1000, "code": "am" }, "max-jerk": {