diff options
| -rw-r--r-- | robot/bin.c | 34 | ||||
| -rw-r--r-- | robot/bin.h | 39 | ||||
| -rw-r--r-- | robot/errcatch.c | 3 | ||||
| -rw-r--r-- | robot/errcatch.h | 4 | ||||
| -rw-r--r-- | robot/hypervisor.c | 4 | ||||
| -rw-r--r-- | robot/makefile | 2 | ||||
| -rw-r--r-- | robot/sercomm.c | 48 | ||||
| -rw-r--r-- | robot/sercomm.h | 23 | ||||
| -rw-r--r-- | robot/setup.c | 27 | ||||
| -rw-r--r-- | 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 <stdlib.h> + #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 <stdint.h> +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 <stdio.h> #include <stdlib.h> #include <string.h> @@ -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 <stdlib.h> +#include <string.h> + +#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 <unistd.h> // 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 |