Assert/Alarm system that works correctly from all interrupt levels

diff --git a/src/avr/src/command.c b/src/avr/src/command.c
index 257285ee585f2be78e20a87255655a9b0caaaf88..9dec5d313a50cb4f4056355b4840ac69c3f60da4 100644 (file)
--- 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 6d139f0cff7e19cb44746d43b0d3c93294813879..652f5c0c5d99fcf46c69eef6e2f59a4bd2992d2b 100644 (file)
--- 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 9c8a01fb1cc7fdb99cc945e0969316515d526f67..120985a6a8e7d450e03a579ed476547af131ec17 100644 (file)
--- 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 b75450447170ff1f336a75422bf72b2d08163ecf..762e9753c39e962a70b5f7a8f1a0616c0213470f 100644 (file)
--- 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 f879e652268291600f443eb7afc496378d4e0d72..8aceadd251a01b40a207c9aa417452953cd578be 100644 (file)
--- 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 5b6f7c0d0a5d2caeda363c2b99b9175a3d0eed99..d1a8eee76a88cd4956873aa8d48351e4f3bdcc5b 100644 (file)
--- 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 <avr/io.h>
@@ -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 b8617c097b0d94cf4da7f0773099f6a119f765ea..1ba52e826844ba0484714983853d157084be202c 100644 (file)
--- 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 9c67adec99b8a0bf2a7433660c769d9987131b73..702eda5357db2732287872bce98cf6adf319950e 100644 (file)
--- 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 00a130e7d4bce808cbbdc53c0078445041b0ba53..f9fda83a4610107bd60bc9bdc5e57de9bf8c02a8 100644 (file)
--- 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 019e95c07696b1000266b1da9b9fd72eea5f3804..9424d8472fc29483ef2e991e44b8d17ab7798914 100644 (file)
--- 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