aboutsummaryrefslogtreecommitdiff
path: root/shared
diff options
context:
space:
mode:
Diffstat (limited to 'shared')
-rw-r--r--shared/bin.h7
-rw-r--r--shared/bool.h2
-rw-r--r--shared/consts.h7
-rw-r--r--shared/errcatch.c45
-rw-r--r--shared/errcatch.h (renamed from shared/errors.h)37
-rw-r--r--shared/modes.h16
-rw-r--r--shared/protocol.h1
-rw-r--r--shared/serial_parse.c20
-rw-r--r--shared/util.c2
-rw-r--r--shared/util.h1
10 files changed, 136 insertions, 2 deletions
diff --git a/shared/bin.h b/shared/bin.h
index 48485c8..4f7db81 100644
--- a/shared/bin.h
+++ b/shared/bin.h
@@ -13,6 +13,13 @@
extern uint8_t g_w2_endianness;
+#define W2_CAST_BIN(type, in, out) type *out = (type *)&in->data;
+#define W2_CREATE_MSG_BIN(type, normal, bin) W2_CREATE_MSG_SIZE_BIN(type, sizeof(type), normal, bin)
+#define W2_CREATE_MSG_SIZE_BIN(type, size, normal, bin) \
+ w2_s_bin *bin = malloc(sizeof(w2_s_bin) + size); \
+ bin->bytes = size; \
+ type *normal = (type *)&bin->data;
+
typedef struct {
uint16_t bytes;
uint8_t data[];
diff --git a/shared/bool.h b/shared/bool.h
index 9ea97f7..4ffb6b4 100644
--- a/shared/bool.h
+++ b/shared/bool.h
@@ -4,6 +4,8 @@
/** @file bool.h */
+#ifndef bool
typedef uint8_t bool;
#define false 0 /* NOLINT */
#define true 1 /* NOLINT */
+#endif
diff --git a/shared/consts.h b/shared/consts.h
index cdd96b3..cd6dff1 100644
--- a/shared/consts.h
+++ b/shared/consts.h
@@ -22,7 +22,7 @@
/** size of the serial communication buffer (in messages, not bytes) */
#define W2_SERCOMM_BUFFER_SIZE (16)
/** size of mode history buffer */
-#define W2_MODE_HISTORY_BUFFER_SIZE (4)
+#define W2_MODE_HISTORY_BUFFER_SIZE (8)
/** max logic module execution time in milliseconds */
#define W2_MAX_MODULE_CYCLE_MS (20)
@@ -30,6 +30,11 @@
/** exponential moving average new measurement weight (double 0-1) */
#define W2_EMA_WEIGHT (0.10)
+/** minimal time between pings */
+#define W2_PING_FREQUENCY (1e3)
+/** max time between ping and answer */
+#define W2_PING_TIMEOUT (5e3)
+
/** front-facing distance sensor pinout */
#define W2_FRONT_SENSOR_PIN 5
/** battery voltage sensor pinout */
diff --git a/shared/errcatch.c b/shared/errcatch.c
new file mode 100644
index 0000000..76af10a
--- /dev/null
+++ b/shared/errcatch.c
@@ -0,0 +1,45 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "errcatch.h"
+
+w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {};
+uint8_t g_w2_error_index = 0;
+uint8_t g_w2_error_offset = 0;
+bool g_w2_error_buffer_full = 0;
+bool g_w2_error_uncaught = 0;
+
+void w2_errcatch_main() {
+ while (g_w2_error_offset != g_w2_error_index) {
+ w2_s_error *error = g_w2_error_buffer[g_w2_error_offset];
+ w2_errcatch_handle_error(error);
+ g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE;
+ }
+ if (g_w2_error_buffer_full) {
+ w2_errcatch_throw(W2_E_WARN_ERR_BUFFER_FULL);
+ g_w2_error_buffer_full = 0;
+ }
+ if (g_w2_error_uncaught) {
+ w2_errcatch_throw(W2_E_WARN_UNCAUGHT_ERROR);
+ g_w2_error_uncaught = 0;
+ }
+}
+
+w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message) {
+ w2_s_error *error = malloc(sizeof(w2_s_error) + length);
+ memcpy(error, &(w2_s_error const){.code = code, .message_length = length}, sizeof(w2_s_error));
+ strncpy(error->message, message, length);
+
+ return error;
+}
+
+void w2_errcatch_throw(w2_e_errorcode code) { w2_errcatch_throw_msg(code, 0, ""); }
+void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message) {
+ uint8_t next_index = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE;
+ g_w2_error_buffer_full = next_index == g_w2_error_offset;
+ free(g_w2_error_buffer[g_w2_error_index]);
+ w2_s_error *error = w2_alloc_error(code, length, message);
+ g_w2_error_buffer[g_w2_error_index] = error;
+ if (g_w2_error_buffer_full) return;
+ g_w2_error_index = next_index;
+}
diff --git a/shared/errors.h b/shared/errcatch.h
index 344a506..a56bc00 100644
--- a/shared/errors.h
+++ b/shared/errcatch.h
@@ -1,9 +1,12 @@
#pragma once
-/** @file errors.h */
+/** @file errcatch.h */
#include <stdint.h>
+#include "bool.h"
+#include "consts.h"
+
#define W2_E_TYPE_MASK (0b11 << 6)
#define W2_E_TYPE_CRIT (0b00 << 6)
@@ -49,6 +52,8 @@ typedef enum {
W2_E_WARN_SERIAL_NOISY = 0x09 | W2_E_TYPE_WARN,
/** mode history index out of bounds */
W2_E_WARN_MODE_HISTORY_BUFFER_IOB = 0x0a | W2_E_TYPE_WARN,
+ /** ping timeout reached */
+ W2_E_WARN_PING_TIMEOUT = 0x0b | W2_E_TYPE_WARN,
} w2_e_errorcode;
/**
@@ -62,3 +67,33 @@ typedef struct {
uint8_t message_length;
char message[];
} w2_s_error;
+
+/** error ring buffer */
+extern w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE];
+/** stores head of ring buffer */
+extern uint8_t g_w2_error_index;
+/** stores start of ring buffer */
+extern uint8_t g_w2_error_offset;
+/** error buffer full flag */
+extern bool g_w2_error_buffer_full;
+/** uncaught error flag */
+extern bool g_w2_error_uncaught;
+
+/** error-handler module main */
+void w2_errcatch_main();
+
+/** handle error */
+void w2_errcatch_handle_error(w2_s_error *error);
+
+/** append error to error buffer */
+void w2_errcatch_throw(w2_e_errorcode code);
+
+/** append error to error buffer (with debug message) */
+void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message);
+
+/**
+ * allocate and initialize error struct
+ *
+ * TODO: doesn't handle null pointers from malloc
+ */
+w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message);
diff --git a/shared/modes.h b/shared/modes.h
new file mode 100644
index 0000000..ff939ea
--- /dev/null
+++ b/shared/modes.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#define W2_MODE_COUNT 8
+
+/** mode constants */
+typedef enum {
+ W2_M_PREV = -1,
+ W2_M_MAZE = 0,
+ W2_M_GRID = 1,
+ W2_M_HALT = 2,
+ W2_M_LCAL = 3,
+ W2_M_CHRG = 4,
+ W2_M_DIRC = 5,
+ W2_M_SPIN = 6,
+ W2_M_SCAL = 7,
+} w2_e_mode;
diff --git a/shared/protocol.h b/shared/protocol.h
index 93e53f4..02d5526 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -155,6 +155,7 @@ typedef struct {
uint8_t sercomm_ms;
uint8_t mode_ms;
uint32_t uptime_s;
+ uint8_t mode;
} w2_s_cmd_info_tx;
typedef struct {
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index b1b4f50..d07c67f 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -1,7 +1,11 @@
#include <string.h>
#include "consts.h"
+#include "errcatch.h"
#include "serial_parse.h"
+#ifdef W2_SIM
+#include "../robot/orangutan_shim.h"
+#endif
bool w2_serial_parse(uint8_t byte) {
static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0};
@@ -38,3 +42,19 @@ bool w2_serial_parse(uint8_t byte) {
return W2_SERIAL_READ_SUCCESS;
}
+
+void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+ w2_s_bin *copy = w2_bin_s_alloc(data_length, data);
+ void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]];
+
+ if (handler == NULL) {
+ w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY);
+ } else {
+#ifdef W2_SIM
+ if (DBG_ENABLE_SERIAL) w2_sim_print_serial(copy);
+#endif
+ handler(copy);
+ }
+
+ free(copy);
+}
diff --git a/shared/util.c b/shared/util.c
index 55f3491..68503e8 100644
--- a/shared/util.c
+++ b/shared/util.c
@@ -4,3 +4,5 @@ unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_m
return (unsigned long)((((double)(current_avg)) * ((double)(1.f - W2_EMA_WEIGHT))) +
(((double)(new_meas)) * ((double)(W2_EMA_WEIGHT))));
}
+
+int w2_sign(int n) { return (n > 0) - (n < 0); }
diff --git a/shared/util.h b/shared/util.h
index 465e043..230c3e4 100644
--- a/shared/util.h
+++ b/shared/util.h
@@ -7,3 +7,4 @@
#define W2_RANGE(min, val, max) W2_MIN(max, W2_MAX(val, min))
unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas);
+int w2_sign(int n);