diff options
Diffstat (limited to 'robot')
| -rw-r--r-- | robot/errcatch.c | 13 | ||||
| -rw-r--r-- | robot/sim.c | 8 | ||||
| -rw-r--r-- | robot/sim.h | 4 |
3 files changed, 21 insertions, 4 deletions
diff --git a/robot/errcatch.c b/robot/errcatch.c index 4bdbaef..99f1063 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -5,6 +5,7 @@ #include "halt.h" #include "modes.h" #include "orangutan_shim.h" +#include "sercomm.h" w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {}; uint8_t g_w2_error_index = 0; @@ -66,7 +67,17 @@ void w2_errcatch_handle_error(w2_s_error *error) { } } - // TODO: forward error to sercomm + // forward error to sercomm + size_t msg_size = sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length; + w2_s_cmd_expt_tx *msg = malloc(msg_size); + msg->opcode = W2_CMD_EXPT | W2_CMDDIR_TX; + msg->error = error->code; + msg->length = error->message_length; + memcpy(msg->message, error->message, error->message_length); + w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg); + w2_sercomm_append_msg(msg_bin); + free(msg); + free(msg_bin); return; } diff --git a/robot/sim.c b/robot/sim.c index 0cde6a0..baf8a8a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -9,6 +9,7 @@ #include "../shared/consts.h" #include "../shared/protocol.h" #include "sercomm.h" +#include "errcatch.h" struct timespec reference_time; // NOLINT bool g_w2_sim_headless = false; @@ -74,10 +75,10 @@ void serial_send(char* message, unsigned int length) { putc(message[byte] & 0xff, stdout); return; } - if (!DBG_ENABLE_PRINTFUNC) return; - simprintfunc("serial_send", "<see below>, %u", length); + if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<see below>, %u", length); + if (!DBG_ENABLE_SERIAL) return; w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message); w2_sim_print_serial(bin); free(bin); @@ -103,6 +104,9 @@ void w2_sim_setup(int argc, char **argv) { term.c_cc[VTIME] = 0; term.c_cc[VMIN] = 0; tcsetattr(STDIN_FILENO, 0, &term); + + // debug error + // w2_errcatch_throw(W2_E_WARN_BATTERY_LOW); } void w2_sim_cycle_begin() { diff --git a/robot/sim.h b/robot/sim.h index 6e2498d..25cc713 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -10,10 +10,12 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (1) +#define DBG_ENABLE_PRINTFUNC (0) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) +#define DBG_ENABLE_SERIAL (1) + #define DBG_MAX_CYCLES (10) // debug print options |