From 937a3a736aaf2c468c8c8e8dbc7963a87eae890f Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 24 May 2022 21:56:59 +0200 Subject: move some code to a shared folder --- shared/consts.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 shared/consts.h (limited to 'shared/consts.h') diff --git a/shared/consts.h b/shared/consts.h new file mode 100644 index 0000000..70efcac --- /dev/null +++ b/shared/consts.h @@ -0,0 +1,17 @@ +#pragma once + +#ifndef W2_BUILD_STR +// is defined by CFLAGS += -DW2_BUILD_STR in makefile +#define W2_BUILD_STR ("????????") +#endif + +/** max logic module execution time in milliseconds */ +#define W2_MAX_MODULE_CYCLE_MS (20) +/** serial baud rate (bit/s) */ +#define W2_SERIAL_BAUD (9600) +/** size of the error handling buffer (in errors, not bytes) */ +#define W2_ERROR_BUFFER_SIZE (16) +/** size of the serial communication buffer (in messages, not bytes) */ +#define W2_SERCOMM_BUFFER_SIZE (16) +/** size of input (receive) buffer (in bytes) */ +#define W2_SERIAL_READ_BUFFER_SIZE (255) -- cgit v1.2.3 From f353e78916a57fe21084fe34fd80c13ae4844d32 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 12:42:34 +0200 Subject: implement info command --- robot/hypervisor.c | 17 ++++++++++++++++- robot/hypervisor.h | 6 ++++++ robot/makefile | 2 +- robot/sercomm.c | 26 ++++++++++++++++++++++++-- robot/setup.c | 3 +++ shared/consts.h | 2 ++ shared/util.c | 6 ++++++ shared/util.h | 3 +++ 8 files changed, 61 insertions(+), 4 deletions(-) create mode 100644 shared/util.c create mode 100644 shared/util.h (limited to 'shared/consts.h') diff --git a/robot/hypervisor.c b/robot/hypervisor.c index c4e7aba..0baa406 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -1,11 +1,17 @@ #include "hypervisor.h" +#include "../shared/util.h" #include "errcatch.h" #include "io.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" -uint64_t g_w2_hypervisor_cycles = 0; +uint64_t g_w2_hypervisor_cycles = 0; +uint64_t g_w2_hypervisor_uptime_ms = 0; +unsigned long g_w2_hypervisor_ema_sercomm_ms = 0; +unsigned long g_w2_hypervisor_ema_errcatch_ms = 0; +unsigned long g_w2_hypervisor_ema_io_ms = 0; +unsigned long g_w2_hypervisor_ema_mode_ms = 0; void w2_hypervisor_main() { #ifdef W2_SIM @@ -13,6 +19,7 @@ void w2_hypervisor_main() { if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n"); #endif + g_w2_hypervisor_uptime_ms += get_ms(); time_reset(); w2_sercomm_main(); @@ -24,6 +31,14 @@ void w2_hypervisor_main() { w2_modes_main(); unsigned long mode_time = get_ms() - io_time; + // calculate exponential moving averages + g_w2_hypervisor_ema_sercomm_ms = + w2_util_exp_mov_avg(g_w2_hypervisor_ema_sercomm_ms, sercomm_time); + g_w2_hypervisor_ema_errcatch_ms = + w2_util_exp_mov_avg(g_w2_hypervisor_ema_errcatch_ms, errcatch_time); + g_w2_hypervisor_ema_io_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_io_ms, io_time); + g_w2_hypervisor_ema_mode_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_mode_ms, mode_time); + if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); #ifdef W2_SIM diff --git a/robot/hypervisor.h b/robot/hypervisor.h index 35fa64c..5008c8f 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -3,6 +3,12 @@ #include extern uint64_t g_w2_hypervisor_cycles; +extern uint64_t g_w2_hypervisor_uptime_ms; + +extern unsigned long g_w2_hypervisor_ema_sercomm_ms; +extern unsigned long g_w2_hypervisor_ema_errcatch_ms; +extern unsigned long g_w2_hypervisor_ema_io_ms; +extern unsigned long g_w2_hypervisor_ema_mode_ms; /** * backbone of all other modules diff --git a/robot/makefile b/robot/makefile index 6de886c..a6e96ee 100644 --- a/robot/makefile +++ b/robot/makefile @@ -32,7 +32,7 @@ OBJ2HEX=$(PREFIX)objcopy # debug build info string BUILD_STR=$(shell git update-index -q --refresh; git describe --tags --dirty='*' --broken='x' | cut -c1-20) -CFLAGS += -DW2_BUILD_STR="$(BUILD_STR)" +CFLAGS += -DW2_BUILD_STR=\"$(BUILD_STR)\" clean:: rm -f *.o out.hex a.out diff --git a/robot/sercomm.c b/robot/sercomm.c index 293e3c6..a3ccdb0 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -3,6 +3,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" +#include "hypervisor.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -53,7 +54,7 @@ void w2_cmd_ping_rx(w2_s_bin *data) { size_t return_size = sizeof(w2_s_cmd_ping_tx); w2_s_cmd_ping_tx *return_message = malloc(return_size); - return_message->opcode = (message->opcode & W2_CMD_DIRECTION_MASK) | W2_CMDDIR_TX; + return_message->opcode = W2_CMD_PING | W2_CMDDIR_TX; return_message->id = message->id; w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); @@ -81,7 +82,28 @@ void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } void w2_cmd_sens_rx(w2_s_bin *data) { return; } -void w2_cmd_info_rx(w2_s_bin *data) { return; } +void w2_cmd_info_rx(w2_s_bin *data) { + w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + size_t return_size = sizeof(w2_s_cmd_info_tx); + w2_s_cmd_info_tx *return_message = malloc(return_size); + return_message->opcode = W2_CMD_INFO | W2_CMDDIR_TX; + strncpy((char *)return_message->build_str, W2_BUILD_STR, sizeof(return_message->build_str)); + return_message->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms; + return_message->io_ms = (uint8_t)g_w2_hypervisor_ema_io_ms; + return_message->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_ms; + return_message->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; + return_message->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); + + w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); + + w2_sercomm_append_msg(return_message_bin); + + free(message); + free(return_message); + free(return_message_bin); +} void w2_cmd_disp_rx(w2_s_bin *data) { return; } diff --git a/robot/setup.c b/robot/setup.c index f8468bf..d075c41 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -29,6 +29,9 @@ void w2_setup_main() { serial_set_baud_rate(W2_SERIAL_BAUD); serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); + // reset timer + time_reset(); + // indicate startup done play("L50 c>c"); } diff --git a/shared/consts.h b/shared/consts.h index 70efcac..f9d4c11 100644 --- a/shared/consts.h +++ b/shared/consts.h @@ -15,3 +15,5 @@ #define W2_SERCOMM_BUFFER_SIZE (16) /** size of input (receive) buffer (in bytes) */ #define W2_SERIAL_READ_BUFFER_SIZE (255) +/** exponential moving average new measurement weight (double 0-1) */ +#define W2_EMA_WEIGHT (0.10) diff --git a/shared/util.c b/shared/util.c new file mode 100644 index 0000000..55f3491 --- /dev/null +++ b/shared/util.c @@ -0,0 +1,6 @@ +#include "consts.h" + +unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas) { + return (unsigned long)((((double)(current_avg)) * ((double)(1.f - W2_EMA_WEIGHT))) + + (((double)(new_meas)) * ((double)(W2_EMA_WEIGHT)))); +} diff --git a/shared/util.h b/shared/util.h new file mode 100644 index 0000000..95f4c68 --- /dev/null +++ b/shared/util.h @@ -0,0 +1,3 @@ +#pragma once + +unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas); -- cgit v1.2.3 From 1913a240aab3a2ad72d477aa6fff840afdcad7a3 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 15:20:55 +0200 Subject: implement mode history --- robot/errcatch.c | 2 +- robot/modes.c | 30 +++++++++++++++++++++++++----- robot/modes.h | 11 ++++++----- robot/sercomm.c | 2 +- robot/setup.c | 3 +++ shared/consts.h | 2 ++ shared/errors.h | 2 ++ shared/util.h | 4 ++++ 8 files changed, 44 insertions(+), 12 deletions(-) (limited to 'shared/consts.h') diff --git a/robot/errcatch.c b/robot/errcatch.c index e44acfd..8df90b8 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -51,7 +51,7 @@ void w2_errcatch_handle_error(w2_s_error *error) { uint8_t severity = error->code & W2_E_TYPE_MASK; // trigger emergency mode for critical errors - if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_switch(W2_M_HALT); + if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_call(W2_M_HALT); // TODO: handle more error types switch (error->code) { diff --git a/robot/modes.c b/robot/modes.c index 1f36703..8d3a099 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,15 +1,31 @@ +#include + +#include "../shared/util.h" +#include "errcatch.h" #include "modes.h" #include "sercomm.h" -void (*g_w2_current_mode)() = &w2_mode_halt; +/** function pointer to current mode */ +// static void (*g_w2_current_mode)() = &w2_mode_halt; + +static void (*g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE])(); +static uint8_t g_w2_mode_history_index = 0; + +void w2_modes_main() { (*g_w2_mode_history[g_w2_mode_history_index])(); } -void w2_modes_main() { (*g_w2_current_mode)(); } +void w2_modes_switch(w2_e_mode new_mode, bool replace) { + int16_t next_history_index = + g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * (replace - 1); + if (next_history_index == -1 || next_history_index == W2_MODE_HISTORY_BUFFER_SIZE - 1) { + next_history_index = W2_RANGE(0, next_history_index, W2_MODE_HISTORY_BUFFER_SIZE); + w2_errcatch_throw(W2_E_WARN_MODE_HISTORY_BUFFER_IOB); + } -void w2_modes_switch(w2_e_mode new_mode) { if (new_mode == W2_M_PREV) { - // TODO implement previous mode buffer + g_w2_mode_history_index = next_history_index; } else { - g_w2_current_mode = W2_MODES[new_mode]; + g_w2_mode_history_index = next_history_index; + g_w2_mode_history[g_w2_mode_history_index] = W2_MODES[new_mode]; } // forward mode change to sercomm @@ -22,3 +38,7 @@ void w2_modes_switch(w2_e_mode new_mode) { free(msg); free(msg_bin); } + +void w2_modes_call(w2_e_mode mode) { w2_modes_switch(mode, false); } + +void w2_modes_swap(w2_e_mode mode) { w2_modes_switch(mode, true); } diff --git a/robot/modes.h b/robot/modes.h index a9d96d8..3423a0f 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -1,5 +1,7 @@ #pragma once +#include "../shared/consts.h" + #include "mode_chrg.h" #include "mode_dirc.h" #include "mode_grid.h" @@ -9,9 +11,6 @@ #include "mode_scal.h" #include "mode_spin.h" -/** function pointer to current mode */ -extern void (*g_w2_current_mode)(); - /** * mode logic * @@ -38,5 +37,7 @@ static const void(*const W2_MODES[]) = { &w2_mode_chrg, &w2_mode_dirc, &w2_mode_spin, &w2_mode_scal, }; -/** switch the current mode */ -void w2_modes_switch(w2_e_mode new_mode); +/** switch current mode (allow switching back to previous mode) */ +void w2_modes_call(w2_e_mode mode); +/** switch current mode (replace current mode keeping history index) */ +void w2_modes_swap(w2_e_mode mode); diff --git a/robot/sercomm.c b/robot/sercomm.c index e972f28..c9c6194 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -71,7 +71,7 @@ void w2_cmd_mode_rx(w2_s_bin *data) { w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); memcpy(message, data->data, data->bytes); - w2_modes_switch(message->mode); + w2_modes_swap(message->mode); } void w2_cmd_sped_rx(w2_s_bin *data) { return; } diff --git a/robot/setup.c b/robot/setup.c index 1ecda43..06c8fa4 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -31,6 +31,9 @@ void w2_setup_main() { // reset timer time_reset(); + // set default mode + w2_modes_swap(W2_M_HALT); + // indicate startup done play("L50 c>c"); } diff --git a/shared/consts.h b/shared/consts.h index f9d4c11..25ca94f 100644 --- a/shared/consts.h +++ b/shared/consts.h @@ -17,3 +17,5 @@ #define W2_SERIAL_READ_BUFFER_SIZE (255) /** exponential moving average new measurement weight (double 0-1) */ #define W2_EMA_WEIGHT (0.10) +/** size of mode history buffer */ +#define W2_MODE_HISTORY_BUFFER_SIZE (4) diff --git a/shared/errors.h b/shared/errors.h index 44b29a0..ac8e95f 100644 --- a/shared/errors.h +++ b/shared/errors.h @@ -47,6 +47,8 @@ typedef enum { W2_E_WARN_SERIAL_TIMEOUT = 0x08 | W2_E_TYPE_WARN, /** unknown message encountered (noisy channel?) */ 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, } w2_e_errorcode; /** diff --git a/shared/util.h b/shared/util.h index 95f4c68..9e4d8ac 100644 --- a/shared/util.h +++ b/shared/util.h @@ -1,3 +1,7 @@ #pragma once +#define W2_MIN(a, b) (((a) < (b)) ? (a) : (b)) +#define W2_MAX(a, b) (((a) > (b)) ? (a) : (b)) +#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); -- cgit v1.2.3