diff options
| -rw-r--r-- | protocol.md | 10 | ||||
| -rw-r--r-- | robot/errcatch.c | 13 | ||||
| -rw-r--r-- | robot/sim.c | 8 | ||||
| -rw-r--r-- | robot/sim.h | 4 |
4 files changed, 26 insertions, 9 deletions
diff --git a/protocol.md b/protocol.md index 915fa65..7084d14 100644 --- a/protocol.md +++ b/protocol.md @@ -33,7 +33,7 @@ readability. |code|name|implemented|directions|full name| |--:|---|:-:|:-:|---| |`0x00`|[PING](#ping)|yes|`r <=> c`|<u>ping</u> -|`0x02`|[EXPT](#expt)|no|`r --> c`|<u>ex</u>ce<u>pt</u>ion +|`0x02`|[EXPT](#expt)|yes|`r --> c`|<u>ex</u>ce<u>pt</u>ion |`0x04`|[MODE](#mode)|no|`r <=> c`|<u>mode</u> |`0x06`|[SPED](#sped)|no|`r <-- c`|<u>spe</u>e<u>d</u> |`0x08`|[DIRC](#dirc)|no|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol @@ -294,10 +294,10 @@ requests robot info |-:|-| |`uint8_t`|opcode (`0x14 + 1`)| |`uint8_t[32]`|build string| -|`uint8_t`|errcatch module cycle time (ms)| -|`uint8_t`|io module cycle time (ms)| -|`uint8_t`|sercomm module cycle time (ms)| -|`uint8_t`|modes module cycle time (ms)| +|`uint8_t`|exponential moving average errcatch module cycle time (ms)| +|`uint8_t`|exponential moving average io module cycle time (ms)| +|`uint8_t`|exponential moving average sercomm module cycle time (ms)| +|`uint8_t`|exponential moving average modes module cycle time (ms)| |`uint32_t`|total robot uptime (s)| robot info response 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 |