From ae91affc30bfa0f078f1de67f4b558b8932ed647 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mario=20H=C3=BCttel?= Date: Sun, 27 Sep 2020 22:54:06 +0200 Subject: [PATCH] issue #5: Implement safety weight checking in control loop --- .../include/reflow-controller/oven-driver.h | 2 + .../reflow-controller/safety/safety-config.h | 4 +- stm-firmware/oven-driver.c | 6 + stm-firmware/pid-controller.c | 1 + stm-firmware/safety/safety-controller.c | 104 +++++++++++------- 5 files changed, 77 insertions(+), 40 deletions(-) diff --git a/stm-firmware/include/reflow-controller/oven-driver.h b/stm-firmware/include/reflow-controller/oven-driver.h index 62825a7..a77e489 100644 --- a/stm-firmware/include/reflow-controller/oven-driver.h +++ b/stm-firmware/include/reflow-controller/oven-driver.h @@ -43,6 +43,8 @@ void oven_pid_handle(float target_temp); void oven_pid_stop(void); +void oven_pid_abort(void); + void oven_driver_apply_power_level(void); enum oven_pid_status oven_pid_get_status(void); diff --git a/stm-firmware/include/reflow-controller/safety/safety-config.h b/stm-firmware/include/reflow-controller/safety/safety-config.h index 889375a..1c9d748 100644 --- a/stm-firmware/include/reflow-controller/safety/safety-config.h +++ b/stm-firmware/include/reflow-controller/safety/safety-config.h @@ -132,7 +132,9 @@ enum analog_value_monitor { ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_STACK, SAFETY_FLAG_CONFIG_WEIGHT_PANIC), \ ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_SAFETY_ADC, SAFETY_FLAG_CONFIG_WEIGHT_PANIC), \ ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_SYSTICK, SAFETY_FLAG_CONFIG_WEIGHT_PANIC), \ - ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_WTCHDG_FIRED, SAFETY_FLAG_CONFIG_WEIGHT_PANIC), \ + /* Watchdog timeout is not handled perioodically, but only on startup. + * Therefore, it is not listed here */\ + ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_WTCHDG_FIRED, SAFETY_FLAG_CONFIG_WEIGHT_NONE), \ ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_UNCAL, SAFETY_FLAG_CONFIG_WEIGHT_NONE), \ ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_DEBUG, SAFETY_FLAG_CONFIG_WEIGHT_NONE), \ ERR_FLAG_WEIGHT_ENTRY(ERR_FLAG_TIMING_MAIN_LOOP, SAFETY_FLAG_CONFIG_WEIGHT_PANIC), \ diff --git a/stm-firmware/oven-driver.c b/stm-firmware/oven-driver.c index 8164219..bca6446 100644 --- a/stm-firmware/oven-driver.c +++ b/stm-firmware/oven-driver.c @@ -118,6 +118,12 @@ void oven_pid_stop(void) safety_controller_enable_timing_mon(ERR_TIMING_PID, false); } +void oven_pid_abort(void) +{ + oven_pid_aborted = true; + oven_pid_stop(); +} + enum oven_pid_status oven_pid_get_status(void) { enum oven_pid_status ret = OVEN_PID_ABORTED; diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c index 3113d1f..b41f0f6 100644 --- a/stm-firmware/pid-controller.c +++ b/stm-firmware/pid-controller.c @@ -77,6 +77,7 @@ float pid_sample(struct pid_controller *pid, float deviation) output = deviation * pid->k_p; + /* PID runaway compensation */ if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5) && !(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5)) { calculate_integral(pid, deviation); diff --git a/stm-firmware/safety/safety-controller.c b/stm-firmware/safety/safety-controller.c index 8cc8168..1b4984c 100644 --- a/stm-firmware/safety/safety-controller.c +++ b/stm-firmware/safety/safety-controller.c @@ -37,57 +37,58 @@ #include #include #include +#include #include #define check_flag_persistent(flag) ((flag)->persistency && (flag)->persistency->persistency) #define get_flag_weight(flag) ((flag)->weight ? (flag->weight->weight) : SAFETY_FLAG_CONFIG_WEIGHT_NONE) struct safety_weight { - uint32_t start_dummy; - enum config_weight weight; - enum safety_flag flag; - volatile struct error_flag *flag_ptr; - uint32_t end_dummy; + uint32_t start_dummy; + enum config_weight weight; + enum safety_flag flag; + volatile struct error_flag *flag_ptr; + uint32_t end_dummy; }; struct safety_persistency { - uint32_t start_dummy; - bool persistency; - enum safety_flag flag; - volatile struct error_flag *flag_ptr; - uint32_t end_dummy; + uint32_t start_dummy; + bool persistency; + enum safety_flag flag; + volatile struct error_flag *flag_ptr; + uint32_t end_dummy; }; struct error_flag { - const char *name; - enum safety_flag flag; - bool error_state; - bool error_state_inv; - volatile struct safety_persistency *persistency; - volatile struct safety_weight *weight; - uint32_t key; + const char *name; + enum safety_flag flag; + bool error_state; + bool error_state_inv; + volatile struct safety_persistency *persistency; + volatile struct safety_weight *weight; + uint32_t key; }; struct timing_mon { - const char *name; - enum timing_monitor monitor; - enum safety_flag associated_flag; - uint64_t min_delta; - uint64_t max_delta; - uint64_t last; - uint64_t calculated_delta; - bool enabled; + const char *name; + enum timing_monitor monitor; + enum safety_flag associated_flag; + uint64_t min_delta; + uint64_t max_delta; + uint64_t last; + uint64_t calculated_delta; + bool enabled; }; struct analog_mon { - const char *name; - enum analog_value_monitor monitor; - enum safety_flag associated_flag; - float min; - float max; - float value; - bool valid; - uint64_t timestamp; + const char *name; + enum analog_value_monitor monitor; + enum safety_flag associated_flag; + float min; + float max; + float value; + bool valid; + uint64_t timestamp; }; static volatile struct error_flag IN_SECTION(.ccm.data) flags[] = { @@ -119,9 +120,9 @@ static volatile struct timing_mon IN_SECTION(.ccm.data) timings[] = { static volatile struct analog_mon IN_SECTION(.ccm.data) analog_mons[] = { ANA_MON_ENTRY(ERR_AMON_VREF, SAFETY_ADC_VREF_MVOLT - SAFETY_ADC_VREF_TOL_MVOLT, - SAFETY_ADC_VREF_MVOLT + SAFETY_ADC_VREF_TOL_MVOLT, ERR_FLAG_AMON_VREF), + SAFETY_ADC_VREF_MVOLT + SAFETY_ADC_VREF_TOL_MVOLT, ERR_FLAG_AMON_VREF), ANA_MON_ENTRY(ERR_AMON_UC_TEMP, SAFETY_ADC_TEMP_LOW_LIM, SAFETY_ADC_TEMP_HIGH_LIM, - ERR_FLAG_AMON_UC_TEMP), + ERR_FLAG_AMON_UC_TEMP), }; static const struct safety_weight default_flag_weights[] = { SAFETY_CONFIG_DEFAULT_WEIGHTS }; @@ -216,6 +217,9 @@ static void init_safety_flag_persistencies_from_default(void) static bool error_flag_get_status(const volatile struct error_flag *flag) { + if (!flag) + return true; + if (flag->error_state == flag->error_state_inv) { return true; } else { @@ -576,6 +580,31 @@ static void safety_controller_do_systick_checking() last_systick = systick; } +static void safety_controller_handle_weighted_flags() +{ + uint32_t weight_index; + volatile struct safety_weight *current_weight; + + for (weight_index = 0; weight_index < COUNT_OF(flag_weights); weight_index++) { + current_weight = &flag_weights[weight_index]; + if (error_flag_get_status(current_weight->flag_ptr)) { + switch (current_weight->weight) { + case SAFETY_FLAG_CONFIG_WEIGHT_NONE: + break; + case SAFETY_FLAG_CONFIG_WEIGHT_PID: + oven_pid_abort(); + break; + case SAFETY_FLAG_CONFIG_WEIGHT_PANIC: + /* Expected fallthrough */ + default: + oven_pid_abort(); + panic_mode(); + break; + } + } + } +} + int safety_controller_handle() { int ret = 0; @@ -583,12 +612,9 @@ int safety_controller_handle() safety_controller_check_stack(); safety_controller_handle_safety_adc(); safety_controller_handle_memory_checks(); - safety_controller_do_systick_checking(); - safety_controller_process_monitor_checks(); - - /* TODO: Check flag weights and trigger appropriate safety action */ + safety_controller_handle_weighted_flags(); ret |= watchdog_ack(WATCHDOG_MAGIC_KEY);