From 47672b09d88fa0078362c72f0c5b6907ce9d205a Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 9 Oct 2018 13:23:38 -0700 Subject: [PATCH] Assert/Alarm system that works correctly from all interrupt levels --- src/avr/src/command.c | 2 +- src/avr/src/estop.h | 4 ++++ src/avr/src/exec.c | 10 ++++++---- src/avr/src/i2c.c | 2 +- src/avr/src/messages.def | 7 +++++++ src/avr/src/modbus.c | 6 ++++-- src/avr/src/motor.c | 8 ++++---- src/avr/src/status.c | 9 --------- src/avr/src/status.h | 10 ---------- src/avr/src/stepper.c | 18 +++++++++--------- 10 files changed, 36 insertions(+), 40 deletions(-) diff --git a/src/avr/src/command.c b/src/avr/src/command.c index 257285e..9dec5d3 100644 --- a/src/avr/src/command.c +++ b/src/avr/src/command.c @@ -241,7 +241,7 @@ void command_reset_position() { // Returns true if command queued -// Called from interrupt +// Called by exec.c from low-level interrupt bool command_exec() { if (!cmd.count) { cmd.last_empty = rtc_get_time(); diff --git a/src/avr/src/estop.h b/src/avr/src/estop.h index 6d139f0..652f5c0 100644 --- a/src/avr/src/estop.h +++ b/src/avr/src/estop.h @@ -36,3 +36,7 @@ void estop_init(); bool estop_triggered(); void estop_trigger(stat_t reason); void estop_clear(); + + +#define ESTOP_ASSERT(COND, CODE) \ + do {if (!(COND)) estop_trigger(CODE);} while (0) diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c index 9c8a01f..120985a 100644 --- a/src/avr/src/exec.c +++ b/src/avr/src/exec.c @@ -110,9 +110,10 @@ void exec_set_cb(exec_cb_t cb) {ex.cb = cb;} void exec_move_to_target(const float target[]) { - ASSERT(isfinite(target[AXIS_X]) && isfinite(target[AXIS_Y]) && - isfinite(target[AXIS_Z]) && isfinite(target[AXIS_A]) && - isfinite(target[AXIS_B]) && isfinite(target[AXIS_C])); + ESTOP_ASSERT(isfinite(target[AXIS_X]) && isfinite(target[AXIS_Y]) && + isfinite(target[AXIS_Z]) && isfinite(target[AXIS_A]) && + isfinite(target[AXIS_B]) && isfinite(target[AXIS_C]), + STAT_BAD_FLOAT); // Update position copy_vector(ex.position, target); @@ -186,7 +187,7 @@ stat_t _segment_exec() { stat_t exec_segment(float time, const float target[], float vel, float accel, float maxVel, float maxAccel, float maxJerk) { - ASSERT(time <= SEGMENT_TIME); + ESTOP_ASSERT(time <= SEGMENT_TIME, STAT_SHORT_SEG_TIME); copy_vector(ex.seg.target, target); ex.seg.time += time; @@ -204,6 +205,7 @@ stat_t exec_segment(float time, const float target[], float vel, float accel, } +// Called by stepper.c from low-level interrupt stat_t exec_next() { // Hold if we've reached zero velocity between commands and stopping if (!ex.cb && !exec_get_velocity() && state_get() == STATE_STOPPING) diff --git a/src/avr/src/i2c.c b/src/avr/src/i2c.c index b754504..762e975 100644 --- a/src/avr/src/i2c.c +++ b/src/avr/src/i2c.c @@ -119,7 +119,7 @@ static uint8_t _i2c_default_write_cb(uint8_t offset, bool *done) { void i2c_init() { i2c_set_write_callback(_i2c_default_write_cb); - I2C_DEV.SLAVE.CTRLA = TWI_SLAVE_INTLVL_HI_gc | TWI_SLAVE_DIEN_bm | + I2C_DEV.SLAVE.CTRLA = TWI_SLAVE_INTLVL_MED_gc | TWI_SLAVE_DIEN_bm | TWI_SLAVE_ENABLE_bm | TWI_SLAVE_APIEN_bm | TWI_SLAVE_PIEN_bm; I2C_DEV.SLAVE.ADDR = I2C_ADDR << 1; } diff --git a/src/avr/src/messages.def b/src/avr/src/messages.def index f879e65..8aceadd 100644 --- a/src/avr/src/messages.def +++ b/src/avr/src/messages.def @@ -47,3 +47,10 @@ STAT_MSG(ALL_ZERO_SCURVE_TIMES, "All zero s-curve times") STAT_MSG(NEGATIVE_SCURVE_TIME, "Negative s-curve time") STAT_MSG(SEEK_NOT_ENABLED, "Switch not enabled") STAT_MSG(SEEK_NOT_FOUND, "Switch not found") +STAT_MSG(MOTOR_ID_INVALID, "Invalid motor ID") +STAT_MSG(MOTOR_NOT_PREPPED, "Motor move not prepped") +STAT_MSG(MOTOR_NOT_READY, "Motor not ready for move") +STAT_MSG(STEPPER_NULL_MOVE, "Null move in stepper driver") +STAT_MSG(STEPPER_NOT_READY, "Stepper driver not ready for move") +STAT_MSG(SHORT_SEG_TIME, "Short segment time") +STAT_MSG(MODBUS_BUF_LENGTH, "Modbus invalid buffer length") diff --git a/src/avr/src/modbus.c b/src/avr/src/modbus.c index 5b6f7c0..d1a8eee 100644 --- a/src/avr/src/modbus.c +++ b/src/avr/src/modbus.c @@ -30,6 +30,7 @@ #include "status.h" #include "rtc.h" #include "util.h" +#include "estop.h" #include "config.h" #include @@ -370,8 +371,9 @@ void modbus_func(uint8_t func, uint8_t send, const uint8_t *data, state.last_write = 0; state.retry = 0; - ASSERT(state.command_length <= MODBUS_BUF_SIZE); - ASSERT(state.response_length <= MODBUS_BUF_SIZE); + ESTOP_ASSERT(state.command_length <= MODBUS_BUF_SIZE, STAT_MODBUS_BUF_LENGTH); + ESTOP_ASSERT(state.response_length <= MODBUS_BUF_SIZE, + STAT_MODBUS_BUF_LENGTH); state.command[0] = cfg.id; state.command[1] = func; diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index b8617c0..1ba52e8 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -256,7 +256,7 @@ void motor_load_move(int motor) { motor_t *m = &motors[motor]; // Clear move - ASSERT(m->prepped); + ESTOP_ASSERT(m->prepped, STAT_MOTOR_NOT_PREPPED); m->prepped = false; motor_end_move(motor); @@ -290,11 +290,11 @@ void motor_load_move(int motor) { void motor_prep_move(int motor, float target) { // Validate input - ASSERT(0 <= motor && motor < MOTORS); - ASSERT(isfinite(target)); + ESTOP_ASSERT(0 <= motor && motor < MOTORS, STAT_MOTOR_ID_INVALID); + ESTOP_ASSERT(isfinite(target), STAT_BAD_FLOAT); motor_t *m = &motors[motor]; - ASSERT(!m->prepped); + ESTOP_ASSERT(!m->prepped, STAT_MOTOR_NOT_READY); // Travel in steps int32_t position = _position_to_steps(motor, target); diff --git a/src/avr/src/status.c b/src/avr/src/status.c index 9c67ade..702eda5 100644 --- a/src/avr/src/status.c +++ b/src/avr/src/status.c @@ -93,12 +93,3 @@ stat_t status_message_P(const char *location, status_level_t level, return code; } - - -/// Alarm state; send an exception report and stop processing input -stat_t status_alarm(const char *location, stat_t code, const char *msg) { - status_message_P(location, STAT_LEVEL_ERROR, code, msg); - estop_trigger(code); - while (!usart_tx_empty()) continue; - return code; -} diff --git a/src/avr/src/status.h b/src/avr/src/status.h index 00a130e..f9fda83 100644 --- a/src/avr/src/status.h +++ b/src/avr/src/status.h @@ -59,9 +59,6 @@ const char *status_level_pgmstr(status_level_t level); stat_t status_message_P(const char *location, status_level_t level, stat_t code, const char *msg, ...); -/// Enter alarm state. returns same status code -stat_t status_alarm(const char *location, stat_t status, const char *msg); - #define TO_STRING(x) _TO_STRING(x) #define _TO_STRING(x) #x @@ -82,13 +79,6 @@ stat_t status_alarm(const char *location, stat_t status, const char *msg); #define STATUS_ERROR(CODE, MSG, ...) \ STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__) -#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE, 0) -#define ASSERT(COND) \ - do { \ - if (!(COND)) \ - status_alarm(STATUS_LOCATION, STAT_INTERNAL_ERROR, PSTR(#COND)); \ - } while (0) - #ifdef DEBUG #define DEBUG_CALL(FMT, ...) \ diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index 019e95c..9424d84 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -117,12 +117,13 @@ ISR(STEP_LOW_LEVEL_ISR) { case STAT_AGAIN: continue; // No command executed, try again case STAT_OK: // Move executed - if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued + if (!st.move_queued) + estop_trigger(STAT_EXPECTED_MOVE); // No move was queued st.move_queued = false; st.move_ready = true; break; - default: ALARM(status); break; + default: estop_trigger(status); break; } break; @@ -169,12 +170,11 @@ static void _load_move() { else _end_move(); - if (st.move_type != MOVE_TYPE_NULL) { - st.busy = true; + ESTOP_ASSERT(st.move_type != MOVE_TYPE_NULL, STAT_STEPPER_NULL_MOVE); + st.busy = true; - // Start dwell - st.dwell = st.prep_dwell; - } + // Start dwell + st.dwell = st.prep_dwell; // We are done with this move st.move_type = MOVE_TYPE_NULL; @@ -194,7 +194,7 @@ ISR(STEP_TIMER_ISR) {_load_move();} void st_prep_line(const float target[]) { // Trap conditions that would prevent queuing the line - ASSERT(!st.move_ready); + ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY); // Setup segment parameters st.move_type = MOVE_TYPE_LINE; @@ -210,7 +210,7 @@ void st_prep_line(const float target[]) { /// Add a dwell to the move buffer void st_prep_dwell(float seconds) { - ASSERT(!st.move_ready); + ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY); st.move_type = MOVE_TYPE_DWELL; st.prep_dwell = seconds; st.move_queued = true; // signal prep buffer ready -- 2.27.0