From f00fca5f6f9751b16d868f52bda908c7b4704457 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 17 May 2022 19:26:21 +0200 Subject: changed error code names following @FlamingHotToast's suggestion --- robot/consts.h | 29 +++++++++++++++-------------- robot/errcatch.c | 18 +++++++++--------- robot/errcatch.h | 2 +- robot/hypervisor.c | 2 +- 4 files changed, 26 insertions(+), 25 deletions(-) diff --git a/robot/consts.h b/robot/consts.h index a81908d..1e021e0 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -7,13 +7,14 @@ #define W2_MAX_MODULE_CYCLE_MS (20) #define W2_SERIAL_BAUD (9600) -#define W2_ERROR_BUFFER_SIZE (16) -#define W2_ERR_TYPE_CRIT (0b00 << 6) -#define W2_ERR_TYPE_WARN (0b01 << 6) -#define W2_ERR_TYPE_INFO (0b10 << 6) -#define W2_ERR_TYPE_VERB (0b11 << 6) -#define W2_ERR_TYPE_MASK (0b11 << 6) +#define W2_E_BUFFER_SIZE (16) +#define W2_E_TYPE_MASK (0b11 << 6) + +#define W2_E_TYPE_CRIT (0b00 << 6) +#define W2_E_TYPE_WARN (0b01 << 6) +#define W2_E_TYPE_INFO (0b10 << 6) +#define W2_E_TYPE_VERB (0b11 << 6) /** * enum storing all error codes @@ -23,14 +24,14 @@ */ enum w2_e_errorcodes { // critical error codes - W2_ERR_CONN_LOST = 0x00 | W2_ERR_TYPE_CRIT, - W2_ERR_COM_UNAVAILABLE = 0x01 | W2_ERR_TYPE_CRIT, // client-only - W2_ERR_LINE_LOST = 0x02 | W2_ERR_TYPE_CRIT, - W2_ERR_OBSTACLE_STUCK = 0x03 | W2_ERR_TYPE_CRIT, + W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, + W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, // client-only + W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, + W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, // warnings - W2_ERR_BATTERY_LOW = 0x00 | W2_ERR_TYPE_WARN, - W2_ERR_OBSTACLE_DETECTED = 0x01 | W2_ERR_TYPE_WARN, - W2_ERR_CYCLE_EXPIRED = 0x02 | W2_ERR_TYPE_WARN, - W2_ERR_UNCAUGHT_ERROR = 0x03 | W2_ERR_TYPE_WARN, + W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, + W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, + W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, + W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, }; diff --git a/robot/errcatch.c b/robot/errcatch.c index 0dc60e9..0f97566 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -8,15 +8,15 @@ #include "modes.h" #include "orangutan_shim.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; +w2_s_error *g_w2_error_buffer[W2_E_BUFFER_SIZE] = {}; +uint8_t g_w2_error_index = 0; +uint8_t g_w2_error_offset = 0; void w2_errcatch_main() { while (g_w2_error_index != g_w2_error_offset) { 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; + g_w2_error_offset = (g_w2_error_offset + 1) % W2_E_BUFFER_SIZE; } } @@ -33,22 +33,22 @@ void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const cha 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; - g_w2_error_index = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE; + g_w2_error_index = (g_w2_error_index + 1) % W2_E_BUFFER_SIZE; } void w2_errcatch_handle_error(w2_s_error *error) { - uint8_t severity = error->code & W2_ERR_TYPE_MASK; + uint8_t severity = error->code & W2_E_TYPE_MASK; // trigger emergency mode for critical errors - if ((severity ^ W2_ERR_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt; + if ((severity ^ W2_E_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt; // TODO: handle more error types switch (error->code) { - case W2_ERR_UNCAUGHT_ERROR: { + case W2_E_WARN_UNCAUGHT_ERROR: { break; } default: { - w2_errcatch_throw(W2_ERR_UNCAUGHT_ERROR); + w2_errcatch_throw(W2_E_WARN_UNCAUGHT_ERROR); #ifdef W2_SIM simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code); #endif diff --git a/robot/errcatch.h b/robot/errcatch.h index fa7a15b..98bb7d0 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -17,7 +17,7 @@ typedef struct { } w2_s_error; /** error ring buffer */ -extern w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE]; +extern w2_s_error *g_w2_error_buffer[W2_E_BUFFER_SIZE]; /** stores head of ring buffer */ extern uint8_t g_w2_error_index; /** stores start of ring buffer */ diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 6b32776..dbdc62a 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -27,5 +27,5 @@ void w2_hypervisor_main() { usleep(100e3); #endif - if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED); + if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); } -- cgit v1.2.3 From 365dcc18fbd98645585cdbe009f537ecdaa90c1a Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 17 May 2022 21:21:00 +0200 Subject: WIP sercomm implementation - moved some module-specific constants to their respective header files - changed .clang-tidy to ignore global private global constants (starting with `_`) - suppressed some GCC warnings in bin.c and all pololu library warnings - added function signatures for sercomm protocol data generators - added endianness check in setup.c --- .clang-tidy | 10 ++++++---- protocol.md | 7 +++++-- robot/bin.c | 30 ++++++++++++++++++++++++++++++ robot/bin.h | 27 +++++++++++++++++++++++++++ robot/consts.h | 26 -------------------------- robot/errcatch.h | 27 +++++++++++++++++++++++++++ robot/orangutan_shim.h | 3 +++ robot/sercomm.h | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ robot/setup.c | 9 +++++++++ 9 files changed, 155 insertions(+), 32 deletions(-) create mode 100644 robot/bin.c create mode 100644 robot/bin.h diff --git a/.clang-tidy b/.clang-tidy index 054a943..4c8496f 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,16 +1,18 @@ Checks: '-*,readability-identifier-naming' CheckOptions: + - { key: readability-identifier-naming.EnumCase, value: lower_case } + - { key: readability-identifier-naming.EnumPrefix, value: w2_e_ } - { key: readability-identifier-naming.FunctionCase, value: lower_case } - { key: readability-identifier-naming.FunctionPrefix, value: w2_ } - - { key: readability-identifier-naming.GlobalVariableCase, value: lower_case } - - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_w2_ } - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } + - { key: readability-identifier-naming.GlobalConstantIgnoredRegexp, value: _.* } - { key: readability-identifier-naming.GlobalConstantPrefix, value: W2_ } + - { key: readability-identifier-naming.GlobalVariableCase, value: lower_case } + - { key: readability-identifier-naming.GlobalVariableIgnoredRegexp, value: _.* } + - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_w2_ } - { key: readability-identifier-naming.MacroDefinitionCase, value: UPPER_CASE } - { key: readability-identifier-naming.MacroDefinitionPrefix, value: W2_ } - { key: readability-identifier-naming.StructCase, value: lower_case } - { key: readability-identifier-naming.StructPrefix, value: w2_s_ } - - { key: readability-identifier-naming.EnumCase, value: lower_case } - - { key: readability-identifier-naming.EnumPrefix, value: w2_e_ } # vim: ft=yaml diff --git a/protocol.md b/protocol.md index f5bb0cc..d41b00f 100644 --- a/protocol.md +++ b/protocol.md @@ -25,8 +25,8 @@ is converted to a single `0xff` on the receiving end, so these duplicated bytes and the starting byte don't count towards message length. opcodes are picked sequentially, but the direction bit (LSB) is reserved to -indicate a transfer from robot to client. this means that the opcode for a -sensor data request would be `0x12`, but the response opcode would be `0x13`. +indicate a transfer from robot to client (`tx`). this means that the opcode for +a sensor data request would be `0x12`, but the response opcode would be `0x13`. these opcodes are stored as enum constants inside consts.h for code readability. @@ -54,6 +54,9 @@ a double stroke arrow means that the command can be initiated from either the robot or the client, while a single arrow indicates a request-response structure. +in *both* the robot and client code `r <-- c` is referred to as `rx` and `r +--> c` as `tx` (from the *robots* view). + ### PING #### ping (`r <=> c`) (2 bytes) diff --git a/robot/bin.c b/robot/bin.c new file mode 100644 index 0000000..4261b50 --- /dev/null +++ b/robot/bin.c @@ -0,0 +1,30 @@ +#include "bin.h" + +#define W2_ENDIAN_LITTLE (1) +#define W2_ENDIAN_BIG (0) + +#define _SHIFT_0B (8 * 0) +#define _SHIFT_1B (8 * 1) +#define _SHIFT_2B (8 * 2) +#define _SHIFT_3B (8 * 3) +#define _BYTE_0 ((uint32_t)(0xff << (_SHIFT_0B))) +#define _BYTE_1 ((uint32_t)(0xff << (_SHIFT_1B))) +#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B))) +#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B))) + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wshift-count-overflow" +uint32_t w2_bin_hton32(uint32_t h32) { + if(g_w2_endianness == W2_ENDIAN_BIG) return h32; + return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) + | ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); +} +#pragma GCC diagnostic pop + +uint16_t w2_bin_hton16(uint16_t h16) { + if(g_w2_endianness == W2_ENDIAN_BIG) return h16; + return ((h16 & _BYTE_0) << _SHIFT_1B) | ((h16 & _BYTE_1) >> _SHIFT_1B); +} + +uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } +uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } diff --git a/robot/bin.h b/robot/bin.h new file mode 100644 index 0000000..1f21f64 --- /dev/null +++ b/robot/bin.h @@ -0,0 +1,27 @@ +#pragma once + +#include + +typedef struct { + uint16_t bytes; + uint8_t data[]; +} w2_s_bin; + +extern uint8_t g_w2_endianness; + + +/** + * helper file for binary data + * + * - fix endianness with functions inspired by UNIX arpa/inet.h + */ + +/** convert 32-bit value from host endian to network (big-endian) */ +uint32_t w2_bin_hton32(uint32_t h32); +/** convert 16-bit value from host endian to network (big-endian) */ +uint16_t w2_bin_hton16(uint16_t h16); +/** convert 32-bit value from network (big-endian) to host endian */ +uint32_t w2_bin_ntoh32(uint32_t n32); +/** convert 16-bit value from network (big-endian) to host endian */ +uint16_t w2_bin_ntoh16(uint16_t n16); + diff --git a/robot/consts.h b/robot/consts.h index 1e021e0..3558da0 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -7,31 +7,5 @@ #define W2_MAX_MODULE_CYCLE_MS (20) #define W2_SERIAL_BAUD (9600) - #define W2_E_BUFFER_SIZE (16) -#define W2_E_TYPE_MASK (0b11 << 6) - -#define W2_E_TYPE_CRIT (0b00 << 6) -#define W2_E_TYPE_WARN (0b01 << 6) -#define W2_E_TYPE_INFO (0b10 << 6) -#define W2_E_TYPE_VERB (0b11 << 6) - -/** - * enum storing all error codes - * - * error codes are between 0-63 because the two most significant bits are - * reserved for error type checking - */ -enum w2_e_errorcodes { - // critical error codes - W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, - W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, // client-only - W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, - W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, - // warnings - W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, - W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, - W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, - W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, -}; diff --git a/robot/errcatch.h b/robot/errcatch.h index 98bb7d0..ec4b90f 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -4,6 +4,33 @@ #include "consts.h" +#define W2_E_TYPE_MASK (0b11 << 6) + +#define W2_E_TYPE_CRIT (0b00 << 6) +#define W2_E_TYPE_WARN (0b01 << 6) +#define W2_E_TYPE_INFO (0b10 << 6) +#define W2_E_TYPE_VERB (0b11 << 6) + +/** + * enum storing all error codes + * + * error codes are between 0-63 because the two most significant bits are + * reserved for error type checking + */ +enum w2_e_errorcodes { + // critical error codes + W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, + W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, // client-only + W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, + W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, + + // warnings + W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, + W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, + W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, + W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, +}; + /** * error struct * diff --git a/robot/orangutan_shim.h b/robot/orangutan_shim.h index de57c98..cd624d0 100644 --- a/robot/orangutan_shim.h +++ b/robot/orangutan_shim.h @@ -3,5 +3,8 @@ #ifdef W2_SIM #include "sim.h" #else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" #include +#pragma GCC diagnostic pop #endif diff --git a/robot/sercomm.h b/robot/sercomm.h index 58c79b9..1533a4c 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -1,5 +1,27 @@ #pragma once +#include "bin.h" + +#define W2_CMD_RX (0) +#define W2_CMD_TX (1) + +enum w2_e_serial_commands { + W2_E_CMD_PING = 0x00, + W2_E_CMD_EXPT = 0x02, + W2_E_CMD_MODE = 0x04, + W2_E_CMD_SPED = 0x06, + W2_E_CMD_DIRC = 0x08, + W2_E_CMD_CORD = 0x0a, + W2_E_CMD_BOMD = 0x0c, + W2_E_CMD_SRES = 0x0e, + W2_E_CMD_MCFG = 0x10, + W2_E_CMD_SENS = 0x12, + W2_E_CMD_INFO = 0x14, + W2_E_CMD_DISP = 0x16, + W2_E_CMD_PLAY = 0x18, + W2_E_CMD_CLED = 0x1a, +}; + /** * serial pc-robot communication module * @@ -7,3 +29,29 @@ * - sends all data in the message buffer */ void w2_sercomm_main(); + +void w2_sercomm_append_msg(w2_s_bin data); + +w2_s_bin w2_sercomm_rx_generic(); +w2_s_bin w2_sercomm_tx_generic(); + +w2_s_bin w2_sercomm_rx_ping(); +w2_s_bin w2_sercomm_tx_ping(); +w2_s_bin w2_sercomm_tx_expt(); +w2_s_bin w2_sercomm_rx_mode(); +w2_s_bin w2_sercomm_tx_mode(); +w2_s_bin w2_sercomm_rx_sped(); +w2_s_bin w2_sercomm_rx_dirc(); +w2_s_bin w2_sercomm_rx_cord(); +w2_s_bin w2_sercomm_tx_cord(); +w2_s_bin w2_sercomm_rx_bomd(); +w2_s_bin w2_sercomm_tx_bomd(); +w2_s_bin w2_sercomm_rx_sres(); +w2_s_bin w2_sercomm_rx_mcfg(); +w2_s_bin w2_sercomm_rx_sens(); +w2_s_bin w2_sercomm_tx_sens(); +w2_s_bin w2_sercomm_rx_info(); +w2_s_bin w2_sercomm_tx_info(); +w2_s_bin w2_sercomm_rx_disp(); +w2_s_bin w2_sercomm_rx_play(); +w2_s_bin w2_sercomm_rx_cled(); diff --git a/robot/setup.c b/robot/setup.c index c74cca9..bdd5991 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -5,8 +5,17 @@ #include "modes.h" #include "orangutan_shim.h" #include "setup.h" +#include "bin.h" + +// pointers for endianness check +static const uint16_t _test = 1; +static const uint8_t* _ptest = (uint8_t*)&_test; +uint8_t g_w2_endianness; void w2_setup_main() { + // check endianness + g_w2_endianness = *_ptest; + serial_set_baud_rate(W2_SERIAL_BAUD); // reset underside leds -- cgit v1.2.3 From 2b75425070662b869c15673623df44e30ce43ebe Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 17 May 2022 21:28:08 +0200 Subject: code format --- .clang-tidy | 1 + robot/bin.c | 8 ++++---- robot/bin.h | 2 -- robot/consts.h | 1 - robot/errcatch.c | 4 ++-- robot/setup.c | 6 +++--- 6 files changed, 10 insertions(+), 12 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 4c8496f..4dd70ad 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -11,6 +11,7 @@ CheckOptions: - { key: readability-identifier-naming.GlobalVariableIgnoredRegexp, value: _.* } - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_w2_ } - { key: readability-identifier-naming.MacroDefinitionCase, value: UPPER_CASE } + - { key: readability-identifier-naming.MacroDefinitionIgnoredRegexp, value: _.* } - { key: readability-identifier-naming.MacroDefinitionPrefix, value: W2_ } - { key: readability-identifier-naming.StructCase, value: lower_case } - { key: readability-identifier-naming.StructPrefix, value: w2_s_ } diff --git a/robot/bin.c b/robot/bin.c index 4261b50..4242edc 100644 --- a/robot/bin.c +++ b/robot/bin.c @@ -15,14 +15,14 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wshift-count-overflow" uint32_t w2_bin_hton32(uint32_t h32) { - if(g_w2_endianness == W2_ENDIAN_BIG) return h32; - return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) - | ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); + if (g_w2_endianness == W2_ENDIAN_BIG) return h32; + return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) | + ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); } #pragma GCC diagnostic pop uint16_t w2_bin_hton16(uint16_t h16) { - if(g_w2_endianness == W2_ENDIAN_BIG) return h16; + if (g_w2_endianness == W2_ENDIAN_BIG) return h16; return ((h16 & _BYTE_0) << _SHIFT_1B) | ((h16 & _BYTE_1) >> _SHIFT_1B); } diff --git a/robot/bin.h b/robot/bin.h index 1f21f64..ccde132 100644 --- a/robot/bin.h +++ b/robot/bin.h @@ -9,7 +9,6 @@ typedef struct { extern uint8_t g_w2_endianness; - /** * helper file for binary data * @@ -24,4 +23,3 @@ uint16_t w2_bin_hton16(uint16_t h16); uint32_t w2_bin_ntoh32(uint32_t n32); /** convert 16-bit value from network (big-endian) to host endian */ uint16_t w2_bin_ntoh16(uint16_t n16); - diff --git a/robot/consts.h b/robot/consts.h index 3558da0..d74d034 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -8,4 +8,3 @@ #define W2_MAX_MODULE_CYCLE_MS (20) #define W2_SERIAL_BAUD (9600) #define W2_E_BUFFER_SIZE (16) - diff --git a/robot/errcatch.c b/robot/errcatch.c index 0f97566..1262a38 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -9,8 +9,8 @@ #include "orangutan_shim.h" w2_s_error *g_w2_error_buffer[W2_E_BUFFER_SIZE] = {}; -uint8_t g_w2_error_index = 0; -uint8_t g_w2_error_offset = 0; +uint8_t g_w2_error_index = 0; +uint8_t g_w2_error_offset = 0; void w2_errcatch_main() { while (g_w2_error_index != g_w2_error_offset) { diff --git a/robot/setup.c b/robot/setup.c index bdd5991..d040286 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -1,15 +1,15 @@ #include +#include "bin.h" #include "consts.h" #include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "setup.h" -#include "bin.h" // pointers for endianness check -static const uint16_t _test = 1; -static const uint8_t* _ptest = (uint8_t*)&_test; +static const uint16_t _test = 1; +static const uint8_t *_ptest = (uint8_t *)&_test; uint8_t g_w2_endianness; void w2_setup_main() { -- cgit v1.2.3 From 154df68cb2a74a3b22456e8ec5af3af54352a41f Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 17 May 2022 22:37:09 +0200 Subject: fix buffer overflow in error handling module - fixed ring buffer overflow in errcatch module - fixed naming of commands in sercomm module - added comments to macro's in consts.h - properly handle `W2_E_WARN_UNCAUGHT_ERROR` and `W2_E_WARN_ERR_BUFFER_FULL` (these used to cause infinite loops) - added buffer full warning code - added options to hide some simulation messages - more boilerplate sercomm module code --- robot/consts.h | 7 ++++++- robot/errcatch.c | 25 +++++++++++++++++++------ robot/errcatch.h | 3 ++- robot/hypervisor.c | 15 +++++++-------- robot/makefile | 2 +- robot/sercomm.h | 40 ++++++++++++++++++++++++---------------- robot/sim.h | 13 +++++++++---- 7 files changed, 68 insertions(+), 37 deletions(-) diff --git a/robot/consts.h b/robot/consts.h index d74d034..94a161d 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -5,6 +5,11 @@ #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) -#define W2_E_BUFFER_SIZE (16) +/** 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) diff --git a/robot/errcatch.c b/robot/errcatch.c index 1262a38..f66b2a1 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -8,15 +8,25 @@ #include "modes.h" #include "orangutan_shim.h" -w2_s_error *g_w2_error_buffer[W2_E_BUFFER_SIZE] = {}; -uint8_t g_w2_error_index = 0; -uint8_t g_w2_error_offset = 0; +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; +uint8_t g_w2_error_buffer_full = 0; +uint8_t g_w2_error_uncaught = 0; void w2_errcatch_main() { while (g_w2_error_index != g_w2_error_offset) { 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_E_BUFFER_SIZE; + 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; } } @@ -30,10 +40,13 @@ w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const cha void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); } void w2_errcatch_throw_msg(enum w2_e_errorcodes 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; - g_w2_error_index = (g_w2_error_index + 1) % W2_E_BUFFER_SIZE; + if (g_w2_error_buffer_full) return; + g_w2_error_index = next_index; } void w2_errcatch_handle_error(w2_s_error *error) { @@ -48,7 +61,7 @@ void w2_errcatch_handle_error(w2_s_error *error) { break; } default: { - w2_errcatch_throw(W2_E_WARN_UNCAUGHT_ERROR); + g_w2_error_uncaught = 1; #ifdef W2_SIM simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code); #endif diff --git a/robot/errcatch.h b/robot/errcatch.h index ec4b90f..9f737d8 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -29,6 +29,7 @@ enum w2_e_errorcodes { W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, + W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, }; /** @@ -44,7 +45,7 @@ typedef struct { } w2_s_error; /** error ring buffer */ -extern w2_s_error *g_w2_error_buffer[W2_E_BUFFER_SIZE]; +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 */ diff --git a/robot/hypervisor.c b/robot/hypervisor.c index dbdc62a..c558de8 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -1,5 +1,4 @@ #include "hypervisor.h" -#include "consts.h" #include "errcatch.h" #include "io.h" #include "modes.h" @@ -7,6 +6,10 @@ #include "sercomm.h" void w2_hypervisor_main() { +#ifdef W2_SIM + siminfo("cycle start\n"); +#endif + time_reset(); w2_sercomm_main(); @@ -18,14 +21,10 @@ void w2_hypervisor_main() { w2_modes_main(); unsigned long mode_time = get_ms() - io_time; -#ifdef W2_SIM - siminfo("sercomm: %lums\n", sercomm_time); - siminfo("errcatch: %lums\n", errcatch_time); - siminfo("io: %lums\n", io_time); - siminfo("mode: %lums\n", mode_time); + if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); +#ifdef W2_SIM + siminfo("cycle end\n"); usleep(100e3); #endif - - if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); } diff --git a/robot/makefile b/robot/makefile index 6f50519..ce57090 100644 --- a/robot/makefile +++ b/robot/makefile @@ -34,7 +34,7 @@ CFLAGS += -DW2_BUILD_STR="$(BUILD_STR)" all: $(if $(SIM), a.out, out.hex) clean: - rm -f *.o out.hex a.out compile_commands.json + rm -f *.o out.hex a.out a.out: $(OBJECTS) $(CC) $(OBJECTS) $(CFLAGS) $(LDFLAGS) diff --git a/robot/sercomm.h b/robot/sercomm.h index 1533a4c..d960d0d 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -1,27 +1,35 @@ #pragma once #include "bin.h" +#include "consts.h" -#define W2_CMD_RX (0) -#define W2_CMD_TX (1) +#define W2_CMDDIR_RX (0) +#define W2_CMDDIR_TX (1) enum w2_e_serial_commands { - W2_E_CMD_PING = 0x00, - W2_E_CMD_EXPT = 0x02, - W2_E_CMD_MODE = 0x04, - W2_E_CMD_SPED = 0x06, - W2_E_CMD_DIRC = 0x08, - W2_E_CMD_CORD = 0x0a, - W2_E_CMD_BOMD = 0x0c, - W2_E_CMD_SRES = 0x0e, - W2_E_CMD_MCFG = 0x10, - W2_E_CMD_SENS = 0x12, - W2_E_CMD_INFO = 0x14, - W2_E_CMD_DISP = 0x16, - W2_E_CMD_PLAY = 0x18, - W2_E_CMD_CLED = 0x1a, + W2_CMD_PING = 0x00, + W2_CMD_EXPT = 0x02, + W2_CMD_MODE = 0x04, + W2_CMD_SPED = 0x06, + W2_CMD_DIRC = 0x08, + W2_CMD_CORD = 0x0a, + W2_CMD_BOMD = 0x0c, + W2_CMD_SRES = 0x0e, + W2_CMD_MCFG = 0x10, + W2_CMD_SENS = 0x12, + W2_CMD_INFO = 0x14, + W2_CMD_DISP = 0x16, + W2_CMD_PLAY = 0x18, + W2_CMD_CLED = 0x1a, }; +/** sercomm ring buffer */ +extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_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; + /** * serial pc-robot communication module * diff --git a/robot/sim.h b/robot/sim.h index 15a1b4b..9c80ecf 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -4,6 +4,11 @@ #include #include +// debug fine-tuning +#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_SIMWARN (1) +#define DBG_ENABLE_SIMINFO (1) + // debug print options #define DBG_BYTES_PER_LINE 16 @@ -21,10 +26,10 @@ // debug stdout print macros #define simprintf(message, ...) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) #define simprint(message) simprintf(message "\n") -#define simprintfunc(name, fmt, ...) simprintf(COL_BLU "[FUNC] " \ - COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__) -#define simwarn(message, ...) simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__) -#define siminfo(message, ...) simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__) +#define simprintfunc(name, fmt, ...) if (DBG_ENABLE_PRINTFUNC) { simprintf(COL_BLU "[FUNC] " \ + COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__); } +#define simwarn(message, ...) if (DBG_ENABLE_SIMWARN) { simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__); } +#define siminfo(message, ...) if (DBG_ENABLE_SIMINFO) { simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__); } /** * simulates pololu library functions for local testing -- cgit v1.2.3 From 837acf351ae96e2392efde175a61fd33e0774961 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 18 May 2022 09:02:55 +0200 Subject: added comments to error code enum --- robot/errcatch.h | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/robot/errcatch.h b/robot/errcatch.h index 9f737d8..c6edcfb 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -18,18 +18,27 @@ * reserved for error type checking */ enum w2_e_errorcodes { - // critical error codes - W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, - W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, // client-only - W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, - W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, + /** wireless connection lost from either robot or client-side */ + W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, + /** serial COM-port unavalable. client-side only */ + W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, + /** line unable to be found automatically */ + W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, + /** obstacle unavoidable, robot stuck */ + W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, - // warnings - W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, + /** battery low, returning to charging station */ + W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, + /** obstacle detected, waiting then trying other route */ W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, - W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, - W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, - W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, + /** logic cycle took longer than `W2_MAX_MODULE_CYCLE_MS` */ + W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, + /** error thrown without handler, gets thrown on next cycle */ + W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, + /** error buffer full, gets thrown on next cycle */ + W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, + /** line lost, trying to calibrate */ + W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, }; /** -- cgit v1.2.3 From 3f4b1c7284304d8c4ae2e4dd17359a2b4c1c573c Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 18 May 2022 11:18:04 +0200 Subject: [WIP] serial communication (broken w/ segfault) --- robot/bin.c | 34 ++++++++++++++++++++++++++++++++++ robot/bin.h | 39 +++++++++++++++++++++++++++++++++------ robot/errcatch.c | 3 +-- robot/errcatch.h | 4 +++- robot/hypervisor.c | 4 ++-- robot/makefile | 2 +- robot/sercomm.c | 48 +++++++++++++++++++++++++++++++++++++++++++++++- robot/sercomm.h | 23 ++++++++++++++++++----- robot/setup.c | 27 +++++++++++++++++++++++++++ robot/sim.h | 3 ++- 10 files changed, 168 insertions(+), 19 deletions(-) diff --git a/robot/bin.c b/robot/bin.c index 4242edc..a2c91a4 100644 --- a/robot/bin.c +++ b/robot/bin.c @@ -1,3 +1,5 @@ +#include + #include "bin.h" #define W2_ENDIAN_LITTLE (1) @@ -12,8 +14,40 @@ #define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B))) #define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B))) +const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)}; + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wshift-count-overflow" +w2_s_bin *w2_bin_from_uint8_t(uint8_t data) { + size_t size = 1; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + ret->bytes = size; + ret->data[0] = data; + return ret; +} + +w2_s_bin *w2_bin_from_uint16_t(uint16_t data) { + size_t size = 2; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + data = w2_bin_hton16(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[1] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +w2_s_bin *w2_bin_from_uint32_t(uint32_t data) { + size_t size = 4; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + data = w2_bin_hton32(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_3) >> _SHIFT_3B; + ret->data[1] = (data & _BYTE_2) >> _SHIFT_2B; + ret->data[2] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[3] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + uint32_t w2_bin_hton32(uint32_t h32) { if (g_w2_endianness == W2_ENDIAN_BIG) return h32; return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) | diff --git a/robot/bin.h b/robot/bin.h index ccde132..1c9b951 100644 --- a/robot/bin.h +++ b/robot/bin.h @@ -1,19 +1,46 @@ #pragma once +/** + * helper file for binary data + * + * - fix endianness with functions inspired by UNIX arpa/inet.h + * - convert uint16_t and uint32_t to w2_s_bin + */ #include +extern uint8_t g_w2_endianness; + +#define W2_T_UINT8_T (0) +#define W2_T_UINT16_T (1) +#define W2_T_UINT32_T (2) + +enum w2_e_struct_types { + W2_ST_UINT8_T, + W2_ST_UINT16_T, + W2_ST_UINT32_T, +}; + +extern const uint8_t W2_STRUCT_T_SIZES[]; + typedef struct { uint16_t bytes; uint8_t data[]; } w2_s_bin; -extern uint8_t g_w2_endianness; +typedef struct { + enum w2_e_struct_types type; + uint16_t length; + const uint8_t *data; +} w2_s_struct_property; -/** - * helper file for binary data - * - * - fix endianness with functions inspired by UNIX arpa/inet.h - */ +typedef struct { + uint16_t length; + w2_s_struct_property *properties[]; +} w2_s_property_list; + +w2_s_bin *w2_bin_from_uint8_t(uint8_t data); +w2_s_bin *w2_bin_from_uint16_t(uint16_t data); +w2_s_bin *w2_bin_from_uint32_t(uint32_t data); /** convert 32-bit value from host endian to network (big-endian) */ uint32_t w2_bin_hton32(uint32_t h32); diff --git a/robot/errcatch.c b/robot/errcatch.c index f66b2a1..82da820 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -1,4 +1,3 @@ -#include #include #include @@ -31,7 +30,7 @@ void w2_errcatch_main() { } w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message) { - w2_s_error *error = calloc(sizeof(w2_s_error) + length, 1); + 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); diff --git a/robot/errcatch.h b/robot/errcatch.h index c6edcfb..f0e25d9 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -39,6 +39,8 @@ enum w2_e_errorcodes { W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, /** line lost, trying to calibrate */ W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, + /** serial buffer full, gets thrown on next cycle */ + W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, }; /** @@ -75,6 +77,6 @@ void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const cha /** * allocate and initialize error struct * - * TODO: doesn't handle null pointers from calloc + * TODO: doesn't handle null pointers from malloc */ w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message); diff --git a/robot/hypervisor.c b/robot/hypervisor.c index c558de8..c08a24c 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -7,7 +7,7 @@ void w2_hypervisor_main() { #ifdef W2_SIM - siminfo("cycle start\n"); + if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n"); #endif time_reset(); @@ -24,7 +24,7 @@ void w2_hypervisor_main() { if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); #ifdef W2_SIM - siminfo("cycle end\n"); + if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); usleep(100e3); #endif } diff --git a/robot/makefile b/robot/makefile index ce57090..53010dd 100644 --- a/robot/makefile +++ b/robot/makefile @@ -14,7 +14,7 @@ SOURCES := $(filter-out sim.c, $(wildcard *.c)) HEADERS := $(filter-out sim.h, $(wildcard *.h)) # simulation -# SIM = true +SIM = true CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU)) LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE)) PREFIX := $(if $(SIM), , avr-) diff --git a/robot/sercomm.c b/robot/sercomm.c index a0eed3a..e845b1a 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -1,3 +1,49 @@ +#include +#include + +#include "bin.h" +#include "orangutan_shim.h" #include "sercomm.h" -void w2_sercomm_main() {} +w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {}; +uint8_t g_w2_sercomm_index = 0; +uint8_t g_w2_sercomm_offset = 0; +uint8_t g_w2_sercomm_buffer_full = 0; + +void w2_sercomm_main() { +#ifdef W2_SIM + simprintfunc("w2_sercomm_main", ""); +#endif + // send data + while (g_w2_sercomm_index != g_w2_sercomm_offset) { +#ifdef W2_SIM + simprint("line 0"); + w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_index]; + simprint("line 1"); + + simprintf("bytes: %i\n", data->bytes); + char* data_cast = malloc(data->bytes); + simprint("line 2"); + memcpy(data_cast, data->data, data->bytes); + simprint("line 3"); + serial_send(data_cast, data->bytes); + simprint("line 4"); + g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; + simprint("line 5"); +#endif + } +} + +void w2_sercomm_append_msg(w2_s_bin *data) { +#ifdef W2_SIM + simprintfunc("w2_sercomm_append_msg", ""); +#endif + uint8_t next_index = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE; + g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset; + free(g_w2_sercomm_buffer[g_w2_sercomm_index]); + w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes); + memcpy(&data_copy->bytes, data->data, data->bytes); + g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy; + if (g_w2_sercomm_buffer_full) return; + g_w2_sercomm_index = next_index; +} diff --git a/robot/sercomm.h b/robot/sercomm.h index d960d0d..dd17af8 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -7,28 +7,42 @@ #define W2_CMDDIR_TX (1) enum w2_e_serial_commands { + /** ping command */ W2_CMD_PING = 0x00, + /** exception command */ W2_CMD_EXPT = 0x02, + /** mode command */ W2_CMD_MODE = 0x04, + /** speed command */ W2_CMD_SPED = 0x06, + /** direct control command */ W2_CMD_DIRC = 0x08, + /** coordinate command */ W2_CMD_CORD = 0x0a, + /** backorder modify command */ W2_CMD_BOMD = 0x0c, + /** soft reset command */ W2_CMD_SRES = 0x0e, + /** map config command */ W2_CMD_MCFG = 0x10, + /** sensor data command */ W2_CMD_SENS = 0x12, + /** info command */ W2_CMD_INFO = 0x14, + /** display control command */ W2_CMD_DISP = 0x16, + /** play midi command */ W2_CMD_PLAY = 0x18, + /** control leds command */ W2_CMD_CLED = 0x1a, }; /** sercomm ring buffer */ extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE]; /** stores head of ring buffer */ -extern uint8_t g_w2_error_index; +extern uint8_t g_w2_sercomm_index; /** stores start of ring buffer */ -extern uint8_t g_w2_error_offset; +extern uint8_t g_w2_sercomm_offset; /** * serial pc-robot communication module @@ -38,10 +52,9 @@ extern uint8_t g_w2_error_offset; */ void w2_sercomm_main(); -void w2_sercomm_append_msg(w2_s_bin data); +void w2_sercomm_append_msg(w2_s_bin *data); -w2_s_bin w2_sercomm_rx_generic(); -w2_s_bin w2_sercomm_tx_generic(); +w2_s_bin w2_sercomm_struct_pack(w2_s_property_list generic_struct); w2_s_bin w2_sercomm_rx_ping(); w2_s_bin w2_sercomm_tx_ping(); diff --git a/robot/setup.c b/robot/setup.c index d040286..59a6404 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -27,4 +27,31 @@ void w2_setup_main() { // indicate startup done play("L50 c>c"); + +#ifdef W2_SIM + #include "sercomm.h" + w2_s_bin *data = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * 20); + data->bytes = 20; + data->data[0] = 0x00; + data->data[1] = 0x01; + data->data[2] = 0x00; + data->data[3] = 0x01; + data->data[4] = 0x00; + data->data[5] = 0x01; + data->data[6] = 0x00; + data->data[7] = 0x01; + data->data[8] = 0xde; + data->data[9] = 0xad; + data->data[10] = 0xbe; + data->data[11] = 0xef; + data->data[12] = 0xff; + data->data[13] = 0xff; + data->data[14] = 0x00; + data->data[15] = 0x00; + data->data[16] = 0xff; + data->data[17] = 0xff; + data->data[18] = 0x00; + data->data[19] = 0x69; + w2_sercomm_append_msg(data); +#endif } diff --git a/robot/sim.h b/robot/sim.h index 9c80ecf..c872dda 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -5,9 +5,10 @@ #include // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_PRINTFUNC (1) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) +#define DBG_ENABLE_CYCLEINFO (0) // debug print options #define DBG_BYTES_PER_LINE 16 -- cgit v1.2.3 From 22f92030b316f98c5f8d3a54d0a937ab12fe148e Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 18 May 2022 11:18:34 +0200 Subject: `make format` --- robot/sercomm.c | 2 +- robot/setup.c | 24 ++++++++++++------------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/robot/sercomm.c b/robot/sercomm.c index e845b1a..9c67aab 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -22,7 +22,7 @@ void w2_sercomm_main() { simprint("line 1"); simprintf("bytes: %i\n", data->bytes); - char* data_cast = malloc(data->bytes); + char *data_cast = malloc(data->bytes); simprint("line 2"); memcpy(data_cast, data->data, data->bytes); simprint("line 3"); diff --git a/robot/setup.c b/robot/setup.c index 59a6404..fcac6d4 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -29,19 +29,19 @@ void w2_setup_main() { play("L50 c>c"); #ifdef W2_SIM - #include "sercomm.h" +#include "sercomm.h" w2_s_bin *data = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * 20); - data->bytes = 20; - data->data[0] = 0x00; - data->data[1] = 0x01; - data->data[2] = 0x00; - data->data[3] = 0x01; - data->data[4] = 0x00; - data->data[5] = 0x01; - data->data[6] = 0x00; - data->data[7] = 0x01; - data->data[8] = 0xde; - data->data[9] = 0xad; + data->bytes = 20; + data->data[0] = 0x00; + data->data[1] = 0x01; + data->data[2] = 0x00; + data->data[3] = 0x01; + data->data[4] = 0x00; + data->data[5] = 0x01; + data->data[6] = 0x00; + data->data[7] = 0x01; + data->data[8] = 0xde; + data->data[9] = 0xad; data->data[10] = 0xbe; data->data[11] = 0xef; data->data[12] = 0xff; -- cgit v1.2.3 From 2c94e4478558551dbebdc8fa8682661fa60a1c75 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 23 May 2022 12:56:21 +0200 Subject: fix segfault --- robot/errcatch.c | 2 +- robot/sercomm.c | 17 ++++------------- robot/sim.c | 2 +- 3 files changed, 6 insertions(+), 15 deletions(-) diff --git a/robot/errcatch.c b/robot/errcatch.c index 82da820..2a59d3d 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -14,7 +14,7 @@ uint8_t g_w2_error_buffer_full = 0; uint8_t g_w2_error_uncaught = 0; void w2_errcatch_main() { - while (g_w2_error_index != g_w2_error_offset) { + 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; diff --git a/robot/sercomm.c b/robot/sercomm.c index 9c67aab..59b0f5a 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -15,22 +15,12 @@ void w2_sercomm_main() { simprintfunc("w2_sercomm_main", ""); #endif // send data - while (g_w2_sercomm_index != g_w2_sercomm_offset) { -#ifdef W2_SIM - simprint("line 0"); - w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_index]; - simprint("line 1"); - - simprintf("bytes: %i\n", data->bytes); + while (g_w2_sercomm_offset != g_w2_sercomm_index) { + w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; char *data_cast = malloc(data->bytes); - simprint("line 2"); memcpy(data_cast, data->data, data->bytes); - simprint("line 3"); serial_send(data_cast, data->bytes); - simprint("line 4"); g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; - simprint("line 5"); -#endif } } @@ -42,7 +32,8 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset; free(g_w2_sercomm_buffer[g_w2_sercomm_index]); w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes); - memcpy(&data_copy->bytes, data->data, data->bytes); + memcpy(&data_copy->data, data->data, data->bytes); + data_copy->bytes = data->bytes; g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy; if (g_w2_sercomm_buffer_full) return; g_w2_sercomm_index = next_index; diff --git a/robot/sim.c b/robot/sim.c index 6bd5838..1e88feb 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -55,7 +55,7 @@ void serial_send(char* message, unsigned int length) { printf("\n"); simprintf(""); } - printf("%02x ", message[byte]); + printf("%02x ", message[byte] & 0xff); bytes++; } printf("\n"); -- cgit v1.2.3 From 33540c7855d52343eb943d3a5144d6b736c234da Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 23 May 2022 12:56:37 +0200 Subject: make format --- robot/sercomm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot/sercomm.c b/robot/sercomm.c index 59b0f5a..723a231 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -16,7 +16,7 @@ void w2_sercomm_main() { #endif // send data while (g_w2_sercomm_offset != g_w2_sercomm_index) { - w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; + w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; char *data_cast = malloc(data->bytes); memcpy(data_cast, data->data, data->bytes); serial_send(data_cast, data->bytes); @@ -33,7 +33,7 @@ void w2_sercomm_append_msg(w2_s_bin *data) { free(g_w2_sercomm_buffer[g_w2_sercomm_index]); w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes); memcpy(&data_copy->data, data->data, data->bytes); - data_copy->bytes = data->bytes; + data_copy->bytes = data->bytes; g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy; if (g_w2_sercomm_buffer_full) return; g_w2_sercomm_index = next_index; -- cgit v1.2.3 From 92c394b44cc846ba044c2862b4f08eadc2160805 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 23 May 2022 13:40:17 +0200 Subject: WIP serial receive --- robot/consts.h | 3 +++ robot/sercomm.c | 13 ++++++++++++- robot/sercomm.h | 5 +++++ robot/setup.c | 31 ++++--------------------------- robot/sim.c | 11 +++++++++++ robot/sim.h | 2 ++ 6 files changed, 37 insertions(+), 28 deletions(-) diff --git a/robot/consts.h b/robot/consts.h index 94a161d..a8b8397 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -13,3 +13,6 @@ #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) + diff --git a/robot/sercomm.c b/robot/sercomm.c index 723a231..d7dc7f1 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -5,11 +5,14 @@ #include "orangutan_shim.h" #include "sercomm.h" -w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {}; +w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {0}; uint8_t g_w2_sercomm_index = 0; uint8_t g_w2_sercomm_offset = 0; uint8_t g_w2_sercomm_buffer_full = 0; +char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0}; +uint8_t g_w2_serial_buffer_index = 0; + void w2_sercomm_main() { #ifdef W2_SIM simprintfunc("w2_sercomm_main", ""); @@ -22,6 +25,14 @@ void w2_sercomm_main() { serial_send(data_cast, data->bytes); g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } + + while(serial_get_received_bytes() != g_w2_serial_buffer_index) { + uint8_t byte = g_w2_serial_buffer[g_w2_serial_buffer_index]; +#ifdef W2_SIM + simprintf("serial byte: %02x\n", byte); +#endif + g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; + } } void w2_sercomm_append_msg(w2_s_bin *data) { diff --git a/robot/sercomm.h b/robot/sercomm.h index dd17af8..f411374 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -44,6 +44,11 @@ extern uint8_t g_w2_sercomm_index; /** stores start of ring buffer */ extern uint8_t g_w2_sercomm_offset; +/** serial input (receive) buffer */ +extern char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE]; +/** serial input (receive) buffer current position */ +extern uint8_t g_w2_serial_buffer_index; + /** * serial pc-robot communication module * diff --git a/robot/setup.c b/robot/setup.c index fcac6d4..36c5da1 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -5,6 +5,7 @@ #include "halt.h" #include "modes.h" #include "orangutan_shim.h" +#include "sercomm.h" #include "setup.h" // pointers for endianness check @@ -25,33 +26,9 @@ void w2_setup_main() { // clear lcd clear(); + // start serial input + serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); + // indicate startup done play("L50 c>c"); - -#ifdef W2_SIM -#include "sercomm.h" - w2_s_bin *data = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * 20); - data->bytes = 20; - data->data[0] = 0x00; - data->data[1] = 0x01; - data->data[2] = 0x00; - data->data[3] = 0x01; - data->data[4] = 0x00; - data->data[5] = 0x01; - data->data[6] = 0x00; - data->data[7] = 0x01; - data->data[8] = 0xde; - data->data[9] = 0xad; - data->data[10] = 0xbe; - data->data[11] = 0xef; - data->data[12] = 0xff; - data->data[13] = 0xff; - data->data[14] = 0x00; - data->data[15] = 0x00; - data->data[16] = 0xff; - data->data[17] = 0xff; - data->data[18] = 0x00; - data->data[19] = 0x69; - w2_sercomm_append_msg(data); -#endif } diff --git a/robot/sim.c b/robot/sim.c index 1e88feb..554b174 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -1,6 +1,7 @@ #include #include #include +#include #include "sim.h" @@ -62,3 +63,13 @@ void serial_send(char* message, unsigned int length) { return; } +void serial_receive_ring(char* buffer, unsigned char size) { + simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size); + return; +} + +unsigned char serial_get_received_bytes() { + simprintfunc("serial_get_received_bytes", ""); + return 0; +} + diff --git a/robot/sim.h b/robot/sim.h index c872dda..75f6a5c 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -44,3 +44,5 @@ void clear(); // NOLINT void play(const char* melody); // NOLINT void serial_set_baud_rate(unsigned int rate); // NOLINT void serial_send(char* message, unsigned int length); // NOLINT +void serial_receive_ring(char* buffer, unsigned char size); // NOLINT +unsigned char serial_get_received_bytes(); // NOLINT -- cgit v1.2.3 From 7e3ece64cd5138081263413461dc6856a668bfae Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 24 May 2022 21:04:12 +0200 Subject: sim reading serial in from stdin working --- robot/hypervisor.c | 1 + robot/main.c | 9 ++++++++- robot/sercomm.c | 1 + robot/sercomm.h | 2 ++ robot/sim.c | 30 +++++++++++++++++++++++++++++- robot/sim.h | 10 ++++++++-- robot/tests/info.bin | 1 + robot/tests/ping.bin | Bin 0 -> 3 bytes robot/tests/readme.md | 11 +++++++++++ 9 files changed, 61 insertions(+), 4 deletions(-) create mode 100644 robot/tests/info.bin create mode 100644 robot/tests/ping.bin create mode 100644 robot/tests/readme.md diff --git a/robot/hypervisor.c b/robot/hypervisor.c index c08a24c..4d46a12 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -7,6 +7,7 @@ void w2_hypervisor_main() { #ifdef W2_SIM + w2_sim_cycle_begin(); if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n"); #endif diff --git a/robot/main.c b/robot/main.c index fbfd38b..d76dbaf 100644 --- a/robot/main.c +++ b/robot/main.c @@ -1,8 +1,15 @@ #include "main.h" #include "hypervisor.h" #include "setup.h" +#ifdef W2_SIM +#include "sim.h" +#endif + +int main(int argc, char **argv) { +#ifdef W2_SIM + w2_sim_setup(argc, argv); +#endif -int main() { w2_setup_main(); for (;;) w2_hypervisor_main(); diff --git a/robot/sercomm.c b/robot/sercomm.c index d7dc7f1..2faaa27 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -12,6 +12,7 @@ uint8_t g_w2_sercomm_buffer_full = 0; char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0}; uint8_t g_w2_serial_buffer_index = 0; +uint8_t g_w2_serial_buffer_head = 0; void w2_sercomm_main() { #ifdef W2_SIM diff --git a/robot/sercomm.h b/robot/sercomm.h index f411374..bc9fc1e 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -48,6 +48,8 @@ extern uint8_t g_w2_sercomm_offset; extern char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE]; /** serial input (receive) buffer current position */ extern uint8_t g_w2_serial_buffer_index; +/** serial input (receive) buffer head (sim only) */ +extern uint8_t g_w2_serial_buffer_head; /** * serial pc-robot communication module diff --git a/robot/sim.c b/robot/sim.c index 554b174..47c0c78 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -2,10 +2,15 @@ #include #include #include +#include +#include #include "sim.h" +#include "consts.h" +#include "sercomm.h" struct timespec reference_time; // NOLINT +bool g_w2_sim_headless = false; void time_reset() { simprintfunc("time_reset", ""); @@ -70,6 +75,29 @@ void serial_receive_ring(char* buffer, unsigned char size) { unsigned char serial_get_received_bytes() { simprintfunc("serial_get_received_bytes", ""); - return 0; + return g_w2_serial_buffer_head; +} + +void w2_sim_setup(int argc, char **argv) { + if (argc > 1 && strcmp(argv[1], "headless") == 0) + g_w2_sim_headless = true; + + // disable echo and enable raw mode + struct termios term; + tcgetattr(STDIN_FILENO, &term); + term.c_lflag &= ~(ECHO | ICANON); + term.c_cc[VTIME] = 0; + term.c_cc[VMIN] = 0; + tcsetattr(STDIN_FILENO, 0, &term); + + return; +} + +void w2_sim_cycle_begin() { + // read bytes from stdin + while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0) + g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE; + + return; } diff --git a/robot/sim.h b/robot/sim.h index 75f6a5c..501552e 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -3,9 +3,12 @@ #include #include #include +#include + +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) @@ -25,7 +28,7 @@ #define COL_RST "\e[0m" // debug stdout print macros -#define simprintf(message, ...) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) +#define simprintf(message, ...) if (!g_w2_sim_headless) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) #define simprint(message) simprintf(message "\n") #define simprintfunc(name, fmt, ...) if (DBG_ENABLE_PRINTFUNC) { simprintf(COL_BLU "[FUNC] " \ COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__); } @@ -46,3 +49,6 @@ void serial_set_baud_rate(unsigned int rate); // NOLINT void serial_send(char* message, unsigned int length); // NOLINT void serial_receive_ring(char* buffer, unsigned char size); // NOLINT unsigned char serial_get_received_bytes(); // NOLINT +void w2_sim_setup(int argc, char **argv); +void w2_sim_cycle_begin(); + diff --git a/robot/tests/info.bin b/robot/tests/info.bin new file mode 100644 index 0000000..e1ce36c --- /dev/null +++ b/robot/tests/info.bin @@ -0,0 +1 @@ +ÿ \ No newline at end of file diff --git a/robot/tests/ping.bin b/robot/tests/ping.bin new file mode 100644 index 0000000..456804e Binary files /dev/null and b/robot/tests/ping.bin differ diff --git a/robot/tests/readme.md b/robot/tests/readme.md new file mode 100644 index 0000000..7f4cc9a --- /dev/null +++ b/robot/tests/readme.md @@ -0,0 +1,11 @@ +this is a subdirectory that contains binary example commands to send to the +simultation robot code + +to use: + +``` +./a.out < file +``` + +where `file` is one of the .bin files in this folder + -- cgit v1.2.3 From ede8a89706209fa26e151a34a28e64affbffd23d Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 24 May 2022 21:04:35 +0200 Subject: make format --- robot/consts.h | 1 - robot/sercomm.c | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/robot/consts.h b/robot/consts.h index a8b8397..70efcac 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -15,4 +15,3 @@ #define W2_SERCOMM_BUFFER_SIZE (16) /** size of input (receive) buffer (in bytes) */ #define W2_SERIAL_READ_BUFFER_SIZE (255) - diff --git a/robot/sercomm.c b/robot/sercomm.c index 2faaa27..d1bb3b3 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -11,8 +11,8 @@ uint8_t g_w2_sercomm_offset = 0; uint8_t g_w2_sercomm_buffer_full = 0; char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0}; -uint8_t g_w2_serial_buffer_index = 0; -uint8_t g_w2_serial_buffer_head = 0; +uint8_t g_w2_serial_buffer_index = 0; +uint8_t g_w2_serial_buffer_head = 0; void w2_sercomm_main() { #ifdef W2_SIM @@ -27,7 +27,7 @@ void w2_sercomm_main() { g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } - while(serial_get_received_bytes() != g_w2_serial_buffer_index) { + while (serial_get_received_bytes() != g_w2_serial_buffer_index) { uint8_t byte = g_w2_serial_buffer[g_w2_serial_buffer_index]; #ifdef W2_SIM simprintf("serial byte: %02x\n", byte); -- cgit v1.2.3 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 --- client/makefile | 1 + robot/bin.c | 64 -------------------------------------------------------- robot/bin.h | 52 --------------------------------------------- robot/consts.h | 17 --------------- robot/errcatch.c | 1 - robot/errcatch.h | 2 +- robot/makefile | 1 + robot/sercomm.c | 2 +- robot/sercomm.h | 4 ++-- robot/setup.c | 4 ++-- robot/sim.c | 2 +- shared/bin.c | 64 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ shared/bin.h | 52 +++++++++++++++++++++++++++++++++++++++++++++ shared/consts.h | 17 +++++++++++++++ shared/makefile | 2 ++ shared/readme.md | 8 +++++++ 16 files changed, 152 insertions(+), 141 deletions(-) delete mode 100644 robot/bin.c delete mode 100644 robot/bin.h delete mode 100644 robot/consts.h create mode 100644 shared/bin.c create mode 100644 shared/bin.h create mode 100644 shared/consts.h create mode 100644 shared/makefile create mode 100644 shared/readme.md diff --git a/client/makefile b/client/makefile index 23d5ce6..2209c95 100644 --- a/client/makefile +++ b/client/makefile @@ -6,6 +6,7 @@ EXECNAME = main SOURCES := $(wildcard *.c) HEADERS := $(wildcard *.h) +include ../shared/makefile OBJECTS := $(patsubst %.c,%.o, $(SOURCES)) all: main diff --git a/robot/bin.c b/robot/bin.c deleted file mode 100644 index a2c91a4..0000000 --- a/robot/bin.c +++ /dev/null @@ -1,64 +0,0 @@ -#include - -#include "bin.h" - -#define W2_ENDIAN_LITTLE (1) -#define W2_ENDIAN_BIG (0) - -#define _SHIFT_0B (8 * 0) -#define _SHIFT_1B (8 * 1) -#define _SHIFT_2B (8 * 2) -#define _SHIFT_3B (8 * 3) -#define _BYTE_0 ((uint32_t)(0xff << (_SHIFT_0B))) -#define _BYTE_1 ((uint32_t)(0xff << (_SHIFT_1B))) -#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B))) -#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B))) - -const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)}; - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wshift-count-overflow" -w2_s_bin *w2_bin_from_uint8_t(uint8_t data) { - size_t size = 1; - w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); - ret->bytes = size; - ret->data[0] = data; - return ret; -} - -w2_s_bin *w2_bin_from_uint16_t(uint16_t data) { - size_t size = 2; - w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); - data = w2_bin_hton16(data); - ret->bytes = size; - ret->data[0] = (data & _BYTE_1) >> _SHIFT_1B; - ret->data[1] = (data & _BYTE_0) >> _SHIFT_0B; - return ret; -} - -w2_s_bin *w2_bin_from_uint32_t(uint32_t data) { - size_t size = 4; - w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); - data = w2_bin_hton32(data); - ret->bytes = size; - ret->data[0] = (data & _BYTE_3) >> _SHIFT_3B; - ret->data[1] = (data & _BYTE_2) >> _SHIFT_2B; - ret->data[2] = (data & _BYTE_1) >> _SHIFT_1B; - ret->data[3] = (data & _BYTE_0) >> _SHIFT_0B; - return ret; -} - -uint32_t w2_bin_hton32(uint32_t h32) { - if (g_w2_endianness == W2_ENDIAN_BIG) return h32; - return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) | - ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); -} -#pragma GCC diagnostic pop - -uint16_t w2_bin_hton16(uint16_t h16) { - if (g_w2_endianness == W2_ENDIAN_BIG) return h16; - return ((h16 & _BYTE_0) << _SHIFT_1B) | ((h16 & _BYTE_1) >> _SHIFT_1B); -} - -uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } -uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } diff --git a/robot/bin.h b/robot/bin.h deleted file mode 100644 index 1c9b951..0000000 --- a/robot/bin.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once -/** - * helper file for binary data - * - * - fix endianness with functions inspired by UNIX arpa/inet.h - * - convert uint16_t and uint32_t to w2_s_bin - */ - -#include - -extern uint8_t g_w2_endianness; - -#define W2_T_UINT8_T (0) -#define W2_T_UINT16_T (1) -#define W2_T_UINT32_T (2) - -enum w2_e_struct_types { - W2_ST_UINT8_T, - W2_ST_UINT16_T, - W2_ST_UINT32_T, -}; - -extern const uint8_t W2_STRUCT_T_SIZES[]; - -typedef struct { - uint16_t bytes; - uint8_t data[]; -} w2_s_bin; - -typedef struct { - enum w2_e_struct_types type; - uint16_t length; - const uint8_t *data; -} w2_s_struct_property; - -typedef struct { - uint16_t length; - w2_s_struct_property *properties[]; -} w2_s_property_list; - -w2_s_bin *w2_bin_from_uint8_t(uint8_t data); -w2_s_bin *w2_bin_from_uint16_t(uint16_t data); -w2_s_bin *w2_bin_from_uint32_t(uint32_t data); - -/** convert 32-bit value from host endian to network (big-endian) */ -uint32_t w2_bin_hton32(uint32_t h32); -/** convert 16-bit value from host endian to network (big-endian) */ -uint16_t w2_bin_hton16(uint16_t h16); -/** convert 32-bit value from network (big-endian) to host endian */ -uint32_t w2_bin_ntoh32(uint32_t n32); -/** convert 16-bit value from network (big-endian) to host endian */ -uint16_t w2_bin_ntoh16(uint16_t n16); diff --git a/robot/consts.h b/robot/consts.h deleted file mode 100644 index 70efcac..0000000 --- a/robot/consts.h +++ /dev/null @@ -1,17 +0,0 @@ -#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) diff --git a/robot/errcatch.c b/robot/errcatch.c index 2a59d3d..4bdbaef 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -1,7 +1,6 @@ #include #include -#include "consts.h" #include "errcatch.h" #include "halt.h" #include "modes.h" diff --git a/robot/errcatch.h b/robot/errcatch.h index f0e25d9..1e273bd 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -2,7 +2,7 @@ #include -#include "consts.h" +#include "../shared/consts.h" #define W2_E_TYPE_MASK (0b11 << 6) diff --git a/robot/makefile b/robot/makefile index 53010dd..cd5c032 100644 --- a/robot/makefile +++ b/robot/makefile @@ -12,6 +12,7 @@ LDFLAGS=-Wl,-gc-sections -Wl,-relax SOURCES := $(filter-out sim.c, $(wildcard *.c)) HEADERS := $(filter-out sim.h, $(wildcard *.h)) +include ../shared/makefile # simulation SIM = true diff --git a/robot/sercomm.c b/robot/sercomm.c index d1bb3b3..7072f9e 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -1,7 +1,7 @@ #include #include -#include "bin.h" +#include "../shared/bin.h" #include "orangutan_shim.h" #include "sercomm.h" diff --git a/robot/sercomm.h b/robot/sercomm.h index bc9fc1e..44fdf08 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -1,7 +1,7 @@ #pragma once -#include "bin.h" -#include "consts.h" +#include "../shared/bin.h" +#include "../shared/consts.h" #define W2_CMDDIR_RX (0) #define W2_CMDDIR_TX (1) diff --git a/robot/setup.c b/robot/setup.c index 36c5da1..6af1a05 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -1,7 +1,7 @@ #include -#include "bin.h" -#include "consts.h" +#include "../shared/bin.h" +#include "../shared/consts.h" #include "halt.h" #include "modes.h" #include "orangutan_shim.h" diff --git a/robot/sim.c b/robot/sim.c index 47c0c78..b061c9a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -6,7 +6,7 @@ #include #include "sim.h" -#include "consts.h" +#include "../shared/consts.h" #include "sercomm.h" struct timespec reference_time; // NOLINT diff --git a/shared/bin.c b/shared/bin.c new file mode 100644 index 0000000..a2c91a4 --- /dev/null +++ b/shared/bin.c @@ -0,0 +1,64 @@ +#include + +#include "bin.h" + +#define W2_ENDIAN_LITTLE (1) +#define W2_ENDIAN_BIG (0) + +#define _SHIFT_0B (8 * 0) +#define _SHIFT_1B (8 * 1) +#define _SHIFT_2B (8 * 2) +#define _SHIFT_3B (8 * 3) +#define _BYTE_0 ((uint32_t)(0xff << (_SHIFT_0B))) +#define _BYTE_1 ((uint32_t)(0xff << (_SHIFT_1B))) +#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B))) +#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B))) + +const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)}; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wshift-count-overflow" +w2_s_bin *w2_bin_from_uint8_t(uint8_t data) { + size_t size = 1; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + ret->bytes = size; + ret->data[0] = data; + return ret; +} + +w2_s_bin *w2_bin_from_uint16_t(uint16_t data) { + size_t size = 2; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + data = w2_bin_hton16(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[1] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +w2_s_bin *w2_bin_from_uint32_t(uint32_t data) { + size_t size = 4; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + data = w2_bin_hton32(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_3) >> _SHIFT_3B; + ret->data[1] = (data & _BYTE_2) >> _SHIFT_2B; + ret->data[2] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[3] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +uint32_t w2_bin_hton32(uint32_t h32) { + if (g_w2_endianness == W2_ENDIAN_BIG) return h32; + return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) | + ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); +} +#pragma GCC diagnostic pop + +uint16_t w2_bin_hton16(uint16_t h16) { + if (g_w2_endianness == W2_ENDIAN_BIG) return h16; + return ((h16 & _BYTE_0) << _SHIFT_1B) | ((h16 & _BYTE_1) >> _SHIFT_1B); +} + +uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } +uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } diff --git a/shared/bin.h b/shared/bin.h new file mode 100644 index 0000000..1c9b951 --- /dev/null +++ b/shared/bin.h @@ -0,0 +1,52 @@ +#pragma once +/** + * helper file for binary data + * + * - fix endianness with functions inspired by UNIX arpa/inet.h + * - convert uint16_t and uint32_t to w2_s_bin + */ + +#include + +extern uint8_t g_w2_endianness; + +#define W2_T_UINT8_T (0) +#define W2_T_UINT16_T (1) +#define W2_T_UINT32_T (2) + +enum w2_e_struct_types { + W2_ST_UINT8_T, + W2_ST_UINT16_T, + W2_ST_UINT32_T, +}; + +extern const uint8_t W2_STRUCT_T_SIZES[]; + +typedef struct { + uint16_t bytes; + uint8_t data[]; +} w2_s_bin; + +typedef struct { + enum w2_e_struct_types type; + uint16_t length; + const uint8_t *data; +} w2_s_struct_property; + +typedef struct { + uint16_t length; + w2_s_struct_property *properties[]; +} w2_s_property_list; + +w2_s_bin *w2_bin_from_uint8_t(uint8_t data); +w2_s_bin *w2_bin_from_uint16_t(uint16_t data); +w2_s_bin *w2_bin_from_uint32_t(uint32_t data); + +/** convert 32-bit value from host endian to network (big-endian) */ +uint32_t w2_bin_hton32(uint32_t h32); +/** convert 16-bit value from host endian to network (big-endian) */ +uint16_t w2_bin_hton16(uint16_t h16); +/** convert 32-bit value from network (big-endian) to host endian */ +uint32_t w2_bin_ntoh32(uint32_t n32); +/** convert 16-bit value from network (big-endian) to host endian */ +uint16_t w2_bin_ntoh16(uint16_t n16); 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) diff --git a/shared/makefile b/shared/makefile new file mode 100644 index 0000000..815d33c --- /dev/null +++ b/shared/makefile @@ -0,0 +1,2 @@ +SOURCES += $(wildcard ../shared/*.c) +HEADERS += $(wildcard ../shared/*.h) diff --git a/shared/readme.md b/shared/readme.md new file mode 100644 index 0000000..870f015 --- /dev/null +++ b/shared/readme.md @@ -0,0 +1,8 @@ +# shared code + +this is the subdirectory for all code that is shared between the robot code and +the client code. to use these, include the .h files with a relative path (e.g. +`#include "../shared/consts.h"`). makefiles should add `include +../shared/makefile` to add the .c and .h files to `$SOURCES` and `$HEADERS` in +the makefile targets (this is already done for the robot and client +subdirectories). -- cgit v1.2.3 From b0a905b14ff90f2240e5aa2928d5329d8494cb46 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 24 May 2022 22:06:09 +0200 Subject: fix `make clean` --- client/makefile | 6 +++--- robot/makefile | 6 +++--- shared/makefile | 3 +++ 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/client/makefile b/client/makefile index 2209c95..8bc8ac7 100644 --- a/client/makefile +++ b/client/makefile @@ -4,20 +4,20 @@ RM = rm -f CFLAGS = EXECNAME = main +all: $(EXECNAME) + SOURCES := $(wildcard *.c) HEADERS := $(wildcard *.h) include ../shared/makefile OBJECTS := $(patsubst %.c,%.o, $(SOURCES)) -all: main - .o: $(CC) -c $(CFLAGS) $< $(EXECNAME): $(OBJECTS) $(CC) $(OBJECTS) -o $(EXECNAME) -clean: +clean:: $(RM) $(EXECNAME) *.o format: diff --git a/robot/makefile b/robot/makefile index cd5c032..6de886c 100644 --- a/robot/makefile +++ b/robot/makefile @@ -10,6 +10,8 @@ PORT ?= /dev/ttyACM0 CFLAGS=-g -Wall $(DEVICE_SPECIFIC_CFLAGS) -Os LDFLAGS=-Wl,-gc-sections -Wl,-relax +all: $(if $(SIM), a.out, out.hex) + SOURCES := $(filter-out sim.c, $(wildcard *.c)) HEADERS := $(filter-out sim.h, $(wildcard *.h)) include ../shared/makefile @@ -32,9 +34,7 @@ OBJ2HEX=$(PREFIX)objcopy BUILD_STR=$(shell git update-index -q --refresh; git describe --tags --dirty='*' --broken='x' | cut -c1-20) CFLAGS += -DW2_BUILD_STR="$(BUILD_STR)" -all: $(if $(SIM), a.out, out.hex) - -clean: +clean:: rm -f *.o out.hex a.out a.out: $(OBJECTS) diff --git a/shared/makefile b/shared/makefile index 815d33c..cfdf8ac 100644 --- a/shared/makefile +++ b/shared/makefile @@ -1,2 +1,5 @@ SOURCES += $(wildcard ../shared/*.c) HEADERS += $(wildcard ../shared/*.h) + +clean:: + rm -f ../shared/*.o -- cgit v1.2.3 From 4f0718caefa7f7a1baf02997fec0b448d0d6615a Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 11:29:50 +0200 Subject: move more shared code --- robot/errcatch.h | 52 +--------------------------------------------------- robot/sercomm.h | 35 +---------------------------------- robot/setup.c | 3 +-- shared/errors.h | 54 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ shared/protocol.h | 35 +++++++++++++++++++++++++++++++++++ 5 files changed, 92 insertions(+), 87 deletions(-) create mode 100644 shared/errors.h create mode 100644 shared/protocol.h diff --git a/robot/errcatch.h b/robot/errcatch.h index 1e273bd..dcb6a97 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -3,57 +3,7 @@ #include #include "../shared/consts.h" - -#define W2_E_TYPE_MASK (0b11 << 6) - -#define W2_E_TYPE_CRIT (0b00 << 6) -#define W2_E_TYPE_WARN (0b01 << 6) -#define W2_E_TYPE_INFO (0b10 << 6) -#define W2_E_TYPE_VERB (0b11 << 6) - -/** - * enum storing all error codes - * - * error codes are between 0-63 because the two most significant bits are - * reserved for error type checking - */ -enum w2_e_errorcodes { - /** wireless connection lost from either robot or client-side */ - W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, - /** serial COM-port unavalable. client-side only */ - W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, - /** line unable to be found automatically */ - W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, - /** obstacle unavoidable, robot stuck */ - W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, - - /** battery low, returning to charging station */ - W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, - /** obstacle detected, waiting then trying other route */ - W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, - /** logic cycle took longer than `W2_MAX_MODULE_CYCLE_MS` */ - W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, - /** error thrown without handler, gets thrown on next cycle */ - W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, - /** error buffer full, gets thrown on next cycle */ - W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, - /** line lost, trying to calibrate */ - W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, - /** serial buffer full, gets thrown on next cycle */ - W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, -}; - -/** - * error struct - * - * holds an error with type `code`, and an optional `message` with length - * `message_length` - */ -typedef struct { - enum w2_e_errorcodes code; - uint8_t message_length; - char message[]; -} w2_s_error; +#include "../shared/errors.h" /** error ring buffer */ extern w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE]; diff --git a/robot/sercomm.h b/robot/sercomm.h index 44fdf08..a53d3e2 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -2,40 +2,7 @@ #include "../shared/bin.h" #include "../shared/consts.h" - -#define W2_CMDDIR_RX (0) -#define W2_CMDDIR_TX (1) - -enum w2_e_serial_commands { - /** ping command */ - W2_CMD_PING = 0x00, - /** exception command */ - W2_CMD_EXPT = 0x02, - /** mode command */ - W2_CMD_MODE = 0x04, - /** speed command */ - W2_CMD_SPED = 0x06, - /** direct control command */ - W2_CMD_DIRC = 0x08, - /** coordinate command */ - W2_CMD_CORD = 0x0a, - /** backorder modify command */ - W2_CMD_BOMD = 0x0c, - /** soft reset command */ - W2_CMD_SRES = 0x0e, - /** map config command */ - W2_CMD_MCFG = 0x10, - /** sensor data command */ - W2_CMD_SENS = 0x12, - /** info command */ - W2_CMD_INFO = 0x14, - /** display control command */ - W2_CMD_DISP = 0x16, - /** play midi command */ - W2_CMD_PLAY = 0x18, - /** control leds command */ - W2_CMD_CLED = 0x1a, -}; +#include "../shared/protocol.h" /** sercomm ring buffer */ extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE]; diff --git a/robot/setup.c b/robot/setup.c index 6af1a05..6dfef50 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -17,8 +17,6 @@ void w2_setup_main() { // check endianness g_w2_endianness = *_ptest; - serial_set_baud_rate(W2_SERIAL_BAUD); - // reset underside leds red_led(0); green_led(0); @@ -27,6 +25,7 @@ void w2_setup_main() { clear(); // start serial input + serial_set_baud_rate(W2_SERIAL_BAUD); serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); // indicate startup done diff --git a/shared/errors.h b/shared/errors.h new file mode 100644 index 0000000..280c5ef --- /dev/null +++ b/shared/errors.h @@ -0,0 +1,54 @@ +#pragma once + +#include + +#define W2_E_TYPE_MASK (0b11 << 6) + +#define W2_E_TYPE_CRIT (0b00 << 6) +#define W2_E_TYPE_WARN (0b01 << 6) +#define W2_E_TYPE_INFO (0b10 << 6) +#define W2_E_TYPE_VERB (0b11 << 6) + +/** + * enum storing all error codes + * + * error codes are between 0-63 because the two most significant bits are + * reserved for error type checking + */ +enum w2_e_errorcodes { + /** wireless connection lost from either robot or client-side */ + W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, + /** serial COM-port unavalable. client-side only */ + W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, + /** line unable to be found automatically */ + W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, + /** obstacle unavoidable, robot stuck */ + W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, + + /** battery low, returning to charging station */ + W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, + /** obstacle detected, waiting then trying other route */ + W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, + /** logic cycle took longer than `W2_MAX_MODULE_CYCLE_MS` */ + W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, + /** error thrown without handler, gets thrown on next cycle */ + W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, + /** error buffer full, gets thrown on next cycle */ + W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, + /** line lost, trying to calibrate */ + W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, + /** serial buffer full, gets thrown on next cycle */ + W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, +}; + +/** + * error struct + * + * holds an error with type `code`, and an optional `message` with length + * `message_length` + */ +typedef struct { + enum w2_e_errorcodes code; + uint8_t message_length; + char message[]; +} w2_s_error; diff --git a/shared/protocol.h b/shared/protocol.h new file mode 100644 index 0000000..b400252 --- /dev/null +++ b/shared/protocol.h @@ -0,0 +1,35 @@ +#pragma once + +#define W2_CMDDIR_RX (0) +#define W2_CMDDIR_TX (1) + +enum w2_e_serial_commands { + /** ping command */ + W2_CMD_PING = 0x00, + /** exception command */ + W2_CMD_EXPT = 0x02, + /** mode command */ + W2_CMD_MODE = 0x04, + /** speed command */ + W2_CMD_SPED = 0x06, + /** direct control command */ + W2_CMD_DIRC = 0x08, + /** coordinate command */ + W2_CMD_CORD = 0x0a, + /** backorder modify command */ + W2_CMD_BOMD = 0x0c, + /** soft reset command */ + W2_CMD_SRES = 0x0e, + /** map config command */ + W2_CMD_MCFG = 0x10, + /** sensor data command */ + W2_CMD_SENS = 0x12, + /** info command */ + W2_CMD_INFO = 0x14, + /** display control command */ + W2_CMD_DISP = 0x16, + /** play midi command */ + W2_CMD_PLAY = 0x18, + /** control leds command */ + W2_CMD_CLED = 0x1a, +}; -- cgit v1.2.3 From 9cd83d662849fe72c83038a018107d9bbf5c2398 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 16:30:21 +0200 Subject: WIP serial parsing --- robot/hypervisor.c | 8 ++++- robot/hypervisor.h | 4 +++ robot/readme.md | 2 ++ robot/sercomm.c | 12 +++----- robot/sim.c | 6 ++++ robot/sim.h | 3 +- robot/tests/padded_info.bin | 1 + shared/bin.c | 8 +++++ shared/bin.h | 3 ++ shared/protocol.c | 10 ++++++ shared/protocol.h | 74 +++++++++++++++++++++++++++++++++++++++++++-- shared/serial_parse.c | 54 +++++++++++++++++++++++++++++++++ shared/serial_parse.h | 9 ++++++ 13 files changed, 182 insertions(+), 12 deletions(-) create mode 100644 robot/tests/padded_info.bin create mode 100644 shared/protocol.c create mode 100644 shared/serial_parse.c create mode 100644 shared/serial_parse.h diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 4d46a12..0f754a3 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -5,6 +5,8 @@ #include "orangutan_shim.h" #include "sercomm.h" +uint64_t g_w2_hypervisor_cycles = 0; + void w2_hypervisor_main() { #ifdef W2_SIM w2_sim_cycle_begin(); @@ -26,6 +28,10 @@ void w2_hypervisor_main() { #ifdef W2_SIM if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); - usleep(100e3); + if (!g_w2_sim_headless) usleep(100e3); + + if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) exit(0); #endif + + g_w2_hypervisor_cycles++; } diff --git a/robot/hypervisor.h b/robot/hypervisor.h index 0e73259..35fa64c 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -1,5 +1,9 @@ #pragma once +#include + +extern uint64_t g_w2_hypervisor_cycles; + /** * backbone of all other modules * diff --git a/robot/readme.md b/robot/readme.md index 4a7aca3..2de14db 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -110,6 +110,8 @@ global todo: module (for last ping time measurement) - [ ] calibrate (line-detecting) light sensors in setup.c, or manually by placing the robot and pressing a button (maybe make this a seperate mode) +- [ ] create labeled timer functions like nodejs `console.time()` and + `console.timeEnd()` (use for serial read timeout constraint) ### hypervisor diff --git a/robot/sercomm.c b/robot/sercomm.c index 7072f9e..f4b7eb5 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -2,6 +2,7 @@ #include #include "../shared/bin.h" +#include "../shared/serial_parse.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -27,11 +28,9 @@ void w2_sercomm_main() { g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } + // read and parse data while (serial_get_received_bytes() != g_w2_serial_buffer_index) { - uint8_t byte = g_w2_serial_buffer[g_w2_serial_buffer_index]; -#ifdef W2_SIM - simprintf("serial byte: %02x\n", byte); -#endif + w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]); g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; } } @@ -43,10 +42,7 @@ void w2_sercomm_append_msg(w2_s_bin *data) { uint8_t next_index = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE; g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset; free(g_w2_sercomm_buffer[g_w2_sercomm_index]); - w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes); - memcpy(&data_copy->data, data->data, data->bytes); - data_copy->bytes = data->bytes; - g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy; + g_w2_sercomm_buffer[g_w2_sercomm_index] = w2_bin_s_alloc(data->bytes, data->data); if (g_w2_sercomm_buffer_full) return; g_w2_sercomm_index = next_index; } diff --git a/robot/sim.c b/robot/sim.c index b061c9a..8af672e 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -52,6 +52,12 @@ void serial_set_baud_rate(unsigned int rate) { } void serial_send(char* message, unsigned int length) { + if (g_w2_sim_headless) { + for (unsigned int byte = 0; byte < length; byte++) + putc(message[byte] & 0xff, stdout); + return; + } + if (!DBG_ENABLE_PRINTFUNC) return; simprintfunc("serial_send", ", %u", length); unsigned int bytes = 0; simprintf(""); diff --git a/robot/sim.h b/robot/sim.h index 501552e..336592b 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -8,10 +8,11 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_PRINTFUNC (1) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) +#define DBG_MAX_CYCLES (10) // debug print options #define DBG_BYTES_PER_LINE 16 diff --git a/robot/tests/padded_info.bin b/robot/tests/padded_info.bin new file mode 100644 index 0000000..2ff1d8f --- /dev/null +++ b/robot/tests/padded_info.bin @@ -0,0 +1 @@ +=ãÿ=íÿÿI89ÿ \ No newline at end of file diff --git a/shared/bin.c b/shared/bin.c index a2c91a4..fc6e14b 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -1,4 +1,5 @@ #include +#include #include "bin.h" @@ -62,3 +63,10 @@ uint16_t w2_bin_hton16(uint16_t h16) { uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } + +w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) { + w2_s_bin* temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes); + temp->bytes = bytes; + memcpy(&temp->data, data, bytes); + return temp; +} diff --git a/shared/bin.h b/shared/bin.h index 1c9b951..d9eacd7 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -27,6 +27,9 @@ typedef struct { uint8_t data[]; } w2_s_bin; +/** allocate new w2_s_bin struct and fill with `*data` for `bytes` bytes */ +w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data); + typedef struct { enum w2_e_struct_types type; uint16_t length; diff --git a/shared/protocol.c b/shared/protocol.c new file mode 100644 index 0000000..2707725 --- /dev/null +++ b/shared/protocol.c @@ -0,0 +1,10 @@ +#include "protocol.h" + +W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX) + +w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *parsed = malloc(data->bytes); + memcpy(parsed, data->data, data->bytes); + return parsed; +} + diff --git a/shared/protocol.h b/shared/protocol.h index b400252..b0602cf 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,7 +1,18 @@ #pragma once -#define W2_CMDDIR_RX (0) -#define W2_CMDDIR_TX (1) +#include +#include +#include + +#include "bin.h" + +#define W2_SERIAL_START_BYTE 0xff + +#define W2_CMDDIR_RX 0 +#define W2_CMDDIR_TX 1 + +#define W2_CMD_CODE_MASK (~1) +#define W2_CMD_DIRECTION_MASK (1) enum w2_e_serial_commands { /** ping command */ @@ -33,3 +44,62 @@ enum w2_e_serial_commands { /** control leds command */ W2_CMD_CLED = 0x1a, }; + +// TODO +// array met indicies die structs opslaan met eigenschappen over de protocol bericht +// +// belangrijke eigenschappen: +// lengte!!! +// generic struct (voor parsen) +// parse functie +// dump functie +// +// +// als het kan deze allemaal met macro's op deze manier definieren: +// +#define W2_PROTOCOL_CMD(name, direction, ...) +#define W2_PROTOCOL_DEFINE(a) +#define W2_PROTOCOL_DECLARE(a) +#define W2_PROTOCOL_PROP(type, name) + +#define W2_CMDDIR_NAME_0 rx +#define W2_CMDDIR_NAME_1 tx + +#define W2_CMDDIR(dir) W2_CMDDIR_NAME_##dir + +#define W2_PROTOCOL_UINT8_T +#define W2_PROTOCOL_UINT16_T +#define W2_PROTOCOL_UINT32_T +#define W2_PROTOCOL_INT8_T +#define W2_PROTOCOL_INT16_T +#define W2_PROTOCOL_INT32_T + +#define W2_PROTOCOL_UINT8_T_TYPE uint8_t +#define W2_PROTOCOL_UINT16_T_TYPE uint16_t +#define W2_PROTOCOL_UINT32_T_TYPE uint32_t +#define W2_PROTOCOL_INT8_T_TYPE int8_t +#define W2_PROTOCOL_INT16_T_TYPE int16_t +#define W2_PROTOCOL_INT32_T_TYPE int32_t + +#define W2_PROTOCOL_UINT8_T_SIZE 1 +#define W2_PROTOCOL_UINT16_T_SIZE 2 +#define W2_PROTOCOL_UINT32_T_SIZE 4 +#define W2_PROTOCOL_INT8_T_SIZE 1 +#define W2_PROTOCOL_INT16_T_SIZE 2 +#define W2_PROTOCOL_INT32_T_SIZE 4 + +#define W2_PROTOCOL_CMD_PING_RX \ +W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \ + W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \ + W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id) \ +) + +W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX) + +typedef struct { + uint8_t opcode; + uint8_t id; +} w2_s_cmd_ping_rx; + +w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data); + diff --git a/shared/serial_parse.c b/shared/serial_parse.c new file mode 100644 index 0000000..c8a3fae --- /dev/null +++ b/shared/serial_parse.c @@ -0,0 +1,54 @@ +#include + +#include "consts.h" +#include "serial_parse.h" +#include "bin.h" +#ifdef W2_SIM +#include "../robot/orangutan_shim.h" +#endif + +void w2_serial_handle(w2_s_bin *code) { +#ifdef W2_SIM + serial_send((char*)code->data, code->bytes); + // simprintf("yeah: %02x\n", code); +#endif +} + +void w2_serial_parse(uint8_t byte) { + static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; + static uint8_t current_message_index = 0; + static uint8_t complete_message_length = 2; + + static bool attentive = false; + static bool listening = false; + + if (byte == W2_SERIAL_START_BYTE) { + attentive = !attentive; + // if (attentive && listening) { + // current_message[current_message_index++] = byte; + // } + } else { + // activate listen after non-0xff byte after 0xff + if (attentive && !listening) { + attentive = false; + listening = true; + } + } + + if (!listening) return; + current_message[current_message_index++] = byte; + + if (current_message_index == complete_message_length) { + w2_s_bin *copy = w2_bin_s_alloc(current_message_index, current_message); + w2_serial_handle(copy); + free(copy); + + memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); + current_message_index = 0; + complete_message_length = 1; + attentive = false; + listening = false; + return; + } +} + diff --git a/shared/serial_parse.h b/shared/serial_parse.h new file mode 100644 index 0000000..516bf8a --- /dev/null +++ b/shared/serial_parse.h @@ -0,0 +1,9 @@ +#pragma once + +#include + +#include "protocol.h" + +/** parse serial data byte by byte */ +void w2_serial_parse(uint8_t byte); + -- cgit v1.2.3 From afa10fa556585d965e3a7d46c2fd5436d29cf3c6 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 16:31:23 +0200 Subject: `make format` --- robot/hypervisor.c | 3 ++- shared/bin.c | 6 +++--- shared/protocol.c | 1 - shared/protocol.h | 34 ++++++++++++++++------------------ shared/serial_parse.c | 17 ++++++++--------- shared/serial_parse.h | 1 - 6 files changed, 29 insertions(+), 33 deletions(-) diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 0f754a3..c4e7aba 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -30,7 +30,8 @@ void w2_hypervisor_main() { if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); if (!g_w2_sim_headless) usleep(100e3); - if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) exit(0); + if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) + exit(0); #endif g_w2_hypervisor_cycles++; diff --git a/shared/bin.c b/shared/bin.c index fc6e14b..44c2e94 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -1,5 +1,5 @@ -#include #include +#include #include "bin.h" @@ -65,8 +65,8 @@ uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) { - w2_s_bin* temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes); - temp->bytes = bytes; + w2_s_bin *temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes); + temp->bytes = bytes; memcpy(&temp->data, data, bytes); return temp; } diff --git a/shared/protocol.c b/shared/protocol.c index 2707725..c634a09 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -7,4 +7,3 @@ w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) { memcpy(parsed, data->data, data->bytes); return parsed; } - diff --git a/shared/protocol.h b/shared/protocol.h index b0602cf..775c2aa 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,8 +1,8 @@ #pragma once +#include #include #include -#include #include "bin.h" @@ -53,7 +53,7 @@ enum w2_e_serial_commands { // generic struct (voor parsen) // parse functie // dump functie -// +// // // als het kan deze allemaal met macro's op deze manier definieren: // @@ -74,25 +74,24 @@ enum w2_e_serial_commands { #define W2_PROTOCOL_INT16_T #define W2_PROTOCOL_INT32_T -#define W2_PROTOCOL_UINT8_T_TYPE uint8_t -#define W2_PROTOCOL_UINT16_T_TYPE uint16_t -#define W2_PROTOCOL_UINT32_T_TYPE uint32_t -#define W2_PROTOCOL_INT8_T_TYPE int8_t -#define W2_PROTOCOL_INT16_T_TYPE int16_t -#define W2_PROTOCOL_INT32_T_TYPE int32_t +#define W2_PROTOCOL_UINT8_T_TYPE uint8_t +#define W2_PROTOCOL_UINT16_T_TYPE uint16_t +#define W2_PROTOCOL_UINT32_T_TYPE uint32_t +#define W2_PROTOCOL_INT8_T_TYPE int8_t +#define W2_PROTOCOL_INT16_T_TYPE int16_t +#define W2_PROTOCOL_INT32_T_TYPE int32_t -#define W2_PROTOCOL_UINT8_T_SIZE 1 +#define W2_PROTOCOL_UINT8_T_SIZE 1 #define W2_PROTOCOL_UINT16_T_SIZE 2 #define W2_PROTOCOL_UINT32_T_SIZE 4 -#define W2_PROTOCOL_INT8_T_SIZE 1 -#define W2_PROTOCOL_INT16_T_SIZE 2 -#define W2_PROTOCOL_INT32_T_SIZE 4 +#define W2_PROTOCOL_INT8_T_SIZE 1 +#define W2_PROTOCOL_INT16_T_SIZE 2 +#define W2_PROTOCOL_INT32_T_SIZE 4 -#define W2_PROTOCOL_CMD_PING_RX \ -W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \ - W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \ - W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id) \ -) +#define W2_PROTOCOL_CMD_PING_RX \ + W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \ + W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \ + W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id)) W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX) @@ -102,4 +101,3 @@ typedef struct { } w2_s_cmd_ping_rx; w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data); - diff --git a/shared/serial_parse.c b/shared/serial_parse.c index c8a3fae..0cda2d8 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,27 +1,27 @@ #include +#include "bin.h" #include "consts.h" #include "serial_parse.h" -#include "bin.h" #ifdef W2_SIM #include "../robot/orangutan_shim.h" #endif void w2_serial_handle(w2_s_bin *code) { #ifdef W2_SIM - serial_send((char*)code->data, code->bytes); + serial_send((char *)code->data, code->bytes); // simprintf("yeah: %02x\n", code); #endif } void w2_serial_parse(uint8_t byte) { static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; - static uint8_t current_message_index = 0; - static uint8_t complete_message_length = 2; + static uint8_t current_message_index = 0; + static uint8_t complete_message_length = 2; static bool attentive = false; static bool listening = false; - + if (byte == W2_SERIAL_START_BYTE) { attentive = !attentive; // if (attentive && listening) { @@ -44,11 +44,10 @@ void w2_serial_parse(uint8_t byte) { free(copy); memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); - current_message_index = 0; + current_message_index = 0; complete_message_length = 1; - attentive = false; - listening = false; + attentive = false; + listening = false; return; } } - diff --git a/shared/serial_parse.h b/shared/serial_parse.h index 516bf8a..a1c8fb9 100644 --- a/shared/serial_parse.h +++ b/shared/serial_parse.h @@ -6,4 +6,3 @@ /** parse serial data byte by byte */ void w2_serial_parse(uint8_t byte); - -- cgit v1.2.3 From 87900d6787f8e0af8d37cc313c4e0510ef0b1bf8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:02:23 +0200 Subject: serial receive working with empty handlers --- robot/sercomm.c | 61 +++++++++++++++ robot/sercomm.h | 23 +----- robot/setup.c | 2 +- robot/sim.c | 31 ++++++++ robot/sim.h | 9 ++- shared/bin.c | 12 ++- shared/bin.h | 25 +----- shared/protocol.c | 92 ++++++++++++++++++++-- shared/protocol.h | 213 ++++++++++++++++++++++++++++++++++++++------------ shared/serial_parse.c | 17 +--- 10 files changed, 363 insertions(+), 122 deletions(-) diff --git a/robot/sercomm.c b/robot/sercomm.c index f4b7eb5..a1aa31c 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -46,3 +46,64 @@ void w2_sercomm_append_msg(w2_s_bin *data) { if (g_w2_sercomm_buffer_full) return; g_w2_sercomm_index = next_index; } + +void w2_scmd_ping_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_mode_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_sped_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_dirc_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_cord_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_bomd_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_sres_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_mcfg_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_sens_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_info_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_disp_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_play_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_cled_rx(w2_s_bin *data) { + return; +} + +void w2_scmd_ping_tx(w2_s_bin *data) { } +void w2_scmd_expt_tx(w2_s_bin *data) { } +void w2_scmd_mode_tx(w2_s_bin *data) { } +void w2_scmd_cord_tx(w2_s_bin *data) { } +void w2_scmd_bomd_tx(w2_s_bin *data) { } +void w2_scmd_sens_tx(w2_s_bin *data) { } +void w2_scmd_info_tx(w2_s_bin *data) { } + diff --git a/robot/sercomm.h b/robot/sercomm.h index a53d3e2..d717f01 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -26,27 +26,6 @@ extern uint8_t g_w2_serial_buffer_head; */ void w2_sercomm_main(); +/** append binary message to send buffer */ void w2_sercomm_append_msg(w2_s_bin *data); -w2_s_bin w2_sercomm_struct_pack(w2_s_property_list generic_struct); - -w2_s_bin w2_sercomm_rx_ping(); -w2_s_bin w2_sercomm_tx_ping(); -w2_s_bin w2_sercomm_tx_expt(); -w2_s_bin w2_sercomm_rx_mode(); -w2_s_bin w2_sercomm_tx_mode(); -w2_s_bin w2_sercomm_rx_sped(); -w2_s_bin w2_sercomm_rx_dirc(); -w2_s_bin w2_sercomm_rx_cord(); -w2_s_bin w2_sercomm_tx_cord(); -w2_s_bin w2_sercomm_rx_bomd(); -w2_s_bin w2_sercomm_tx_bomd(); -w2_s_bin w2_sercomm_rx_sres(); -w2_s_bin w2_sercomm_rx_mcfg(); -w2_s_bin w2_sercomm_rx_sens(); -w2_s_bin w2_sercomm_tx_sens(); -w2_s_bin w2_sercomm_rx_info(); -w2_s_bin w2_sercomm_tx_info(); -w2_s_bin w2_sercomm_rx_disp(); -w2_s_bin w2_sercomm_rx_play(); -w2_s_bin w2_sercomm_rx_cled(); diff --git a/robot/setup.c b/robot/setup.c index 6dfef50..944ea68 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -24,7 +24,7 @@ void w2_setup_main() { // clear lcd clear(); - // start serial input + // start serial i/o serial_set_baud_rate(W2_SERIAL_BAUD); serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); diff --git a/robot/sim.c b/robot/sim.c index 8af672e..57e0ef5 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -7,11 +7,34 @@ #include "sim.h" #include "../shared/consts.h" +#include "../shared/protocol.h" #include "sercomm.h" struct timespec reference_time; // NOLINT bool g_w2_sim_headless = false; +static const char* const W2_CMD_NAMES[] = { + "PING", + "EXPT", + "MODE", + "SPED", + "DIRC", + "CORD", + "BOMD", + "SRES", + "MCFG", + "SENS", + "INFO", + "DISP", + "PLAY", + "CLED", +}; + +static const char* const W2_CMD_DIRECTIONS[] = { + "RX", + "TX", +}; + void time_reset() { simprintfunc("time_reset", ""); clock_gettime(CLOCK_MONOTONIC, &reference_time); @@ -107,3 +130,11 @@ void w2_sim_cycle_begin() { return; } +void w2_sim_print_serial(w2_s_bin *data) { + if (g_w2_sim_headless) return; + simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]); + for (int i = 0; i < data->bytes; i++) + printf(" %02x", data->data[i]); + printf("\n"); +} + diff --git a/robot/sim.h b/robot/sim.h index 336592b..6e2498d 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -5,6 +5,8 @@ #include #include +#include "../shared/bin.h" + extern bool g_w2_sim_headless; // debug fine-tuning @@ -45,11 +47,12 @@ unsigned long get_ms(); // NOLINT void red_led(unsigned char on); // NOLINT void green_led(unsigned char on); // NOLINT void clear(); // NOLINT -void play(const char* melody); // NOLINT +void play(const char *melody); // NOLINT void serial_set_baud_rate(unsigned int rate); // NOLINT -void serial_send(char* message, unsigned int length); // NOLINT -void serial_receive_ring(char* buffer, unsigned char size); // NOLINT +void serial_send(char *message, unsigned int length); // NOLINT +void serial_receive_ring(char *buffer, unsigned char size); // NOLINT unsigned char serial_get_received_bytes(); // NOLINT void w2_sim_setup(int argc, char **argv); void w2_sim_cycle_begin(); +void w2_sim_print_serial(w2_s_bin *data); diff --git a/shared/bin.c b/shared/bin.c index 44c2e94..dac8415 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -15,8 +15,6 @@ #define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B))) #define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B))) -const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)}; - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wshift-count-overflow" w2_s_bin *w2_bin_from_uint8_t(uint8_t data) { @@ -70,3 +68,13 @@ w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) { memcpy(&temp->data, data, bytes); return temp; } + +w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b) { + uint8_t data[a->bytes + b->bytes]; + memcpy(data, a->data, a->bytes); + memcpy(data + a->bytes, b->data, b->bytes); + w2_s_bin* c = w2_bin_s_alloc(a->bytes + b->bytes, data); + free(a); + free(b); + return c; +} diff --git a/shared/bin.h b/shared/bin.h index d9eacd7..2b65c95 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -10,18 +10,6 @@ extern uint8_t g_w2_endianness; -#define W2_T_UINT8_T (0) -#define W2_T_UINT16_T (1) -#define W2_T_UINT32_T (2) - -enum w2_e_struct_types { - W2_ST_UINT8_T, - W2_ST_UINT16_T, - W2_ST_UINT32_T, -}; - -extern const uint8_t W2_STRUCT_T_SIZES[]; - typedef struct { uint16_t bytes; uint8_t data[]; @@ -29,17 +17,8 @@ typedef struct { /** allocate new w2_s_bin struct and fill with `*data` for `bytes` bytes */ w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data); - -typedef struct { - enum w2_e_struct_types type; - uint16_t length; - const uint8_t *data; -} w2_s_struct_property; - -typedef struct { - uint16_t length; - w2_s_struct_property *properties[]; -} w2_s_property_list; +/** concatenate 2 w2_s_bin structs, deallocates `a` and `b` */ +w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b); w2_s_bin *w2_bin_from_uint8_t(uint8_t data); w2_s_bin *w2_bin_from_uint16_t(uint16_t data); diff --git a/shared/protocol.c b/shared/protocol.c index c634a09..af38804 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -1,9 +1,91 @@ +#include + #include "protocol.h" +#ifdef W2_SIM +#include "../robot/orangutan_shim.h" +#endif + +uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { + if ((data[0] & W2_CMD_CODE_MASK) == W2_CMD_PING) return 2; + + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return 2; + + if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2; + + if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5; + + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return 6; + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return 6; + + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return 9; + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return 6; + + if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2; + + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return 1; + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return 0; + + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1; + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41; + + w2_s_bin *copy = w2_bin_s_alloc(data_length, data); + uint8_t length = 1; -W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX) + if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_scmd_expt_tx_length(copy); + if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_scmd_mcfg_rx_length(copy); -w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) { - w2_s_cmd_ping_rx *parsed = malloc(data->bytes); - memcpy(parsed, data->data, data->bytes); - return parsed; + free(copy); + + return length; +} + +uint8_t w2_scmd_expt_tx_length(w2_s_bin *data) { + return 3 + (data->bytes > 2 ? (sizeof(uint8_t) * data->data[2]) : 0); } + +uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data) { + // TODO 3 = sizeof(feature) -> protocol.md + return 4 + (data->bytes > 3 ? (3 * data->data[3]) : 0); +} + +void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { + w2_s_bin *copy = w2_bin_s_alloc(data_length, data); + bool unknown = false; + + switch(data[0]) { + case W2_CMD_PING | W2_CMDDIR_RX: { w2_scmd_ping_rx(copy); break; } + case W2_CMD_PING | W2_CMDDIR_TX: { w2_scmd_ping_tx(copy); break; } + case W2_CMD_EXPT | W2_CMDDIR_TX: { w2_scmd_expt_tx(copy); break; } + case W2_CMD_MODE | W2_CMDDIR_RX: { w2_scmd_mode_rx(copy); break; } + case W2_CMD_MODE | W2_CMDDIR_TX: { w2_scmd_mode_tx(copy); break; } + case W2_CMD_SPED | W2_CMDDIR_RX: { w2_scmd_sped_rx(copy); break; } + case W2_CMD_DIRC | W2_CMDDIR_RX: { w2_scmd_dirc_rx(copy); break; } + case W2_CMD_CORD | W2_CMDDIR_RX: { w2_scmd_cord_rx(copy); break; } + case W2_CMD_CORD | W2_CMDDIR_TX: { w2_scmd_cord_tx(copy); break; } + case W2_CMD_BOMD | W2_CMDDIR_RX: { w2_scmd_bomd_rx(copy); break; } + case W2_CMD_BOMD | W2_CMDDIR_TX: { w2_scmd_bomd_tx(copy); break; } + case W2_CMD_SRES | W2_CMDDIR_RX: { w2_scmd_sres_rx(copy); break; } + case W2_CMD_MCFG | W2_CMDDIR_RX: { w2_scmd_mcfg_rx(copy); break; } + case W2_CMD_SENS | W2_CMDDIR_RX: { w2_scmd_sens_rx(copy); break; } + case W2_CMD_SENS | W2_CMDDIR_TX: { w2_scmd_sens_tx(copy); break; } + case W2_CMD_INFO | W2_CMDDIR_RX: { w2_scmd_info_rx(copy); break; } + case W2_CMD_INFO | W2_CMDDIR_TX: { w2_scmd_info_tx(copy); break; } + case W2_CMD_DISP | W2_CMDDIR_RX: { w2_scmd_disp_rx(copy); break; } + case W2_CMD_PLAY | W2_CMDDIR_RX: { w2_scmd_play_rx(copy); break; } + case W2_CMD_CLED | W2_CMDDIR_RX: { w2_scmd_cled_rx(copy); break; } + default: { +#ifdef W2_SIM + simwarn("unknown serial message with code 0x%02x\n", data[0]); +#endif + unknown = true; + } + } + +#ifdef W2_SIM + if (!unknown) w2_sim_print_serial(copy); +#endif + + free(copy); +} + diff --git a/shared/protocol.h b/shared/protocol.h index 775c2aa..a9967d7 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -5,6 +5,7 @@ #include #include "bin.h" +#include "consts.h" #define W2_SERIAL_START_BYTE 0xff @@ -14,7 +15,7 @@ #define W2_CMD_CODE_MASK (~1) #define W2_CMD_DIRECTION_MASK (1) -enum w2_e_serial_commands { +enum w2_e_scmds { /** ping command */ W2_CMD_PING = 0x00, /** exception command */ @@ -45,59 +46,167 @@ enum w2_e_serial_commands { W2_CMD_CLED = 0x1a, }; -// TODO -// array met indicies die structs opslaan met eigenschappen over de protocol bericht -// -// belangrijke eigenschappen: -// lengte!!! -// generic struct (voor parsen) -// parse functie -// dump functie -// -// -// als het kan deze allemaal met macro's op deze manier definieren: -// -#define W2_PROTOCOL_CMD(name, direction, ...) -#define W2_PROTOCOL_DEFINE(a) -#define W2_PROTOCOL_DECLARE(a) -#define W2_PROTOCOL_PROP(type, name) - -#define W2_CMDDIR_NAME_0 rx -#define W2_CMDDIR_NAME_1 tx - -#define W2_CMDDIR(dir) W2_CMDDIR_NAME_##dir - -#define W2_PROTOCOL_UINT8_T -#define W2_PROTOCOL_UINT16_T -#define W2_PROTOCOL_UINT32_T -#define W2_PROTOCOL_INT8_T -#define W2_PROTOCOL_INT16_T -#define W2_PROTOCOL_INT32_T - -#define W2_PROTOCOL_UINT8_T_TYPE uint8_t -#define W2_PROTOCOL_UINT16_T_TYPE uint16_t -#define W2_PROTOCOL_UINT32_T_TYPE uint32_t -#define W2_PROTOCOL_INT8_T_TYPE int8_t -#define W2_PROTOCOL_INT16_T_TYPE int16_t -#define W2_PROTOCOL_INT32_T_TYPE int32_t - -#define W2_PROTOCOL_UINT8_T_SIZE 1 -#define W2_PROTOCOL_UINT16_T_SIZE 2 -#define W2_PROTOCOL_UINT32_T_SIZE 4 -#define W2_PROTOCOL_INT8_T_SIZE 1 -#define W2_PROTOCOL_INT16_T_SIZE 2 -#define W2_PROTOCOL_INT32_T_SIZE 4 - -#define W2_PROTOCOL_CMD_PING_RX \ - W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \ - W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \ - W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id)) - -W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX) - typedef struct { uint8_t opcode; uint8_t id; } w2_s_cmd_ping_rx; -w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data); +typedef struct { + uint8_t opcode; + uint8_t id; +} w2_s_cmd_ping_tx; + +typedef struct { + uint8_t opcode; + uint8_t error; + uint8_t length; + uint8_t message[]; +} w2_s_cmd_expt_tx; + +typedef struct { + uint8_t opcode; + uint8_t mode; +} w2_s_cmd_mode_rx; + +typedef struct { + uint8_t opcode; + uint8_t mode; +} w2_s_cmd_mode_tx; + +typedef struct { + uint8_t opcode; + uint8_t speed; +} w2_s_cmd_sped_rx; + +typedef struct { + uint8_t opcode; + uint16_t left; + uint16_t right; +} w2_s_cmd_dirc_rx; + +typedef struct { + uint8_t opcode; + uint32_t position; + uint8_t orientation; +} w2_s_cmd_cord_rx; + +typedef struct { + uint8_t opcode; + uint32_t position; + uint8_t orientation; +} w2_s_cmd_cord_tx; + +typedef struct { + uint8_t opcode; + uint32_t id; + uint32_t position; +} w2_s_cmd_bomd_rx; + +typedef struct { + uint8_t opcode; + uint32_t id; + uint8_t status; +} w2_s_cmd_bomd_tx; + +typedef struct { + uint8_t opcode; + uint8_t type; +} w2_s_cmd_sres_rx; + +typedef struct { + uint16_t position; + uint8_t kind; +} w2_s_cmd_mcfg_feature; + +typedef struct { + uint8_t opcode; + uint8_t width; + uint8_t height; + uint8_t length; + w2_s_cmd_mcfg_feature features[]; +} w2_s_cmd_mcfg_rx; + +typedef struct { + uint8_t opcode; +} w2_s_cmd_sens_rx; + +typedef struct { + uint8_t opcode; + // TODO: sensor data +} w2_s_cmd_sens_tx; + +typedef struct { + uint8_t opcode; +} w2_s_cmd_info_rx; + +typedef struct { + uint8_t opcode; + uint8_t build_str[32]; + uint8_t errcatch_ms; + uint8_t io_ms; + uint8_t sercomm_ms; + uint8_t mode_ms; + uint32_t uptime_s; +} w2_s_cmd_info_tx; + +typedef struct { +} w2_s_cmd_disp_rx; + +typedef struct { +} w2_s_cmd_play_rx; + +typedef struct { +} w2_s_cmd_cled_rx; + + +/** global handler for complete messages */ +void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); +/** calculate message length */ +uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); + +/** handler for ping_rx (on complete message) */ +void w2_scmd_ping_rx(w2_s_bin *data); +/** handler for ping_tx (on complete message) */ +void w2_scmd_ping_tx(w2_s_bin *data); +/** handler for expt_tx (on complete message) */ +void w2_scmd_expt_tx(w2_s_bin *data); +/** handler for mode_rx (on complete message) */ +void w2_scmd_mode_rx(w2_s_bin *data); +/** handler for mode_tx (on complete message) */ +void w2_scmd_mode_tx(w2_s_bin *data); +/** handler for sped_rx (on complete message) */ +void w2_scmd_sped_rx(w2_s_bin *data); +/** handler for dirc_rx (on complete message) */ +void w2_scmd_dirc_rx(w2_s_bin *data); +/** handler for cord_rx (on complete message) */ +void w2_scmd_cord_rx(w2_s_bin *data); +/** handler for cord_tx (on complete message) */ +void w2_scmd_cord_tx(w2_s_bin *data); +/** handler for bomd_rx (on complete message) */ +void w2_scmd_bomd_rx(w2_s_bin *data); +/** handler for bomd_tx (on complete message) */ +void w2_scmd_bomd_tx(w2_s_bin *data); +/** handler for sres_rx (on complete message) */ +void w2_scmd_sres_rx(w2_s_bin *data); +/** handler for mcfg_rx (on complete message) */ +void w2_scmd_mcfg_rx(w2_s_bin *data); +/** handler for sens_rx (on complete message) */ +void w2_scmd_sens_rx(w2_s_bin *data); +/** handler for sens_tx (on complete message) */ +void w2_scmd_sens_tx(w2_s_bin *data); +/** handler for info_rx (on complete message) */ +void w2_scmd_info_rx(w2_s_bin *data); +/** handler for info_tx (on complete message) */ +void w2_scmd_info_tx(w2_s_bin *data); +/** handler for disp_rx (on complete message) */ +void w2_scmd_disp_rx(w2_s_bin *data); +/** handler for play_rx (on complete message) */ +void w2_scmd_play_rx(w2_s_bin *data); +/** handler for cled_rx (on complete message) */ +void w2_scmd_cled_rx(w2_s_bin *data); + +/** calculate message length for expt_tx (incomplete message) */ +uint8_t w2_scmd_expt_tx_length(w2_s_bin *data); +/** calculate message length for mcfg_rx (incomplete message) */ +uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data); + diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 0cda2d8..0ebe377 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,18 +1,7 @@ #include -#include "bin.h" #include "consts.h" #include "serial_parse.h" -#ifdef W2_SIM -#include "../robot/orangutan_shim.h" -#endif - -void w2_serial_handle(w2_s_bin *code) { -#ifdef W2_SIM - serial_send((char *)code->data, code->bytes); - // simprintf("yeah: %02x\n", code); -#endif -} void w2_serial_parse(uint8_t byte) { static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; @@ -38,10 +27,10 @@ void w2_serial_parse(uint8_t byte) { if (!listening) return; current_message[current_message_index++] = byte; + complete_message_length = w2_scmd_length(current_message, current_message_index); + if (current_message_index == complete_message_length) { - w2_s_bin *copy = w2_bin_s_alloc(current_message_index, current_message); - w2_serial_handle(copy); - free(copy); + w2_scmd_handler(current_message, current_message_index); memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); current_message_index = 0; -- cgit v1.2.3 From 734e5ea7d05c0cebf219cff78c6b0794f4cbe9ae Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:03:24 +0200 Subject: `make format` --- robot/sercomm.c | 67 ++++++++++------------------------ robot/sercomm.h | 1 - shared/bin.c | 2 +- shared/protocol.c | 107 ++++++++++++++++++++++++++++++++++++++++++------------ shared/protocol.h | 42 ++++++++++----------- 5 files changed, 124 insertions(+), 95 deletions(-) diff --git a/robot/sercomm.c b/robot/sercomm.c index a1aa31c..da632b5 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -47,63 +47,36 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -void w2_scmd_ping_rx(w2_s_bin *data) { - return; -} +void w2_scmd_ping_rx(w2_s_bin *data) { return; } -void w2_scmd_mode_rx(w2_s_bin *data) { - return; -} +void w2_scmd_mode_rx(w2_s_bin *data) { return; } -void w2_scmd_sped_rx(w2_s_bin *data) { - return; -} +void w2_scmd_sped_rx(w2_s_bin *data) { return; } -void w2_scmd_dirc_rx(w2_s_bin *data) { - return; -} +void w2_scmd_dirc_rx(w2_s_bin *data) { return; } -void w2_scmd_cord_rx(w2_s_bin *data) { - return; -} - -void w2_scmd_bomd_rx(w2_s_bin *data) { - return; -} +void w2_scmd_cord_rx(w2_s_bin *data) { return; } -void w2_scmd_sres_rx(w2_s_bin *data) { - return; -} +void w2_scmd_bomd_rx(w2_s_bin *data) { return; } -void w2_scmd_mcfg_rx(w2_s_bin *data) { - return; -} +void w2_scmd_sres_rx(w2_s_bin *data) { return; } -void w2_scmd_sens_rx(w2_s_bin *data) { - return; -} +void w2_scmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_scmd_info_rx(w2_s_bin *data) { - return; -} +void w2_scmd_sens_rx(w2_s_bin *data) { return; } -void w2_scmd_disp_rx(w2_s_bin *data) { - return; -} +void w2_scmd_info_rx(w2_s_bin *data) { return; } -void w2_scmd_play_rx(w2_s_bin *data) { - return; -} +void w2_scmd_disp_rx(w2_s_bin *data) { return; } -void w2_scmd_cled_rx(w2_s_bin *data) { - return; -} +void w2_scmd_play_rx(w2_s_bin *data) { return; } -void w2_scmd_ping_tx(w2_s_bin *data) { } -void w2_scmd_expt_tx(w2_s_bin *data) { } -void w2_scmd_mode_tx(w2_s_bin *data) { } -void w2_scmd_cord_tx(w2_s_bin *data) { } -void w2_scmd_bomd_tx(w2_s_bin *data) { } -void w2_scmd_sens_tx(w2_s_bin *data) { } -void w2_scmd_info_tx(w2_s_bin *data) { } +void w2_scmd_cled_rx(w2_s_bin *data) { return; } +void w2_scmd_ping_tx(w2_s_bin *data) {} +void w2_scmd_expt_tx(w2_s_bin *data) {} +void w2_scmd_mode_tx(w2_s_bin *data) {} +void w2_scmd_cord_tx(w2_s_bin *data) {} +void w2_scmd_bomd_tx(w2_s_bin *data) {} +void w2_scmd_sens_tx(w2_s_bin *data) {} +void w2_scmd_info_tx(w2_s_bin *data) {} diff --git a/robot/sercomm.h b/robot/sercomm.h index d717f01..b1f69c7 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -28,4 +28,3 @@ void w2_sercomm_main(); /** append binary message to send buffer */ void w2_sercomm_append_msg(w2_s_bin *data); - diff --git a/shared/bin.c b/shared/bin.c index dac8415..e9592e6 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -73,7 +73,7 @@ w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b) { uint8_t data[a->bytes + b->bytes]; memcpy(data, a->data, a->bytes); memcpy(data + a->bytes, b->data, b->bytes); - w2_s_bin* c = w2_bin_s_alloc(a->bytes + b->bytes, data); + w2_s_bin *c = w2_bin_s_alloc(a->bytes + b->bytes, data); free(a); free(b); return c; diff --git a/shared/protocol.c b/shared/protocol.c index af38804..18ee227 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -51,29 +51,89 @@ uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data) { void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { w2_s_bin *copy = w2_bin_s_alloc(data_length, data); - bool unknown = false; - - switch(data[0]) { - case W2_CMD_PING | W2_CMDDIR_RX: { w2_scmd_ping_rx(copy); break; } - case W2_CMD_PING | W2_CMDDIR_TX: { w2_scmd_ping_tx(copy); break; } - case W2_CMD_EXPT | W2_CMDDIR_TX: { w2_scmd_expt_tx(copy); break; } - case W2_CMD_MODE | W2_CMDDIR_RX: { w2_scmd_mode_rx(copy); break; } - case W2_CMD_MODE | W2_CMDDIR_TX: { w2_scmd_mode_tx(copy); break; } - case W2_CMD_SPED | W2_CMDDIR_RX: { w2_scmd_sped_rx(copy); break; } - case W2_CMD_DIRC | W2_CMDDIR_RX: { w2_scmd_dirc_rx(copy); break; } - case W2_CMD_CORD | W2_CMDDIR_RX: { w2_scmd_cord_rx(copy); break; } - case W2_CMD_CORD | W2_CMDDIR_TX: { w2_scmd_cord_tx(copy); break; } - case W2_CMD_BOMD | W2_CMDDIR_RX: { w2_scmd_bomd_rx(copy); break; } - case W2_CMD_BOMD | W2_CMDDIR_TX: { w2_scmd_bomd_tx(copy); break; } - case W2_CMD_SRES | W2_CMDDIR_RX: { w2_scmd_sres_rx(copy); break; } - case W2_CMD_MCFG | W2_CMDDIR_RX: { w2_scmd_mcfg_rx(copy); break; } - case W2_CMD_SENS | W2_CMDDIR_RX: { w2_scmd_sens_rx(copy); break; } - case W2_CMD_SENS | W2_CMDDIR_TX: { w2_scmd_sens_tx(copy); break; } - case W2_CMD_INFO | W2_CMDDIR_RX: { w2_scmd_info_rx(copy); break; } - case W2_CMD_INFO | W2_CMDDIR_TX: { w2_scmd_info_tx(copy); break; } - case W2_CMD_DISP | W2_CMDDIR_RX: { w2_scmd_disp_rx(copy); break; } - case W2_CMD_PLAY | W2_CMDDIR_RX: { w2_scmd_play_rx(copy); break; } - case W2_CMD_CLED | W2_CMDDIR_RX: { w2_scmd_cled_rx(copy); break; } + bool unknown = false; + + switch (data[0]) { + case W2_CMD_PING | W2_CMDDIR_RX: { + w2_scmd_ping_rx(copy); + break; + } + case W2_CMD_PING | W2_CMDDIR_TX: { + w2_scmd_ping_tx(copy); + break; + } + case W2_CMD_EXPT | W2_CMDDIR_TX: { + w2_scmd_expt_tx(copy); + break; + } + case W2_CMD_MODE | W2_CMDDIR_RX: { + w2_scmd_mode_rx(copy); + break; + } + case W2_CMD_MODE | W2_CMDDIR_TX: { + w2_scmd_mode_tx(copy); + break; + } + case W2_CMD_SPED | W2_CMDDIR_RX: { + w2_scmd_sped_rx(copy); + break; + } + case W2_CMD_DIRC | W2_CMDDIR_RX: { + w2_scmd_dirc_rx(copy); + break; + } + case W2_CMD_CORD | W2_CMDDIR_RX: { + w2_scmd_cord_rx(copy); + break; + } + case W2_CMD_CORD | W2_CMDDIR_TX: { + w2_scmd_cord_tx(copy); + break; + } + case W2_CMD_BOMD | W2_CMDDIR_RX: { + w2_scmd_bomd_rx(copy); + break; + } + case W2_CMD_BOMD | W2_CMDDIR_TX: { + w2_scmd_bomd_tx(copy); + break; + } + case W2_CMD_SRES | W2_CMDDIR_RX: { + w2_scmd_sres_rx(copy); + break; + } + case W2_CMD_MCFG | W2_CMDDIR_RX: { + w2_scmd_mcfg_rx(copy); + break; + } + case W2_CMD_SENS | W2_CMDDIR_RX: { + w2_scmd_sens_rx(copy); + break; + } + case W2_CMD_SENS | W2_CMDDIR_TX: { + w2_scmd_sens_tx(copy); + break; + } + case W2_CMD_INFO | W2_CMDDIR_RX: { + w2_scmd_info_rx(copy); + break; + } + case W2_CMD_INFO | W2_CMDDIR_TX: { + w2_scmd_info_tx(copy); + break; + } + case W2_CMD_DISP | W2_CMDDIR_RX: { + w2_scmd_disp_rx(copy); + break; + } + case W2_CMD_PLAY | W2_CMDDIR_RX: { + w2_scmd_play_rx(copy); + break; + } + case W2_CMD_CLED | W2_CMDDIR_RX: { + w2_scmd_cled_rx(copy); + break; + } default: { #ifdef W2_SIM simwarn("unknown serial message with code 0x%02x\n", data[0]); @@ -88,4 +148,3 @@ void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_leng free(copy); } - diff --git a/shared/protocol.h b/shared/protocol.h index a9967d7..52ba2bd 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -158,55 +158,53 @@ typedef struct { typedef struct { } w2_s_cmd_cled_rx; - /** global handler for complete messages */ void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** calculate message length */ uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** handler for ping_rx (on complete message) */ -void w2_scmd_ping_rx(w2_s_bin *data); +void w2_scmd_ping_rx(w2_s_bin *data); /** handler for ping_tx (on complete message) */ -void w2_scmd_ping_tx(w2_s_bin *data); +void w2_scmd_ping_tx(w2_s_bin *data); /** handler for expt_tx (on complete message) */ -void w2_scmd_expt_tx(w2_s_bin *data); +void w2_scmd_expt_tx(w2_s_bin *data); /** handler for mode_rx (on complete message) */ -void w2_scmd_mode_rx(w2_s_bin *data); +void w2_scmd_mode_rx(w2_s_bin *data); /** handler for mode_tx (on complete message) */ -void w2_scmd_mode_tx(w2_s_bin *data); +void w2_scmd_mode_tx(w2_s_bin *data); /** handler for sped_rx (on complete message) */ -void w2_scmd_sped_rx(w2_s_bin *data); +void w2_scmd_sped_rx(w2_s_bin *data); /** handler for dirc_rx (on complete message) */ -void w2_scmd_dirc_rx(w2_s_bin *data); +void w2_scmd_dirc_rx(w2_s_bin *data); /** handler for cord_rx (on complete message) */ -void w2_scmd_cord_rx(w2_s_bin *data); +void w2_scmd_cord_rx(w2_s_bin *data); /** handler for cord_tx (on complete message) */ -void w2_scmd_cord_tx(w2_s_bin *data); +void w2_scmd_cord_tx(w2_s_bin *data); /** handler for bomd_rx (on complete message) */ -void w2_scmd_bomd_rx(w2_s_bin *data); +void w2_scmd_bomd_rx(w2_s_bin *data); /** handler for bomd_tx (on complete message) */ -void w2_scmd_bomd_tx(w2_s_bin *data); +void w2_scmd_bomd_tx(w2_s_bin *data); /** handler for sres_rx (on complete message) */ -void w2_scmd_sres_rx(w2_s_bin *data); +void w2_scmd_sres_rx(w2_s_bin *data); /** handler for mcfg_rx (on complete message) */ -void w2_scmd_mcfg_rx(w2_s_bin *data); +void w2_scmd_mcfg_rx(w2_s_bin *data); /** handler for sens_rx (on complete message) */ -void w2_scmd_sens_rx(w2_s_bin *data); +void w2_scmd_sens_rx(w2_s_bin *data); /** handler for sens_tx (on complete message) */ -void w2_scmd_sens_tx(w2_s_bin *data); +void w2_scmd_sens_tx(w2_s_bin *data); /** handler for info_rx (on complete message) */ -void w2_scmd_info_rx(w2_s_bin *data); +void w2_scmd_info_rx(w2_s_bin *data); /** handler for info_tx (on complete message) */ -void w2_scmd_info_tx(w2_s_bin *data); +void w2_scmd_info_tx(w2_s_bin *data); /** handler for disp_rx (on complete message) */ -void w2_scmd_disp_rx(w2_s_bin *data); +void w2_scmd_disp_rx(w2_s_bin *data); /** handler for play_rx (on complete message) */ -void w2_scmd_play_rx(w2_s_bin *data); +void w2_scmd_play_rx(w2_s_bin *data); /** handler for cled_rx (on complete message) */ -void w2_scmd_cled_rx(w2_s_bin *data); +void w2_scmd_cled_rx(w2_s_bin *data); /** calculate message length for expt_tx (incomplete message) */ uint8_t w2_scmd_expt_tx_length(w2_s_bin *data); /** calculate message length for mcfg_rx (incomplete message) */ uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data); - -- cgit v1.2.3 From 931cf5f2c3a0fdf947ba2c4b839bc886a43b4701 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:16:41 +0200 Subject: implement ping command --- protocol.md | 4 ++-- robot/sercomm.c | 18 +++++++++++++++++- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/protocol.md b/protocol.md index d41b00f..4db4032 100644 --- a/protocol.md +++ b/protocol.md @@ -27,12 +27,12 @@ and the starting byte don't count towards message length. opcodes are picked sequentially, but the direction bit (LSB) is reserved to indicate a transfer from robot to client (`tx`). this means that the opcode for a sensor data request would be `0x12`, but the response opcode would be `0x13`. -these opcodes are stored as enum constants inside consts.h for code +these opcodes are stored as enum constants inside shared/protocol.h for code readability. |code|name|implemented|directions|full name| |--:|---|:-:|:-:|---| -|`0x00`|[PING](#ping)|no|`r <=> c`|ping +|`0x00`|[PING](#ping)|yes|`r <=> c`|ping |`0x02`|[EXPT](#expt)|no|`r --> c`|exception |`0x04`|[MODE](#mode)|no|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed diff --git a/robot/sercomm.c b/robot/sercomm.c index da632b5..44bf5f9 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -47,7 +47,23 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -void w2_scmd_ping_rx(w2_s_bin *data) { return; } +void w2_scmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + 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->id = message->id; + + 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_scmd_mode_rx(w2_s_bin *data) { return; } -- cgit v1.2.3 From 4ebf24f2c973693d1a05d6cfc7094d46d71b67ae Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:25:26 +0200 Subject: read serial data before answering for quicker responses --- robot/sercomm.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/robot/sercomm.c b/robot/sercomm.c index 44bf5f9..9e40b0e 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -19,6 +19,12 @@ void w2_sercomm_main() { #ifdef W2_SIM simprintfunc("w2_sercomm_main", ""); #endif + // read and parse data + while (serial_get_received_bytes() != g_w2_serial_buffer_index) { + w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]); + g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; + } + // send data while (g_w2_sercomm_offset != g_w2_sercomm_index) { w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; @@ -27,12 +33,6 @@ void w2_sercomm_main() { serial_send(data_cast, data->bytes); g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } - - // read and parse data - while (serial_get_received_bytes() != g_w2_serial_buffer_index) { - w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]); - g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; - } } void w2_sercomm_append_msg(w2_s_bin *data) { -- cgit v1.2.3 From e63abbdff3408c17b45c9032e7f7ba0199b43bd0 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 23:36:43 +0200 Subject: struct packing and dynamic size calculation --- robot/sercomm.c | 42 +++++++++++------------ robot/sim.c | 29 +++------------- robot/tests/mcfg.bin | Bin 0 -> 11 bytes shared/protocol.c | 90 ++++++++++++++++++++++++++------------------------ shared/protocol.h | 52 +++++++++++++++-------------- shared/serial_parse.c | 4 +-- 6 files changed, 103 insertions(+), 114 deletions(-) create mode 100644 robot/tests/mcfg.bin diff --git a/robot/sercomm.c b/robot/sercomm.c index 9e40b0e..293e3c6 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -47,8 +47,8 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -void w2_scmd_ping_rx(w2_s_bin *data) { - w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes)); +void w2_cmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); memcpy(message, data->data, data->bytes); size_t return_size = sizeof(w2_s_cmd_ping_tx); @@ -65,34 +65,34 @@ void w2_scmd_ping_rx(w2_s_bin *data) { free(return_message_bin); } -void w2_scmd_mode_rx(w2_s_bin *data) { return; } +void w2_cmd_mode_rx(w2_s_bin *data) { return; } -void w2_scmd_sped_rx(w2_s_bin *data) { return; } +void w2_cmd_sped_rx(w2_s_bin *data) { return; } -void w2_scmd_dirc_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { return; } -void w2_scmd_cord_rx(w2_s_bin *data) { return; } +void w2_cmd_cord_rx(w2_s_bin *data) { return; } -void w2_scmd_bomd_rx(w2_s_bin *data) { return; } +void w2_cmd_bomd_rx(w2_s_bin *data) { return; } -void w2_scmd_sres_rx(w2_s_bin *data) { return; } +void w2_cmd_sres_rx(w2_s_bin *data) { return; } -void w2_scmd_mcfg_rx(w2_s_bin *data) { return; } +void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_scmd_sens_rx(w2_s_bin *data) { return; } +void w2_cmd_sens_rx(w2_s_bin *data) { return; } -void w2_scmd_info_rx(w2_s_bin *data) { return; } +void w2_cmd_info_rx(w2_s_bin *data) { return; } -void w2_scmd_disp_rx(w2_s_bin *data) { return; } +void w2_cmd_disp_rx(w2_s_bin *data) { return; } -void w2_scmd_play_rx(w2_s_bin *data) { return; } +void w2_cmd_play_rx(w2_s_bin *data) { return; } -void w2_scmd_cled_rx(w2_s_bin *data) { return; } +void w2_cmd_cled_rx(w2_s_bin *data) { return; } -void w2_scmd_ping_tx(w2_s_bin *data) {} -void w2_scmd_expt_tx(w2_s_bin *data) {} -void w2_scmd_mode_tx(w2_s_bin *data) {} -void w2_scmd_cord_tx(w2_s_bin *data) {} -void w2_scmd_bomd_tx(w2_s_bin *data) {} -void w2_scmd_sens_tx(w2_s_bin *data) {} -void w2_scmd_info_tx(w2_s_bin *data) {} +void w2_cmd_ping_tx(w2_s_bin *data) {} +void w2_cmd_expt_tx(w2_s_bin *data) {} +void w2_cmd_mode_tx(w2_s_bin *data) {} +void w2_cmd_cord_tx(w2_s_bin *data) {} +void w2_cmd_bomd_tx(w2_s_bin *data) {} +void w2_cmd_sens_tx(w2_s_bin *data) {} +void w2_cmd_info_tx(w2_s_bin *data) {} diff --git a/robot/sim.c b/robot/sim.c index 57e0ef5..0cde6a0 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -38,7 +38,6 @@ static const char* const W2_CMD_DIRECTIONS[] = { void time_reset() { simprintfunc("time_reset", ""); clock_gettime(CLOCK_MONOTONIC, &reference_time); - return; } unsigned long get_ms() { @@ -51,27 +50,22 @@ unsigned long get_ms() { void red_led(unsigned char on) { simprintfunc("red_led", "%i", on); - return; } void green_led(unsigned char on) { simprintfunc("green_led", "%i", on); - return; } void clear() { simprintfunc("clear", ""); - return; } void play(const char* melody) { simprintfunc("play", "\"%s\"", melody); - return; } void serial_set_baud_rate(unsigned int rate) { simprintfunc("serial_set_baud_rate", "%u", rate); - return; } void serial_send(char* message, unsigned int length) { @@ -81,25 +75,16 @@ void serial_send(char* message, unsigned int length) { return; } if (!DBG_ENABLE_PRINTFUNC) return; + simprintfunc("serial_send", ", %u", length); - unsigned int bytes = 0; - simprintf(""); - for (unsigned int byte = 0; byte < length; byte++) { - if (bytes > DBG_BYTES_PER_LINE) { - bytes = 0; - printf("\n"); - simprintf(""); - } - printf("%02x ", message[byte] & 0xff); - bytes++; - } - printf("\n"); - return; + + w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message); + w2_sim_print_serial(bin); + free(bin); } void serial_receive_ring(char* buffer, unsigned char size) { simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size); - return; } unsigned char serial_get_received_bytes() { @@ -118,16 +103,12 @@ void w2_sim_setup(int argc, char **argv) { term.c_cc[VTIME] = 0; term.c_cc[VMIN] = 0; tcsetattr(STDIN_FILENO, 0, &term); - - return; } void w2_sim_cycle_begin() { // read bytes from stdin while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0) g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE; - - return; } void w2_sim_print_serial(w2_s_bin *data) { diff --git a/robot/tests/mcfg.bin b/robot/tests/mcfg.bin new file mode 100644 index 0000000..a43ecf8 Binary files /dev/null and b/robot/tests/mcfg.bin differ diff --git a/shared/protocol.c b/shared/protocol.c index 18ee227..73063f3 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -5,133 +5,137 @@ #include "../robot/orangutan_shim.h" #endif -uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { - if ((data[0] & W2_CMD_CODE_MASK) == W2_CMD_PING) return 2; +size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { + if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx); + if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx); - if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return 2; - if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return 2; + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_mode_rx); + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_mode_tx); - if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sped_rx); - if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5; + if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_dirc_rx); - if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return 6; - if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return 6; + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_cord_rx); + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_cord_tx); - if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return 9; - if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return 6; + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_bomd_rx); + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_bomd_tx); - if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sres_rx); - if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return 1; - if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return 0; + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sens_rx); + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_sens_tx); - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1; - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41; + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_info_rx); + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_info_tx); w2_s_bin *copy = w2_bin_s_alloc(data_length, data); uint8_t length = 1; - if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_scmd_expt_tx_length(copy); - if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_scmd_mcfg_rx_length(copy); + if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_cmd_expt_tx_sizeof(copy); + if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_cmd_mcfg_rx_sizeof(copy); free(copy); return length; } -uint8_t w2_scmd_expt_tx_length(w2_s_bin *data) { - return 3 + (data->bytes > 2 ? (sizeof(uint8_t) * data->data[2]) : 0); +#define W2_DYN_MEMBER_SIZEOF(struct_t, length_byte, trailing_type) \ + sizeof(struct_t) + \ + (data->bytes > length_byte ? (sizeof(trailing_type) * data->data[length_byte]) : 0) + +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data) { + return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_expt_tx, 2, uint8_t); } -uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data) { - // TODO 3 = sizeof(feature) -> protocol.md - return 4 + (data->bytes > 3 ? (3 * data->data[3]) : 0); +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) { + return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_mcfg_rx, 3, w2_s_cmd_mcfg_feature); } -void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { +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); bool unknown = false; switch (data[0]) { case W2_CMD_PING | W2_CMDDIR_RX: { - w2_scmd_ping_rx(copy); + w2_cmd_ping_rx(copy); break; } case W2_CMD_PING | W2_CMDDIR_TX: { - w2_scmd_ping_tx(copy); + w2_cmd_ping_tx(copy); break; } case W2_CMD_EXPT | W2_CMDDIR_TX: { - w2_scmd_expt_tx(copy); + w2_cmd_expt_tx(copy); break; } case W2_CMD_MODE | W2_CMDDIR_RX: { - w2_scmd_mode_rx(copy); + w2_cmd_mode_rx(copy); break; } case W2_CMD_MODE | W2_CMDDIR_TX: { - w2_scmd_mode_tx(copy); + w2_cmd_mode_tx(copy); break; } case W2_CMD_SPED | W2_CMDDIR_RX: { - w2_scmd_sped_rx(copy); + w2_cmd_sped_rx(copy); break; } case W2_CMD_DIRC | W2_CMDDIR_RX: { - w2_scmd_dirc_rx(copy); + w2_cmd_dirc_rx(copy); break; } case W2_CMD_CORD | W2_CMDDIR_RX: { - w2_scmd_cord_rx(copy); + w2_cmd_cord_rx(copy); break; } case W2_CMD_CORD | W2_CMDDIR_TX: { - w2_scmd_cord_tx(copy); + w2_cmd_cord_tx(copy); break; } case W2_CMD_BOMD | W2_CMDDIR_RX: { - w2_scmd_bomd_rx(copy); + w2_cmd_bomd_rx(copy); break; } case W2_CMD_BOMD | W2_CMDDIR_TX: { - w2_scmd_bomd_tx(copy); + w2_cmd_bomd_tx(copy); break; } case W2_CMD_SRES | W2_CMDDIR_RX: { - w2_scmd_sres_rx(copy); + w2_cmd_sres_rx(copy); break; } case W2_CMD_MCFG | W2_CMDDIR_RX: { - w2_scmd_mcfg_rx(copy); + w2_cmd_mcfg_rx(copy); break; } case W2_CMD_SENS | W2_CMDDIR_RX: { - w2_scmd_sens_rx(copy); + w2_cmd_sens_rx(copy); break; } case W2_CMD_SENS | W2_CMDDIR_TX: { - w2_scmd_sens_tx(copy); + w2_cmd_sens_tx(copy); break; } case W2_CMD_INFO | W2_CMDDIR_RX: { - w2_scmd_info_rx(copy); + w2_cmd_info_rx(copy); break; } case W2_CMD_INFO | W2_CMDDIR_TX: { - w2_scmd_info_tx(copy); + w2_cmd_info_tx(copy); break; } case W2_CMD_DISP | W2_CMDDIR_RX: { - w2_scmd_disp_rx(copy); + w2_cmd_disp_rx(copy); break; } case W2_CMD_PLAY | W2_CMDDIR_RX: { - w2_scmd_play_rx(copy); + w2_cmd_play_rx(copy); break; } case W2_CMD_CLED | W2_CMDDIR_RX: { - w2_scmd_cled_rx(copy); + w2_cmd_cled_rx(copy); break; } default: { diff --git a/shared/protocol.h b/shared/protocol.h index 52ba2bd..adec614 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -46,6 +46,8 @@ enum w2_e_scmds { W2_CMD_CLED = 0x1a, }; +#pragma pack(push, 1) + typedef struct { uint8_t opcode; uint8_t id; @@ -158,53 +160,55 @@ typedef struct { typedef struct { } w2_s_cmd_cled_rx; +#pragma pack(pop) + /** global handler for complete messages */ -void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); +void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** calculate message length */ -uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); +size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** handler for ping_rx (on complete message) */ -void w2_scmd_ping_rx(w2_s_bin *data); +void w2_cmd_ping_rx(w2_s_bin *data); /** handler for ping_tx (on complete message) */ -void w2_scmd_ping_tx(w2_s_bin *data); +void w2_cmd_ping_tx(w2_s_bin *data); /** handler for expt_tx (on complete message) */ -void w2_scmd_expt_tx(w2_s_bin *data); +void w2_cmd_expt_tx(w2_s_bin *data); /** handler for mode_rx (on complete message) */ -void w2_scmd_mode_rx(w2_s_bin *data); +void w2_cmd_mode_rx(w2_s_bin *data); /** handler for mode_tx (on complete message) */ -void w2_scmd_mode_tx(w2_s_bin *data); +void w2_cmd_mode_tx(w2_s_bin *data); /** handler for sped_rx (on complete message) */ -void w2_scmd_sped_rx(w2_s_bin *data); +void w2_cmd_sped_rx(w2_s_bin *data); /** handler for dirc_rx (on complete message) */ -void w2_scmd_dirc_rx(w2_s_bin *data); +void w2_cmd_dirc_rx(w2_s_bin *data); /** handler for cord_rx (on complete message) */ -void w2_scmd_cord_rx(w2_s_bin *data); +void w2_cmd_cord_rx(w2_s_bin *data); /** handler for cord_tx (on complete message) */ -void w2_scmd_cord_tx(w2_s_bin *data); +void w2_cmd_cord_tx(w2_s_bin *data); /** handler for bomd_rx (on complete message) */ -void w2_scmd_bomd_rx(w2_s_bin *data); +void w2_cmd_bomd_rx(w2_s_bin *data); /** handler for bomd_tx (on complete message) */ -void w2_scmd_bomd_tx(w2_s_bin *data); +void w2_cmd_bomd_tx(w2_s_bin *data); /** handler for sres_rx (on complete message) */ -void w2_scmd_sres_rx(w2_s_bin *data); +void w2_cmd_sres_rx(w2_s_bin *data); /** handler for mcfg_rx (on complete message) */ -void w2_scmd_mcfg_rx(w2_s_bin *data); +void w2_cmd_mcfg_rx(w2_s_bin *data); /** handler for sens_rx (on complete message) */ -void w2_scmd_sens_rx(w2_s_bin *data); +void w2_cmd_sens_rx(w2_s_bin *data); /** handler for sens_tx (on complete message) */ -void w2_scmd_sens_tx(w2_s_bin *data); +void w2_cmd_sens_tx(w2_s_bin *data); /** handler for info_rx (on complete message) */ -void w2_scmd_info_rx(w2_s_bin *data); +void w2_cmd_info_rx(w2_s_bin *data); /** handler for info_tx (on complete message) */ -void w2_scmd_info_tx(w2_s_bin *data); +void w2_cmd_info_tx(w2_s_bin *data); /** handler for disp_rx (on complete message) */ -void w2_scmd_disp_rx(w2_s_bin *data); +void w2_cmd_disp_rx(w2_s_bin *data); /** handler for play_rx (on complete message) */ -void w2_scmd_play_rx(w2_s_bin *data); +void w2_cmd_play_rx(w2_s_bin *data); /** handler for cled_rx (on complete message) */ -void w2_scmd_cled_rx(w2_s_bin *data); +void w2_cmd_cled_rx(w2_s_bin *data); /** calculate message length for expt_tx (incomplete message) */ -uint8_t w2_scmd_expt_tx_length(w2_s_bin *data); +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data); /** calculate message length for mcfg_rx (incomplete message) */ -uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data); +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data); diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 0ebe377..3bf9c14 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -27,10 +27,10 @@ void w2_serial_parse(uint8_t byte) { if (!listening) return; current_message[current_message_index++] = byte; - complete_message_length = w2_scmd_length(current_message, current_message_index); + complete_message_length = w2_cmd_sizeof(current_message, current_message_index); if (current_message_index == complete_message_length) { - w2_scmd_handler(current_message, current_message_index); + w2_cmd_handler(current_message, current_message_index); memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); current_message_index = 0; -- cgit v1.2.3 From 54b69efe150e1a102faafb4e214159c92abbb841 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 12:14:04 +0200 Subject: clean up ugly switch case in favor of array lookup --- robot/setup.c | 1 + robot/tests/undefined.bin | 1 + shared/protocol.c | 125 +++++++++++++--------------------------------- shared/protocol.h | 6 +++ shared/serial_parse.c | 1 + 5 files changed, 43 insertions(+), 91 deletions(-) create mode 100644 robot/tests/undefined.bin diff --git a/robot/setup.c b/robot/setup.c index 944ea68..f8468bf 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -25,6 +25,7 @@ void w2_setup_main() { clear(); // start serial i/o + w2_cmd_setup_handlers(); serial_set_baud_rate(W2_SERIAL_BAUD); serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); diff --git a/robot/tests/undefined.bin b/robot/tests/undefined.bin new file mode 100644 index 0000000..d44f518 --- /dev/null +++ b/robot/tests/undefined.bin @@ -0,0 +1 @@ +ÿ€9 4­ \ No newline at end of file diff --git a/shared/protocol.c b/shared/protocol.c index 73063f3..77ec466 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -5,6 +5,30 @@ #include "../robot/orangutan_shim.h" #endif +void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *) = {0}; +void w2_cmd_setup_handlers() { + g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_RX] = w2_cmd_ping_rx; + g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_TX] = w2_cmd_ping_tx; + g_w2_cmd_handlers[W2_CMD_EXPT | W2_CMDDIR_TX] = w2_cmd_expt_tx; + g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_RX] = w2_cmd_mode_rx; + g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_TX] = w2_cmd_mode_tx; + g_w2_cmd_handlers[W2_CMD_SPED | W2_CMDDIR_RX] = w2_cmd_sped_rx; + g_w2_cmd_handlers[W2_CMD_DIRC | W2_CMDDIR_RX] = w2_cmd_dirc_rx; + g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_RX] = w2_cmd_cord_rx; + g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_TX] = w2_cmd_cord_tx; + g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_RX] = w2_cmd_bomd_rx; + g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_TX] = w2_cmd_bomd_tx; + g_w2_cmd_handlers[W2_CMD_SRES | W2_CMDDIR_RX] = w2_cmd_sres_rx; + g_w2_cmd_handlers[W2_CMD_MCFG | W2_CMDDIR_RX] = w2_cmd_mcfg_rx; + g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_RX] = w2_cmd_sens_rx; + g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_TX] = w2_cmd_sens_tx; + g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_RX] = w2_cmd_info_rx; + g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_TX] = w2_cmd_info_tx; + g_w2_cmd_handlers[W2_CMD_DISP | W2_CMDDIR_RX] = w2_cmd_disp_rx; + g_w2_cmd_handlers[W2_CMD_PLAY | W2_CMDDIR_RX] = w2_cmd_play_rx; + g_w2_cmd_handlers[W2_CMD_CLED | W2_CMDDIR_RX] = w2_cmd_cled_rx; +} + size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx); if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx); @@ -54,101 +78,20 @@ size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) { } 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); - bool unknown = false; - - switch (data[0]) { - case W2_CMD_PING | W2_CMDDIR_RX: { - w2_cmd_ping_rx(copy); - break; - } - case W2_CMD_PING | W2_CMDDIR_TX: { - w2_cmd_ping_tx(copy); - break; - } - case W2_CMD_EXPT | W2_CMDDIR_TX: { - w2_cmd_expt_tx(copy); - break; - } - case W2_CMD_MODE | W2_CMDDIR_RX: { - w2_cmd_mode_rx(copy); - break; - } - case W2_CMD_MODE | W2_CMDDIR_TX: { - w2_cmd_mode_tx(copy); - break; - } - case W2_CMD_SPED | W2_CMDDIR_RX: { - w2_cmd_sped_rx(copy); - break; - } - case W2_CMD_DIRC | W2_CMDDIR_RX: { - w2_cmd_dirc_rx(copy); - break; - } - case W2_CMD_CORD | W2_CMDDIR_RX: { - w2_cmd_cord_rx(copy); - break; - } - case W2_CMD_CORD | W2_CMDDIR_TX: { - w2_cmd_cord_tx(copy); - break; - } - case W2_CMD_BOMD | W2_CMDDIR_RX: { - w2_cmd_bomd_rx(copy); - break; - } - case W2_CMD_BOMD | W2_CMDDIR_TX: { - w2_cmd_bomd_tx(copy); - break; - } - case W2_CMD_SRES | W2_CMDDIR_RX: { - w2_cmd_sres_rx(copy); - break; - } - case W2_CMD_MCFG | W2_CMDDIR_RX: { - w2_cmd_mcfg_rx(copy); - break; - } - case W2_CMD_SENS | W2_CMDDIR_RX: { - w2_cmd_sens_rx(copy); - break; - } - case W2_CMD_SENS | W2_CMDDIR_TX: { - w2_cmd_sens_tx(copy); - break; - } - case W2_CMD_INFO | W2_CMDDIR_RX: { - w2_cmd_info_rx(copy); - break; - } - case W2_CMD_INFO | W2_CMDDIR_TX: { - w2_cmd_info_tx(copy); - break; - } - case W2_CMD_DISP | W2_CMDDIR_RX: { - w2_cmd_disp_rx(copy); - break; - } - case W2_CMD_PLAY | W2_CMDDIR_RX: { - w2_cmd_play_rx(copy); - break; - } - case W2_CMD_CLED | W2_CMDDIR_RX: { - w2_cmd_cled_rx(copy); - break; - } - default: { + 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) { #ifdef W2_SIM - simwarn("unknown serial message with code 0x%02x\n", data[0]); + // TODO throw warning + simwarn("unknown serial message with code 0x%02x\n", data[0]); #endif - unknown = true; - } - } - + } else { #ifdef W2_SIM - if (!unknown) w2_sim_print_serial(copy); + w2_sim_print_serial(copy); #endif + handler(copy); + } free(copy); } diff --git a/shared/protocol.h b/shared/protocol.h index adec614..5bdfcbe 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -15,6 +15,7 @@ #define W2_CMD_CODE_MASK (~1) #define W2_CMD_DIRECTION_MASK (1) +#define W2_CMD_COUNT 28 enum w2_e_scmds { /** ping command */ W2_CMD_PING = 0x00, @@ -162,6 +163,11 @@ typedef struct { #pragma pack(pop) +/** stores message handlers in array with opcode as index */ +extern void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *); +/** fills g_w2_cmd_handlers with functions */ +void w2_cmd_setup_handlers(); + /** global handler for complete messages */ void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** calculate message length */ diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 3bf9c14..d755dc8 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -3,6 +3,7 @@ #include "consts.h" #include "serial_parse.h" +// TODO: give this function last time of byte, and measure if >5ms, throw warning void w2_serial_parse(uint8_t byte) { static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; static uint8_t current_message_index = 0; -- 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 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 4487ce5c3083d95fad26ebca790b0f849821d736 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 12:43:11 +0200 Subject: update implementation status in protocol.md --- protocol.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/protocol.md b/protocol.md index 4db4032..915fa65 100644 --- a/protocol.md +++ b/protocol.md @@ -42,7 +42,7 @@ readability. |`0x0e`|[SRES](#sres)|no|`r <-- c`|soft reset |`0x10`|[MCFG](#mcfg)|no|`r <-- c`|map config |`0x12`|[SENS](#sens)|no|`r <-> c`|sensor data -|`0x14`|[INFO](#info)|no|`r <-> c`|info +|`0x14`|[INFO](#info)|yes|`r <-> c`|info |`0x16`|[DISP](#disp)|no|`r <-- c`|display control |`0x18`|[PLAY](#play)|no|`r <-- c`|play midi |`0x1a`|[CLED](#cled)|no|`r <-- c`|control leds -- cgit v1.2.3 From f7387fd6af14a740f474620555de379bc9ba69db Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 13:04:35 +0200 Subject: forward errors to sercomm (implement expt command --- protocol.md | 10 +++++----- robot/errcatch.c | 13 ++++++++++++- robot/sim.c | 8 ++++++-- 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`|ping -|`0x02`|[EXPT](#expt)|no|`r --> c`|exception +|`0x02`|[EXPT](#expt)|yes|`r --> c`|exception |`0x04`|[MODE](#mode)|no|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed |`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control @@ -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", ", %u", length); + if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", ", %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 -- cgit v1.2.3 From d70f4f44f927281d6c9bfff64264bd754d682dc8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:05:53 +0200 Subject: implement mode command --- protocol.md | 13 +++++++++++-- robot/calibration.c | 1 - robot/calibration.h | 9 --------- robot/errcatch.c | 3 +-- robot/grid.c | 1 - robot/grid.h | 8 -------- robot/halt.c | 5 ----- robot/halt.h | 8 -------- robot/maze.c | 1 - robot/maze.h | 8 -------- robot/mode_grid.c | 3 +++ robot/mode_grid.h | 8 ++++++++ robot/mode_halt.c | 3 +++ robot/mode_halt.h | 8 ++++++++ robot/mode_lcal.c | 1 + robot/mode_lcal.h | 9 +++++++++ robot/mode_maze.c | 3 +++ robot/mode_maze.h | 8 ++++++++ robot/modes.c | 20 +++++++++++++++++++- robot/modes.h | 30 ++++++++++++++++++++++++++++++ robot/readme.md | 2 +- robot/sercomm.c | 8 +++++++- robot/setup.c | 1 - robot/tests/mode.bin | 1 + 24 files changed, 113 insertions(+), 49 deletions(-) delete mode 100644 robot/calibration.c delete mode 100644 robot/calibration.h delete mode 100644 robot/grid.c delete mode 100644 robot/grid.h delete mode 100644 robot/halt.c delete mode 100644 robot/halt.h delete mode 100644 robot/maze.c delete mode 100644 robot/maze.h create mode 100644 robot/mode_grid.c create mode 100644 robot/mode_grid.h create mode 100644 robot/mode_halt.c create mode 100644 robot/mode_halt.h create mode 100644 robot/mode_lcal.c create mode 100644 robot/mode_lcal.h create mode 100644 robot/mode_maze.c create mode 100644 robot/mode_maze.h create mode 100644 robot/tests/mode.bin diff --git a/protocol.md b/protocol.md index 7084d14..345fe50 100644 --- a/protocol.md +++ b/protocol.md @@ -34,7 +34,7 @@ readability. |--:|---|:-:|:-:|---| |`0x00`|[PING](#ping)|yes|`r <=> c`|ping |`0x02`|[EXPT](#expt)|yes|`r --> c`|exception -|`0x04`|[MODE](#mode)|no|`r <=> c`|mode +|`0x04`|[MODE](#mode)|yes|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed |`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control |`0x0a`|[CORD](#cord)|no|`r <=> c`|coordinate @@ -96,7 +96,16 @@ message, and can be 0 in case of no message. |`uint8_t`|mode code| when initiated from the client, the **mode** command forces the robot to change -execution mode. the mode codes are undetermined as of now. +execution mode. **mode** can be one of: + +- 0: mode_maze +- 1: mode_grid +- 2: mode_halt +- 3: mode_lcal +- 4: mode_chrg +- 5: mode_dirc +- 6: mode_spin +- 7: mode_scal #### get mode (`r --> c`) (2 bytes) diff --git a/robot/calibration.c b/robot/calibration.c deleted file mode 100644 index 7788532..0000000 --- a/robot/calibration.c +++ /dev/null @@ -1 +0,0 @@ -#include "calibration.h" diff --git a/robot/calibration.h b/robot/calibration.h deleted file mode 100644 index 5c1af38..0000000 --- a/robot/calibration.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -/** - * calibration mode - * - * turns robot on its own axis 360 degress, and aligns the front sensors with - * the line if found, else triggers halt mode (emergency) - */ -void w2_mode_calb(); diff --git a/robot/errcatch.c b/robot/errcatch.c index 99f1063..cf06f5d 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -2,7 +2,6 @@ #include #include "errcatch.h" -#include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -52,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) g_w2_current_mode = &w2_mode_halt; + if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_switch(W2_M_HALT); // TODO: handle more error types switch (error->code) { diff --git a/robot/grid.c b/robot/grid.c deleted file mode 100644 index 0c83272..0000000 --- a/robot/grid.c +++ /dev/null @@ -1 +0,0 @@ -#include "grid.h" diff --git a/robot/grid.h b/robot/grid.h deleted file mode 100644 index fcf9100..0000000 --- a/robot/grid.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * warehouse mode - * - * processes orders from the order buffer - */ -void w2_mode_grid(); diff --git a/robot/halt.c b/robot/halt.c deleted file mode 100644 index 2f159f0..0000000 --- a/robot/halt.c +++ /dev/null @@ -1,5 +0,0 @@ -#include - -#include "halt.h" - -void w2_mode_halt() { return; } diff --git a/robot/halt.h b/robot/halt.h deleted file mode 100644 index d92905e..0000000 --- a/robot/halt.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * halt (emergency) mode - * - * stops all execution until emergency status is manually cleared by the user - */ -void w2_mode_halt(); diff --git a/robot/maze.c b/robot/maze.c deleted file mode 100644 index a27414f..0000000 --- a/robot/maze.c +++ /dev/null @@ -1 +0,0 @@ -#include "maze.h" diff --git a/robot/maze.h b/robot/maze.h deleted file mode 100644 index 9fbeb8c..0000000 --- a/robot/maze.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * maze mode - * - * finds route out of maze - */ -void w2_mode_maze(); diff --git a/robot/mode_grid.c b/robot/mode_grid.c new file mode 100644 index 0000000..8526499 --- /dev/null +++ b/robot/mode_grid.c @@ -0,0 +1,3 @@ +#include "mode_grid.h" + +void w2_mode_grid() {} diff --git a/robot/mode_grid.h b/robot/mode_grid.h new file mode 100644 index 0000000..fcf9100 --- /dev/null +++ b/robot/mode_grid.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * warehouse mode + * + * processes orders from the order buffer + */ +void w2_mode_grid(); diff --git a/robot/mode_halt.c b/robot/mode_halt.c new file mode 100644 index 0000000..88d6183 --- /dev/null +++ b/robot/mode_halt.c @@ -0,0 +1,3 @@ +#include "mode_halt.h" + +void w2_mode_halt() { return; } diff --git a/robot/mode_halt.h b/robot/mode_halt.h new file mode 100644 index 0000000..d92905e --- /dev/null +++ b/robot/mode_halt.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * halt (emergency) mode + * + * stops all execution until emergency status is manually cleared by the user + */ +void w2_mode_halt(); diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c new file mode 100644 index 0000000..6b4e736 --- /dev/null +++ b/robot/mode_lcal.c @@ -0,0 +1 @@ +#include "mode_lcal.h" diff --git a/robot/mode_lcal.h b/robot/mode_lcal.h new file mode 100644 index 0000000..dd373f0 --- /dev/null +++ b/robot/mode_lcal.h @@ -0,0 +1,9 @@ +#pragma once + +/** + * calibration mode + * + * turns robot on its own axis 360 degress, and aligns the front sensors with + * the line if found, else triggers halt mode (emergency) + */ +void w2_mode_lcal(); diff --git a/robot/mode_maze.c b/robot/mode_maze.c new file mode 100644 index 0000000..6ae62be --- /dev/null +++ b/robot/mode_maze.c @@ -0,0 +1,3 @@ +#include "mode_maze.h" + +void w2_mode_maze() {} diff --git a/robot/mode_maze.h b/robot/mode_maze.h new file mode 100644 index 0000000..9fbeb8c --- /dev/null +++ b/robot/mode_maze.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * maze mode + * + * finds route out of maze + */ +void w2_mode_maze(); diff --git a/robot/modes.c b/robot/modes.c index c22875c..1f36703 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,6 +1,24 @@ #include "modes.h" -#include "halt.h" +#include "sercomm.h" void (*g_w2_current_mode)() = &w2_mode_halt; void w2_modes_main() { (*g_w2_current_mode)(); } + +void w2_modes_switch(w2_e_mode new_mode) { + if (new_mode == W2_M_PREV) { + // TODO implement previous mode buffer + } else { + g_w2_current_mode = W2_MODES[new_mode]; + } + + // forward mode change to sercomm + size_t msg_size = sizeof(w2_s_cmd_mode_tx); + w2_s_cmd_mode_tx *msg = malloc(msg_size); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX; + msg->mode = (uint8_t)new_mode; + 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); +} diff --git a/robot/modes.h b/robot/modes.h index dd34690..fe81043 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -1,5 +1,11 @@ #pragma once +#include "mode_grid.h" +#include "mode_halt.h" +#include "mode_lcal.h" +#include "mode_maze.h" + +/** function pointer to current mode */ extern void (*g_w2_current_mode)(); /** @@ -8,3 +14,27 @@ extern void (*g_w2_current_mode)(); * executes mode in g_w2_current_mode */ void w2_modes_main(); + +/** 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; + +/** array that maps w2_e_mode to mode function pointers */ +static const void(*const W2_MODES[]) = { + &w2_mode_maze, + &w2_mode_grid, + &w2_mode_grid, + &w2_mode_halt, +}; + +/** switch the current mode */ +void w2_modes_switch(w2_e_mode new_mode); diff --git a/robot/readme.md b/robot/readme.md index 2de14db..31acafc 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -62,7 +62,7 @@ what they're supposed to do: |maze |`mode_maze `|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module| |warehouse |`mode_grid `|Loek| controls robot during warehouse portion of map; hands off control to maze module| |emergency stop |`mode_halt `|Fiona| stops all execution until emergency mode is reset by software or user| -|calibration |`mode_calb `|Fiona| find line by turning on own axis if lost| +|calibration |`mode_lcal `|Fiona| find line by turning on own axis if lost| ## some standards diff --git a/robot/sercomm.c b/robot/sercomm.c index a3ccdb0..e972f28 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -66,7 +67,12 @@ void w2_cmd_ping_rx(w2_s_bin *data) { free(return_message_bin); } -void w2_cmd_mode_rx(w2_s_bin *data) { return; } +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); +} void w2_cmd_sped_rx(w2_s_bin *data) { return; } diff --git a/robot/setup.c b/robot/setup.c index d075c41..1ecda43 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -2,7 +2,6 @@ #include "../shared/bin.h" #include "../shared/consts.h" -#include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin new file mode 100644 index 0000000..35ebd35 --- /dev/null +++ b/robot/tests/mode.bin @@ -0,0 +1 @@ +ÿ \ No newline at end of file -- cgit v1.2.3 From 2a88cf72a211746f4da582850dc5d65ea7bfd4dc Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:09:41 +0200 Subject: typedef enums --- robot/errcatch.c | 6 +++--- robot/errcatch.h | 6 +++--- shared/errors.h | 6 +++--- shared/protocol.h | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/robot/errcatch.c b/robot/errcatch.c index cf06f5d..e44acfd 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -28,7 +28,7 @@ void w2_errcatch_main() { } } -w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message) { +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); @@ -36,8 +36,8 @@ w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const cha return error; } -void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); } -void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) { +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]); diff --git a/robot/errcatch.h b/robot/errcatch.h index dcb6a97..836da1b 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -19,14 +19,14 @@ void w2_errcatch_main(); void w2_errcatch_handle_error(w2_s_error *error); /** append error to error buffer */ -void w2_errcatch_throw(enum w2_e_errorcodes code); +void w2_errcatch_throw(w2_e_errorcode code); /** append error to error buffer (with debug message) */ -void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *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(enum w2_e_errorcodes code, uint16_t length, const char *message); +w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message); diff --git a/shared/errors.h b/shared/errors.h index 280c5ef..985fcb1 100644 --- a/shared/errors.h +++ b/shared/errors.h @@ -15,7 +15,7 @@ * error codes are between 0-63 because the two most significant bits are * reserved for error type checking */ -enum w2_e_errorcodes { +typedef enum { /** wireless connection lost from either robot or client-side */ W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, /** serial COM-port unavalable. client-side only */ @@ -39,7 +39,7 @@ enum w2_e_errorcodes { W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, /** serial buffer full, gets thrown on next cycle */ W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, -}; +} w2_e_errorcode; /** * error struct @@ -48,7 +48,7 @@ enum w2_e_errorcodes { * `message_length` */ typedef struct { - enum w2_e_errorcodes code; + w2_e_errorcode code; uint8_t message_length; char message[]; } w2_s_error; diff --git a/shared/protocol.h b/shared/protocol.h index 5bdfcbe..875bb16 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -16,7 +16,7 @@ #define W2_CMD_DIRECTION_MASK (1) #define W2_CMD_COUNT 28 -enum w2_e_scmds { +typedef enum { /** ping command */ W2_CMD_PING = 0x00, /** exception command */ @@ -45,7 +45,7 @@ enum w2_e_scmds { W2_CMD_PLAY = 0x18, /** control leds command */ W2_CMD_CLED = 0x1a, -}; +} w2_e_scmds; #pragma pack(push, 1) -- cgit v1.2.3 From fda1e9389c4bcef89a2bcdd2c01f0727e9efdb18 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:45:36 +0200 Subject: add semver parsing, more error codes and verify avr compilation compatibility --- shared/bin.c | 2 +- shared/errors.h | 8 ++++++++ shared/protocol.h | 1 - shared/semver.c | 9 +++++++++ shared/semver.h | 11 +++++++++++ shared/serial_parse.c | 1 + 6 files changed, 30 insertions(+), 2 deletions(-) create mode 100644 shared/semver.c create mode 100644 shared/semver.h diff --git a/shared/bin.c b/shared/bin.c index e9592e6..4b3dcc6 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -1,5 +1,5 @@ -#include #include +#include #include "bin.h" diff --git a/shared/errors.h b/shared/errors.h index 985fcb1..44b29a0 100644 --- a/shared/errors.h +++ b/shared/errors.h @@ -24,6 +24,8 @@ typedef enum { W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, /** obstacle unavoidable, robot stuck */ W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, + /** semver major version doesn't match */ + W2_E_CRIT_VERSION_INCOMPATIBLE = 0x04 | W2_E_TYPE_CRIT, /** battery low, returning to charging station */ W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, @@ -39,6 +41,12 @@ typedef enum { W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, /** serial buffer full, gets thrown on next cycle */ W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, + /** semver minor version doesn't match */ + W2_E_WARN_VERSION_INCOMPATIBLE = 0x07 | W2_E_TYPE_WARN, + /** serial byte took to long to receive */ + 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, } w2_e_errorcode; /** diff --git a/shared/protocol.h b/shared/protocol.h index 875bb16..057f67d 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include diff --git a/shared/semver.c b/shared/semver.c new file mode 100644 index 0000000..07a7057 --- /dev/null +++ b/shared/semver.c @@ -0,0 +1,9 @@ +#include "semver.h" + +#include + +w2_semver_t w2_semver_parse(const char *str, uint8_t length) { + w2_semver_t version; + sscanf(str, "%d.%d.%d", (int *)&version.major, (int *)&version.minor, (int *)&version.patch); + return version; +} diff --git a/shared/semver.h b/shared/semver.h new file mode 100644 index 0000000..a99ae61 --- /dev/null +++ b/shared/semver.h @@ -0,0 +1,11 @@ +#pragma once + +#include + +typedef struct { + uint8_t major; + uint8_t minor; + uint8_t patch; +} w2_semver_t; + +w2_semver_t w2_semver_parse(const char *str, uint8_t length); diff --git a/shared/serial_parse.c b/shared/serial_parse.c index d755dc8..89c5809 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,4 +1,5 @@ #include +#include #include "consts.h" #include "serial_parse.h" -- cgit v1.2.3 From 9c1f3dcc98b639e3bbcb6a8e199d8f1aa4bcc42a Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:55:22 +0200 Subject: add mode shims --- robot/mode_chrg.c | 3 +++ robot/mode_chrg.h | 9 +++++++++ robot/mode_dirc.c | 3 +++ robot/mode_dirc.h | 8 ++++++++ robot/mode_scal.c | 3 +++ robot/mode_scal.h | 8 ++++++++ robot/mode_spin.c | 3 +++ robot/mode_spin.h | 8 ++++++++ robot/modes.h | 10 ++++++---- 9 files changed, 51 insertions(+), 4 deletions(-) create mode 100644 robot/mode_chrg.c create mode 100644 robot/mode_chrg.h create mode 100644 robot/mode_dirc.c create mode 100644 robot/mode_dirc.h create mode 100644 robot/mode_scal.c create mode 100644 robot/mode_scal.h create mode 100644 robot/mode_spin.c create mode 100644 robot/mode_spin.h diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c new file mode 100644 index 0000000..81808d5 --- /dev/null +++ b/robot/mode_chrg.c @@ -0,0 +1,3 @@ +#include "mode_chrg.h" + +void w2_mode_chrg() {} diff --git a/robot/mode_chrg.h b/robot/mode_chrg.h new file mode 100644 index 0000000..d9b5cc0 --- /dev/null +++ b/robot/mode_chrg.h @@ -0,0 +1,9 @@ +#pragma once + +/** + * charge station mode + * + * go to the charging station transition in the grid, and continue until a + * black circle is found + */ +void w2_mode_chrg(); diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c new file mode 100644 index 0000000..6c5d2bb --- /dev/null +++ b/robot/mode_dirc.c @@ -0,0 +1,3 @@ +#include "mode_dirc.h" + +void w2_mode_dirc() {} diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h new file mode 100644 index 0000000..25a664a --- /dev/null +++ b/robot/mode_dirc.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * direct control mode + * + * respond to DIRC commands + */ +void w2_mode_dirc(); diff --git a/robot/mode_scal.c b/robot/mode_scal.c new file mode 100644 index 0000000..e3a9c97 --- /dev/null +++ b/robot/mode_scal.c @@ -0,0 +1,3 @@ +#include "mode_scal.h" + +void w2_mode_scal() {} diff --git a/robot/mode_scal.h b/robot/mode_scal.h new file mode 100644 index 0000000..4f1f04d --- /dev/null +++ b/robot/mode_scal.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * sensor calibration mode + * + * calibrate underside uv sensors + */ +void w2_mode_scal(); diff --git a/robot/mode_spin.c b/robot/mode_spin.c new file mode 100644 index 0000000..9145eb3 --- /dev/null +++ b/robot/mode_spin.c @@ -0,0 +1,3 @@ +#include "mode_spin.h" + +void w2_mode_spin() {} diff --git a/robot/mode_spin.h b/robot/mode_spin.h new file mode 100644 index 0000000..926137e --- /dev/null +++ b/robot/mode_spin.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * wet floor simulation + * + * spin uncontrollably (simulating wet floor??) + */ +void w2_mode_spin(); diff --git a/robot/modes.h b/robot/modes.h index fe81043..a9d96d8 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -1,9 +1,13 @@ #pragma once +#include "mode_chrg.h" +#include "mode_dirc.h" #include "mode_grid.h" #include "mode_halt.h" #include "mode_lcal.h" #include "mode_maze.h" +#include "mode_scal.h" +#include "mode_spin.h" /** function pointer to current mode */ extern void (*g_w2_current_mode)(); @@ -30,10 +34,8 @@ typedef enum { /** array that maps w2_e_mode to mode function pointers */ static const void(*const W2_MODES[]) = { - &w2_mode_maze, - &w2_mode_grid, - &w2_mode_grid, - &w2_mode_halt, + &w2_mode_maze, &w2_mode_grid, &w2_mode_grid, &w2_mode_halt, + &w2_mode_chrg, &w2_mode_dirc, &w2_mode_spin, &w2_mode_scal, }; /** switch the current mode */ -- 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(-) 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 From f073c9d3848dab915bed4844e9d13684aa5e23eb Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 15:34:58 +0200 Subject: implement direct control --- protocol.md | 2 +- robot/mode_dirc.c | 6 +++++- robot/mode_dirc.h | 5 +++++ robot/sercomm.c | 9 ++++++++- robot/sim.c | 3 +++ robot/sim.h | 4 +++- robot/tests/dirc.bin | Bin 0 -> 6 bytes robot/tests/mode.bin | 2 +- 8 files changed, 26 insertions(+), 5 deletions(-) create mode 100644 robot/tests/dirc.bin diff --git a/protocol.md b/protocol.md index 345fe50..c6cb3fb 100644 --- a/protocol.md +++ b/protocol.md @@ -36,7 +36,7 @@ readability. |`0x02`|[EXPT](#expt)|yes|`r --> c`|exception |`0x04`|[MODE](#mode)|yes|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed -|`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control +|`0x08`|[DIRC](#dirc)|yes|`r <-- c`|direct control |`0x0a`|[CORD](#cord)|no|`r <=> c`|coordinate |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|backorder modify |`0x0e`|[SRES](#sres)|no|`r <-- c`|soft reset diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 6c5d2bb..0bbf3cb 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,3 +1,7 @@ #include "mode_dirc.h" +#include "orangutan_shim.h" -void w2_mode_dirc() {} +int16_t g_w2_mode_dirc_motor_l = 0; +int16_t g_w2_mode_dirc_motor_r = 0; + +void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); } diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h index 25a664a..5b9bbf4 100644 --- a/robot/mode_dirc.h +++ b/robot/mode_dirc.h @@ -1,5 +1,10 @@ #pragma once +#include + +extern int16_t g_w2_mode_dirc_motor_l; +extern int16_t g_w2_mode_dirc_motor_r; + /** * direct control mode * diff --git a/robot/sercomm.c b/robot/sercomm.c index c9c6194..2786b85 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "mode_dirc.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -76,7 +77,13 @@ void w2_cmd_mode_rx(w2_s_bin *data) { void w2_cmd_sped_rx(w2_s_bin *data) { return; } -void w2_cmd_dirc_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { + w2_s_cmd_dirc_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + g_w2_mode_dirc_motor_l = w2_bin_ntoh16(message->left); + g_w2_mode_dirc_motor_r = w2_bin_ntoh16(message->right); +} void w2_cmd_cord_rx(w2_s_bin *data) { return; } diff --git a/robot/sim.c b/robot/sim.c index baf8a8a..ddc208a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -123,3 +123,6 @@ void w2_sim_print_serial(w2_s_bin *data) { printf("\n"); } +void set_motors(int left, int right) { + simprintfunc("set_motors", "%i, %i", left, right); +} diff --git a/robot/sim.h b/robot/sim.h index 25cc713..a595042 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -10,7 +10,7 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_PRINTFUNC (1) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) @@ -54,6 +54,8 @@ void serial_set_baud_rate(unsigned int rate); // NOLINT void serial_send(char *message, unsigned int length); // NOLINT void serial_receive_ring(char *buffer, unsigned char size); // NOLINT unsigned char serial_get_received_bytes(); // NOLINT +void set_motors(int left, int right); // NOLINT + void w2_sim_setup(int argc, char **argv); void w2_sim_cycle_begin(); void w2_sim_print_serial(w2_s_bin *data); diff --git a/robot/tests/dirc.bin b/robot/tests/dirc.bin new file mode 100644 index 0000000..1aea35c Binary files /dev/null and b/robot/tests/dirc.bin differ diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin index 35ebd35..9cd9175 100644 --- a/robot/tests/mode.bin +++ b/robot/tests/mode.bin @@ -1 +1 @@ -ÿ \ No newline at end of file +ÿ \ No newline at end of file -- cgit v1.2.3