From ae3970ad5e1b3495726b70d5653c2424fccfba07 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 11 Dec 2022 16:13:03 +0100 Subject: add serial parser from robotrun to project --- shared/protocol.c | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 shared/protocol.c (limited to 'shared/protocol.c') diff --git a/shared/protocol.c b/shared/protocol.c new file mode 100644 index 0000000..474398a --- /dev/null +++ b/shared/protocol.c @@ -0,0 +1,30 @@ +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +size_t cd_cmd_sizeof(uint8_t data[CD_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { + cd_cmd_opcode_t opcode = data[0]; + if (CD_CMD_HANDLERS_SIZE[opcode] > 0) return CD_CMD_HANDLERS_SIZE[opcode]; + + cd_s_bin *copy = cd_bin_s_alloc(data_length, data); + size_t length = (*CD_CMD_HANDLERS_SIZEOF[opcode])(copy); + + free(copy); + + return length; +} + +#define CD_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 cd_cmd_response_sizeof(cd_s_bin* data) { + (void) data; // unused variable TODO: implement this + return 0; +} + +#ifdef __cplusplus +} +#endif -- cgit v1.2.3 From 61dd8a5ee66734dfd53ee9be725c9e71fd8d5414 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 12 Dec 2022 13:57:33 +0100 Subject: implement DYN_MEMBER_SIZEOF macro correctly and implement cd_cmd_response_sizeof --- confui/mainwindow.cpp | 5 ++++- confui/serial.h | 2 +- shared/bin.c | 8 ++++++++ shared/bin.h | 16 ++++++++++++++++ shared/protocol-tests/ping-response.bin | Bin 0 -> 10 bytes shared/protocol.c | 32 +++++++++++++++++++++++++++----- 6 files changed, 56 insertions(+), 7 deletions(-) create mode 100644 shared/protocol-tests/ping-response.bin (limited to 'shared/protocol.c') diff --git a/confui/mainwindow.cpp b/confui/mainwindow.cpp index 1fd34c4..b5bbdf0 100644 --- a/confui/mainwindow.cpp +++ b/confui/mainwindow.cpp @@ -13,7 +13,10 @@ CDMeshConnector *g_cd_mesh_connector = nullptr; CDSerialConnector *g_cd_serial = nullptr; QApplication* g_cd_app = nullptr; -CDMainWindow::~CDMainWindow() { delete this->mesh_connector; } +CDMainWindow::~CDMainWindow() { + delete g_cd_mesh_connector; + delete g_cd_serial; +} CDMainWindow::CDMainWindow(QWidget *parent) : QMainWindow(parent) { g_cd_mesh_connector = new CDMeshConnector(); diff --git a/confui/serial.h b/confui/serial.h index 22f2542..aa0508f 100644 --- a/confui/serial.h +++ b/confui/serial.h @@ -11,7 +11,7 @@ using std::vector; class CDSerialConnector { public: CDSerialConnector(); - ~CDSerialConnector(); + virtual ~CDSerialConnector(); /** @brief get list of available serial ports */ static vector get_ports(); diff --git a/shared/bin.c b/shared/bin.c index fdceb30..875d013 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -65,6 +65,14 @@ uint16_t cd_bin_hton16(uint16_t h16) { uint32_t cd_bin_ntoh32(uint32_t n32) { return cd_bin_hton32(n32); } uint16_t cd_bin_ntoh16(uint16_t n16) { return cd_bin_hton16(n16); } +uint32_t cd_bin_ntohd(uint8_t* n, size_t s) { return cd_bin_htond(n, s); } + +uint32_t cd_bin_htond(uint8_t* h, size_t s) { + if (s == sizeof(uint8_t)) return *h; + else if (s == sizeof(uint16_t)) return cd_bin_hton16(*(uint16_t*) h); + else if (s == sizeof(uint32_t)) return cd_bin_hton32(*(uint32_t*) h); + else return 0; +} cd_s_bin *cd_bin_s_alloc(uint16_t bytes, uint8_t *data) { cd_s_bin *temp = malloc(sizeof(cd_s_bin) + sizeof(uint8_t) * bytes); diff --git a/shared/bin.h b/shared/bin.h index 35d2bc4..0e16ec0 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -10,6 +10,7 @@ */ #include +#include #ifdef __cplusplus extern "C" { @@ -50,6 +51,21 @@ uint32_t cd_bin_ntoh32(uint32_t n32); /** @brief convert 16-bit value from network (big-endian) to host endian */ uint16_t cd_bin_ntoh16(uint16_t n16); +/** + * @brief convert (8*s)-bit value from network (big-endian) to host endian + * (dynamic size) + * + * @param n pointer to number + * @param s size of number in bytes + * + * @return 32-bit integer regardless of `s` + * + * this function is exclusively used by the CD_DYN_MEMBER_SIZEOF macro in + * shared/protocol.c + */ +uint32_t cd_bin_ntohd(uint8_t* n, size_t s); +uint32_t cd_bin_htond(uint8_t* h, size_t s); + /** @brief replace 32-bit value from host endian to network (big-endian) */ void cd_bin_repl_hton32(uint32_t *h32); /** @brief replace 16-bit value from host endian to network (big-endian) */ diff --git a/shared/protocol-tests/ping-response.bin b/shared/protocol-tests/ping-response.bin new file mode 100644 index 0000000..6067349 Binary files /dev/null and b/shared/protocol-tests/ping-response.bin differ diff --git a/shared/protocol.c b/shared/protocol.c index 474398a..1d66f17 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -1,3 +1,5 @@ +#include + #include "protocol.h" #ifdef __cplusplus @@ -16,13 +18,33 @@ size_t cd_cmd_sizeof(uint8_t data[CD_SERIAL_READ_BUFFER_SIZE], uint8_t data_leng return length; } -#define CD_DYN_MEMBER_SIZEOF(struct_t, length_byte, trailing_type) \ - sizeof(struct_t) + \ - (data->bytes > length_byte ? (sizeof(trailing_type) * data->data[length_byte]) : 0) +/** + * @brief macro to calculate size of message based on struct with member to + * indicate length of dynamic (last) field + * + * @param data cd_s_bin pointer to currently received data + * @param struct_t message struct + * @param length_field struct field with dynamic length + * + * @return size_t with calculated size + * + * equivalent c code: + * + * size_t size = sizeof(struct_t); + * size_t dyn_member_offset = offsetof(struct_t, length_field); + * size_t dyn_member_size = sizeof(((struct_t*)0)->length_field); + * if (data->bytes >= (dyn_member_offset + dyn_member_size)) + * size += cd_bin_ntohd(&data->data[dyn_member_offset], dyn_member_size); + * return size; + */ +#define CD_DYN_MEMBER_SIZEOF(data, struct_t, length_field) \ + sizeof(struct_t) + ( \ + (data->bytes >= (offsetof(struct_t, length_field) + sizeof(((struct_t*)0)->length_field))) ? \ + (cd_bin_ntohd(&data->data[offsetof(struct_t, length_field)], sizeof(((struct_t*)0)->length_field))) :\ + 0); size_t cd_cmd_response_sizeof(cd_s_bin* data) { - (void) data; // unused variable TODO: implement this - return 0; + return CD_DYN_MEMBER_SIZEOF(data, cd_s_cmd_response, response_size); } #ifdef __cplusplus -- cgit v1.2.3 From d8a9d9119df43b163ab6b6799b50f1f04811132a Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 12 Dec 2022 16:20:35 +0100 Subject: serial communication *should* work --- confui/confui.pro | 6 ++- confui/serial.cpp | 47 ++++++++++++++------- shared/pclient.c | 122 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ shared/pclient.h | 72 ++++++++++++++++++++++++++++++++ shared/protocol.c | 6 +++ shared/protocol.h | 20 ++++++++- 6 files changed, 255 insertions(+), 18 deletions(-) create mode 100644 shared/pclient.c create mode 100644 shared/pclient.h (limited to 'shared/protocol.c') diff --git a/confui/confui.pro b/confui/confui.pro index 023b31c..f73465b 100644 --- a/confui/confui.pro +++ b/confui/confui.pro @@ -12,7 +12,8 @@ SOURCES += \ serial.cpp \ ../shared/bin.c \ ../shared/protocol.c \ - ../shared/serial_parse.c + ../shared/serial_parse.c \ + ../shared/pclient.c HEADERS += \ mainwindow.h \ @@ -27,7 +28,8 @@ HEADERS += \ ../shared/protocol.h \ ../shared/serial_parse.h \ ../shared/consts.h \ - main.h + main.h \ + ../shared/pclient.h CONFIG += c++17 CONFIG += force_debug_info diff --git a/confui/serial.cpp b/confui/serial.cpp index a20ae1c..c03100b 100644 --- a/confui/serial.cpp +++ b/confui/serial.cpp @@ -1,6 +1,7 @@ #include "serial.h" #include "../shared/serial_parse.h" #include "../shared/bin.h" +#include "../shared/pclient.h" #include #include @@ -34,20 +35,17 @@ void CDSerialConnector::action() { } void CDSerialConnector::write(QByteArray msg) { - _serial->write(msg); - qDebug() << _serial->errorString(); + if (-1 == _serial->write(msg)) + qDebug() << _serial->errorString(); } void CDSerialConnector::connect(string port) { _serial->setPortName(QString::fromStdString(port)); - // QIODevice::ReadOnly - if (!_serial->open(QIODevice::ReadWrite)) qDebug() << _serial->errorString(); + if (!_serial->open(QIODevice::ReadWrite)) + qDebug() << _serial->errorString(); - QObject::connect(_serial, &QSerialPort::readyRead, [&] { - // this is called when readyRead(); - action(); - }); + QObject::connect(_serial, &QSerialPort::readyRead, [&] { action(); }); } void CDSerialConnector::disconnect() { @@ -70,6 +68,17 @@ string CDSerialConnector::get_port() { extern "C" { +void cd_pclient_send(cd_s_bin* data) { + QByteArray converted; + converted.append("\xff", 1); + for (size_t i = 0; i < data->bytes; i++) { + size_t byte = data->data[i]; + byte == 0xff ? converted.append("\xff\xff", 2) + : converted.append((char *) &byte, 1); + } + g_cd_serial->write(converted); +} + // receive handlers (node only) void cd_cmd_get_node(cd_s_bin* data) { (void) data; } void cd_cmd_post_led(cd_s_bin* data) { (void) data; } @@ -81,17 +90,27 @@ void cd_cmd_ping(cd_s_bin* data) { cd_bin_repl_ntoh16(&cast->id); // fix endianness - std::cout << "ping with id " << cast->id << " received!" << std::endl; + std::cout << "ping request with id " << cast->id << " received!" << std::endl; - // TODO: send ping back + cd_s_bin* response = cd_cmd_res_status((cd_e_scmds) cast->opcode, cast->id, false); + cd_pclient_send(response); + free(response); + response = nullptr; } void cd_cmd_response(cd_s_bin* data) { - (void) data; + CD_CAST_BIN(cd_s_cmd_response, data, cast); - std::cout << "received!" << std::endl; - - return; + cd_bin_repl_ntoh16(&cast->id); + cd_bin_repl_ntoh16(&cast->response_id); + + switch (cast->response_type) { + case CD_CMD_PING: { + std::cout << "ping response with id " << cast->response_id << " received!" << std::endl; + break; + } + default: { } + } } } diff --git a/shared/pclient.c b/shared/pclient.c new file mode 100644 index 0000000..41ace27 --- /dev/null +++ b/shared/pclient.c @@ -0,0 +1,122 @@ +#include + +#include "protocol.h" +#include "pclient.h" +#include "bin.h" + +#ifdef __cplusplus +extern "C" { +#endif + +cd_s_bin* cd_cmd_gen_ping() { + CD_CREATE_MSG_BIN(cd_s_cmd_ping, msg, bin); + + msg->opcode = CD_CMD_PING; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + + return bin; +} + +cd_s_bin* cd_cmd_gen_get_node(bool all, cd_uuid_t uuid) { + CD_CREATE_MSG_BIN(cd_s_cmd_get_node, msg, bin); + + msg->opcode = CD_CMD_GET_NODE; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->all = all; + memcpy(&msg->uuid, &uuid, sizeof(cd_uuid_t)); + + return bin; +} + +cd_s_bin* cd_cmd_gen_post_led(bool on, cd_uuid_t uuid) { + CD_CREATE_MSG_BIN(cd_s_cmd_post_led, msg, bin); + + msg->opcode = CD_CMD_POST_LED; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->on = on; + memcpy(&msg->uuid, &uuid, sizeof(cd_uuid_t)); + + return bin; +} + +cd_s_bin* cd_cmd_gen_post_link_add(cd_uuid_t button, cd_uuid_t light, cd_e_cmd_link_type type) { + CD_CREATE_MSG_BIN(cd_s_cmd_post_link, msg, bin); + + msg->opcode = CD_CMD_POST_LINK; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->add = true; + memcpy(&msg->button, &button, sizeof(cd_uuid_t)); + memcpy(&msg->led, &light, sizeof(cd_uuid_t)); + msg->type = type; + + return bin; +} + +cd_s_bin* cd_cmd_gen_post_link_rm(cd_uuid_t button, cd_uuid_t light) { + CD_CREATE_MSG_BIN(cd_s_cmd_post_link, msg, bin); + + msg->opcode = CD_CMD_POST_LINK; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->add = false; + memcpy(&msg->button, &button, sizeof(cd_uuid_t)); + memcpy(&msg->led, &light, sizeof(cd_uuid_t)); + + return bin; +} + +cd_s_bin* cd_cmd_gen_post_net_add(cd_uuid_t uuid) { + CD_CREATE_MSG_BIN(cd_s_cmd_post_net, msg, bin); + + msg->opcode = CD_CMD_POST_NET; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->join = true; + memcpy(&msg->uuid, &uuid, sizeof(cd_uuid_t)); + + return bin; +} + +cd_s_bin* cd_cmd_gen_post_net_rm(cd_uuid_t uuid) { + CD_CREATE_MSG_BIN(cd_s_cmd_post_net, msg, bin); + + msg->opcode = CD_CMD_POST_NET; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->join = false; + memcpy(&msg->uuid, &uuid, sizeof(cd_uuid_t)); + + return bin; +} + +cd_s_bin* cd_cmd_res_status(cd_e_scmds cmd, cd_cmd_id_t id, bool error) { + CD_CREATE_MSG_BIN(cd_s_cmd_response, msg, bin); + + msg->opcode = CD_CMD_RESPONSE; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->response_type = cmd; + msg->response_id = id; + msg->error = error; + msg->response_size = 0; + + return bin; +} + +cd_s_bin* cd_cmd_res_get_node(cd_e_scmds cmd, cd_cmd_id_t id, uint16_t node_count, cd_s_cmd_node* nodes) { + CD_CREATE_MSG_BIN(cd_s_cmd_response, msg, bin); + + msg->opcode = CD_CMD_RESPONSE; + msg->id = cd_bin_hton16(cd_protocol_fresh_message_id()); + msg->response_type = cmd; + msg->response_id = id; + msg->error = false; + + // TODO: test this?? + msg->response_size = sizeof(cd_s_cmd_response_get_node) + sizeof(cd_s_cmd_node) * node_count; + cd_s_cmd_response_get_node* get_node_response_ptr = (cd_s_cmd_response_get_node*) msg->response_info; + memcpy(get_node_response_ptr->nodes, nodes, sizeof(cd_s_cmd_node) * node_count); + + return bin; +} + +#ifdef __cplusplus +} +#endif + diff --git a/shared/pclient.h b/shared/pclient.h new file mode 100644 index 0000000..cf0f182 --- /dev/null +++ b/shared/pclient.h @@ -0,0 +1,72 @@ +#pragma once + +/** @file pclient.h */ + +#include + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** @brief send data over platform standard serial out (doesn't free `data`) */ +void cd_pclient_send(cd_s_bin* data); + +/** @brief generate PING command */ +cd_s_bin* cd_cmd_gen_ping(); +/** + * @brief generate GET_NODE command + * @param all get all nodes + * @param uuid get specific node by uuid + */ +cd_s_bin* cd_cmd_gen_get_node(bool all, cd_uuid_t uuid); +/** + * @brief generate POST_LED command + * @param on light status + * @param uuid node to set light of + */ +cd_s_bin* cd_cmd_gen_post_led(bool on, cd_uuid_t uuid); +/** + * @brief generate POST_LINK command to add or update link + * @param button button node uuid + * @param light light node uuid + * @param type type of link to set + */ +cd_s_bin* cd_cmd_gen_post_link_add(cd_uuid_t button, cd_uuid_t light, cd_e_cmd_link_type type); +/** + * @brief generate POST_LINK command to remove link + * @param button button node uuid + * @param light light node uuid + */ +cd_s_bin* cd_cmd_gen_post_link_rm(cd_uuid_t button, cd_uuid_t light); +/** + * @brief generate POST_NET command to provision node into network + * @param uuid node uuid + */ +cd_s_bin* cd_cmd_gen_post_net_add(cd_uuid_t uuid); +/** + * @brief generate POST_NET command to provision node out of network + * @param uuid node uuid + */ +cd_s_bin* cd_cmd_gen_post_net_rm(cd_uuid_t uuid); + +/** + * @brief generate generic RESPONSE command with error field and no response_info + * @param cmd original command opcode + * @param id original command id + * @param error `true` if some error occurred + */ +cd_s_bin* cd_cmd_res_status(cd_e_scmds cmd, cd_cmd_id_t id, bool error); +/** + * @brief generate RESPONSE command with GET_NODE response_info + * @param cmd original command opcode + * @param id original command id + * @param node_count amount of nodes in `nodes` + * @param nodes pointer to array of `cd_s_cmd_node` + */ +cd_s_bin* cd_cmd_res_get_node(cd_e_scmds cmd, cd_cmd_id_t id, uint16_t node_count, cd_s_cmd_node* nodes); + +#ifdef __cplusplus +} +#endif diff --git a/shared/protocol.c b/shared/protocol.c index 1d66f17..fcc0f41 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -6,6 +6,8 @@ extern "C" { #endif +cd_cmd_id_t g_cd_protocol_fresh_message_id = 0; + size_t cd_cmd_sizeof(uint8_t data[CD_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { cd_cmd_opcode_t opcode = data[0]; if (CD_CMD_HANDLERS_SIZE[opcode] > 0) return CD_CMD_HANDLERS_SIZE[opcode]; @@ -47,6 +49,10 @@ size_t cd_cmd_response_sizeof(cd_s_bin* data) { return CD_DYN_MEMBER_SIZEOF(data, cd_s_cmd_response, response_size); } +cd_cmd_id_t cd_protocol_fresh_message_id() { + return g_cd_protocol_fresh_message_id++; +} + #ifdef __cplusplus } #endif diff --git a/shared/protocol.h b/shared/protocol.h index 7f06a52..63b4a9e 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -29,6 +29,12 @@ typedef uint8_t cd_cmd_bool_t; /** @brief cmd handler function signature */ typedef void (cd_cmd_handler_t)(cd_s_bin *data); +/** @brief used for numbering messages */ +extern cd_cmd_id_t g_cd_protocol_fresh_message_id; + +/** @brief get new message id */ +cd_cmd_id_t cd_protocol_fresh_message_id(); + #pragma pack(push, 1) typedef struct { @@ -50,9 +56,19 @@ typedef struct { cd_uuid_t uuid; /** @brief node uuid to set */ } cd_s_cmd_post_led; +typedef enum { + CD_CMD_LINK_TYPE_TOGGLE = 0x00, + CD_CMD_LINK_TYPE_TURN_ON = 0x01, + CD_CMD_LINK_TYPE_TURN_OFF = 0x02, +} cd_e_cmd_link_type; + typedef struct { cd_cmd_opcode_t opcode; /** @brief cmd opcode */ cd_cmd_id_t id; /** @brief message id */ + cd_uuid_t button; /** @brief uuid of button node */ + cd_uuid_t led; /** @brief uuid of led node */ + cd_cmd_bool_t add; /** @brief `true` to create/overwrite link, `false` to remove link */ + cd_e_cmd_link_type type; /** @brief link type */ } cd_s_cmd_post_link; typedef struct { @@ -80,9 +96,9 @@ typedef struct { typedef struct { cd_cmd_opcode_t opcode; /** @brief cmd opcode */ cd_cmd_id_t id; /** @brief response message id */ - cd_cmd_bool_t success; /** @brief `true` if some error occurred */ - cd_cmd_id_t response_id; /** @brief original message id */ + cd_cmd_bool_t error; /** @brief `true` if some error occurred */ cd_cmd_opcode_t response_type; /** @brief response type, used to cast type of `response_info` */ + cd_cmd_id_t response_id; /** @brief original message id */ uint16_t response_size; /** @brief size of remaining response */ uint8_t response_info[]; /** @brief (CAST) remaining response struct, not read if `response_size`=`0` */ } cd_s_cmd_response; -- cgit v1.2.3