diff options
| -rw-r--r-- | robot/hypervisor.c | 17 | ||||
| -rw-r--r-- | robot/hypervisor.h | 6 | ||||
| -rw-r--r-- | robot/makefile | 2 | ||||
| -rw-r--r-- | robot/sercomm.c | 26 | ||||
| -rw-r--r-- | robot/setup.c | 3 | ||||
| -rw-r--r-- | shared/consts.h | 2 | ||||
| -rw-r--r-- | shared/util.c | 6 | ||||
| -rw-r--r-- | shared/util.h | 3 |
8 files changed, 61 insertions, 4 deletions
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 <stdint.h> 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); |