From ab85ee5a8135f49f17967cd3ecc7a20e40af5c0e Mon Sep 17 00:00:00 2001 From: Roozki Date: Mon, 27 Jul 2026 20:12:27 -0700 Subject: [PATCH] Added comms library --- common_libraries/comms/can_interface.h | 29 +++ common_libraries/comms/comms.c | 254 +++++++++++++++++++++ common_libraries/comms/comms.h | 128 +++++++++++ common_libraries/comms/device_ids.h | 21 ++ common_libraries/comms/test/test_comms.cpp | 12 + 5 files changed, 444 insertions(+) create mode 100644 common_libraries/comms/can_interface.h create mode 100644 common_libraries/comms/comms.c create mode 100644 common_libraries/comms/comms.h create mode 100644 common_libraries/comms/device_ids.h create mode 100644 common_libraries/comms/test/test_comms.cpp diff --git a/common_libraries/comms/can_interface.h b/common_libraries/comms/can_interface.h new file mode 100644 index 0000000..d7c7472 --- /dev/null +++ b/common_libraries/comms/can_interface.h @@ -0,0 +1,29 @@ +#pragma once +#include //msg types + + +enum class CanStatus { + OK, + NOT_OK, // GEneric not ok + RX_QUEUE_EMPTY, // Rx queue is emtpy. Nothing new to read + + ERR_TX_QUEUE_FULL, // tx queue is full. + ERR_TX_HW_BUSY, // For interrupt based tx + ERR_BUS_FAILURE // Hardware failure (can peripheral is in error state). This can be caused by wiring issues or misconfigurations +}; + +class CanInterface { +public: + + virtual CanStatus init() = 0; + + // Depending on the implementation, this may: + // send blocking (blocks until message is sent) + // fills up a queue thats handled by another thread + // etc... + virtual CanStatus send(const can_msg_t& msg) = 0; + + // Up to the user if this is a blocking read that touches hardware, or just checks a queue + virtual CanStatus read(can_msg_t& msg) = 0; + +}; \ No newline at end of file diff --git a/common_libraries/comms/comms.c b/common_libraries/comms/comms.c new file mode 100644 index 0000000..f26c74f --- /dev/null +++ b/common_libraries/comms/comms.c @@ -0,0 +1,254 @@ +#include "comms.h" +//TODO tests tests tests +// Message Structure (not can specific, and not super efficient) +/* +Byte 0-1: (2* bytes) Opener (Controller / device ID) (* 13 usable bits + 3 padding bits)) +Byte 2-3: (2 bytes) Message Type ID +Byte 3-10: (8 bytes) Payload (data) + +When transcoding for can, bytes 0 - 3 are contained within the can address +*/ + + +/* +How to add a message: +- add a message type to the header +- add your message to expected_payload_len(), encode() and decode(). +- Then make a pull request as soon as possible, or deal with minor merge conflicts + +Then, to use messages: +First, set your device id: comms_set_device_id(your_id) -- See device_ids.h + +SENDING: + msg_t msg; + msg.type = MSG_TYPE__YOUR_NEW_MESSAGE; + ! Do not touch msg.sender_id, it is filled automatically ! + msg.payload.your_message_type.data1 = ...; then fill in your data + encoded_msg_t encoded_msg; + encode(&msg, &encoded_msg); in -> out (sorry if you are really used to memcpy) + Then, if sending over can: + can_msg_t can_msg; + convert_to_can(&encoded_msg, &can_msg); + Then, use a can transport derrived from can_interface.h + +RECEIVING; + If from can: + can_msg_t can_msg; + Use a can transport derrived from can_interface.h + encoded_msg_t encoded_msg; + convert_from_can(&can_msg, &encoded_msg); + msg_t msg; + decode(&encoded_msg, &msg); + + Then, its up to the user, but a switch case works great: + switch(msg.type) + { + case YOUR_MESSAGE_TYPE: + handle_your_message_type(msg.payload.message_type); + } + + Also, you can use msg.sender_id if you need to know who sent the message. +*/ + +//? Note: There is no built in CRCs or other checksums. This library is meant to be used with +//? transport layers that handle checksums automagically (CAN, TCP) + +// If this was CPP then we'd add a namespace {}. + +#define PACK(buf, pos, x) \ + do { memcpy((buf) + (pos), &(x), sizeof(x)); (pos) += sizeof(x); } while (0) + +#define UNPACK(buf, pos, x) \ + do { memcpy(&(x), (buf) + (pos), sizeof(x)); (pos) += sizeof(x); } while (0) + +static uint16_t s_device_id = COMMS_INVALID_DEVICE_ID; + +void comms_set_device_id(uint16_t device_id) { s_device_id = device_id & COMMS_DEVICE_ID_MASK; } +uint16_t comms_get_device_id(void) { return s_device_id; } + +// ---- id pack/unpack --------------------------------------------------------- +static inline uint32_t pack_id(uint16_t device_id, uint16_t type) +{ + return (((uint32_t)(device_id & COMMS_DEVICE_ID_MASK)) << COMMS_DEVICE_ID_SHIFT) + | ((uint32_t)type & COMMS_MSG_ID_MASK); +} +static inline uint16_t id_device(uint32_t id) +{ + return (uint16_t)((id >> COMMS_DEVICE_ID_SHIFT) & COMMS_DEVICE_ID_MASK); +} +static inline uint16_t id_type(uint32_t id) +{ + return (uint16_t)(id & COMMS_MSG_ID_MASK); +} + +// The raw frame is fixed-width and carries no length field, so the receiver +// must know how many payload bytes a given type occupies. +// Using sizeof() on structs will often return the wrong value (structs are padded.) +static size_t expected_payload_len(uint16_t type) +{ + switch (type) { + case MSG_TYPE__PING: return 2u * sizeof(uint32_t); // 8 + case MSG_TYPE__HEARTBEAT: return 4u; + case MSG_TYPE__SET_PANEL_PWM_CMD: return sizeof(uint8_t) + sizeof(float); // 5 + case MSG_TYPE__HARDWARE_ERROR: return 2u; + default: return SIZE_MAX; + } +} + +// Messages that are not meant to go over can. +// Set explicitly so user doesn't accedentally make a CAN message that is too big +// Could also be used to distinguish against CAN and CANFD messages, but if you are only using +// this comms library on CAN, you can ignore this. +static bool is_large_message(uint16_t type) +{ + // No large messages rn. + switch (type) { + default: return false; + } +} + +// ---- payload (de)serialization (field-by-field, padding-safe) --------------- +// returns bytes written, or SIZE_MAX on error +static size_t pack_payload(const msg_t* msg, uint8_t* out, size_t cap) +{ + size_t pos = 0; + switch (msg->type) { + case MSG_TYPE__PING: + PACK(out, pos, msg->payload.ping.reserved_1); + PACK(out, pos, msg->payload.ping.reserved_2); + break; + case MSG_TYPE__HEARTBEAT: + PACK(out, pos, msg->payload.heartbeat.timestamp_ms); + break; + case MSG_TYPE__SET_LED_PWM: { + float pct = msg->payload.led_panel_cmd.panel_percent; + // if (pct < 0.0f || pct > 100.0f) return SIZE_MAX; // range guard if we want it? + PACK(out, pos, msg->payload.led_panel_cmd.led_panel_index); + PACK(out, pos, msg->payload.led_panel_cmd.panel_percent); + break; + } + case MSG_TYPE__HARDWARE_ERROR: + PACK(in, pos, msg->payload.hardware_error.error_type); + PACK(in, pos, msg->payload.hardware_error.info); + break; + default: + return SIZE_MAX; // This is an error + } + return pos; +} + +static bool unpack_payload(msg_t* msg, const uint8_t* in, size_t len) +{ + size_t pos = 0; + switch (msg->type) { + case MSG_TYPE__PING: + UNPACK(in, pos, msg->payload.ping.reserved_1); + UNPACK(in, pos, msg->payload.ping.reserved_2); + break; + case MSG_TYPE__HEARTBEAT: + UNPACK(in, pos, msg->payload.heartbeat.timestamp_ms); + break; + case MSG_TYPE__SET_LED_PWM: + UNPACK(in, pos, msg->payload.led_panel_cmd.led_panel_index); + UNPACK(in, pos, msg->payload.led_panel_cmd.panel_percent); + break; + case MSG_TYPE__HARDWARE_ERROR: + UNPACK(in, pos, msg->payload.hardware_error.error_type); + UNPACK(in, pos, msg->payload.hardware_error.info); + break; + default: + return false; + } + (void)pos; + return true; +} + +// User functions +bool encode(const msg_t* msg_in, encoded_msg_t* encoded_msg_out) +{ + if (!msg_in || !encoded_msg_out) return false; + if(s_device_id == COMMS_INVALID_DEVICE_ID) return false; + + memset(encoded_msg_out->payload, 0, sizeof encoded_msg_out->payload); + size_t n = pack_payload(msg_in, encoded_msg_out->payload, + sizeof encoded_msg_out->payload); + if (n == SIZE_MAX) return false; + + encoded_msg_out->id = pack_id(s_device_id, msg_in->type); + encoded_msg_out->payload_len = (uint8_t)n; + return true; +} + +bool decode(const encoded_msg_t* encoded_msg_in, msg_t* msg_out) +{ + if (!encoded_msg_in || !msg_out) return false; + if (encoded_msg_in->payload_len > MAX_LARGE_MESSAGE_PAYLOAD_SIZE_BYTES) return false; + + msg_out->type = id_type(encoded_msg_in->id); + msg_out->sender_id = id_device(encoded_msg_in->id); // Preserve device ID, so we know which device sent this message + + return unpack_payload(msg_out, encoded_msg_in->payload, encoded_msg_in->payload_len); +} + +bool convert_to_can(const encoded_msg_t* in, can_msg_t* out) +{ + if (!in || !out) return false; + if (in->payload_len > MAX_MESSAGE_PAYLOAD_SIZE_BYTES) return false; + + out->id = in->id & COMMS_CAN_EXT_ID_MASK; // 29-bit extended id + out->payload_len = in->payload_len; // DLC = actual payload payload_length + memcpy(out->payload, in->payload, in->payload_len); + return true; +} + +bool convert_from_can(const can_msg_t* in, encoded_msg_t* out) +{ + if (!in || !out) return false; + if (in->payload_len > MAX_MESSAGE_PAYLOAD_SIZE_BYTES) return false; + + out->id = in->id & COMMS_CAN_EXT_ID_MASK; + out->payload_len = in->payload_len; + memset(out->payload, 0, sizeof out->payload); + memcpy(out->payload, in->payload, in->payload_len); + return true; +} + +bool convert_to_raw(const encoded_msg_t* in, uint8_t* raw_out) +{ + if (!in || !raw_out) return false; + + size_t pos = 0; + uint16_t dev = id_device(in->id); + uint16_t type = id_type(in->id); + + size_t max_payload_size = is_large_message(type) ? MAX_LARGE_MESSAGE_PAYLOAD_SIZE_BYTES : MAX_MESSAGE_PAYLOAD_SIZE_BYTES; + if (in->payload_len > max_payload_size) return false; + + PACK(raw_out, pos, dev); // bytes 0-1: device id (LE) + PACK(raw_out, pos, type); // bytes 2-3: message id (LE) + // bytes 4-11: payload, zero-padded to the fixed frame width (unless its large message) + memset(raw_out + pos, 0, max_payload_size); + memcpy(raw_out + pos, in->payload, in->payload_len); + return true; +} + +bool convert_from_raw(const uint8_t* raw_in, encoded_msg_t* out) +{ + if (!raw_in || !out) return false; + + size_t pos = 0; + uint16_t dev = 0; + uint16_t type = 0; + + UNPACK(raw_in, pos, dev); + UNPACK(raw_in, pos, type); + + size_t plen = expected_payload_len(type); // raw frame carries no length + if (plen == SIZE_MAX) return false; // unknown type + + out->id = pack_id(dev, type); + out->payload_len = (uint8_t)plen; + memset(out->payload, 0, sizeof out->payload); + memcpy(out->payload, raw_in + pos, plen); + return true; +} \ No newline at end of file diff --git a/common_libraries/comms/comms.h b/common_libraries/comms/comms.h new file mode 100644 index 0000000..8ab1fb2 --- /dev/null +++ b/common_libraries/comms/comms.h @@ -0,0 +1,128 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif +#include "device_ids.h" +#include +#include +#include +#include +#include // static_assert (C11). Compile with -std=c11 or later. +// To be included by each device + +static_assert(sizeof(float) == 4, "Comms requires 32-bit floats"); + +//* Using CAN +#define MAX_PAYLOAD_SIZE_BYTES_CAN 8u +// #define MAX_PAYLOAD_SIZE_BYTES_CANFD 64u + +#define MAX_MESSAGE_PAYLOAD_SIZE_BYTES MAX_PAYLOAD_SIZE_BYTES_CAN +#define MAX_LARGE_MESSAGE_PAYLOAD_SIZE_BYTES 264u //Probably ignore if using regular CAN + +// Don't touch these +#define COMMS_HEADER_SIZE_BYTES 4u +#define COMMS_RAW_MSG_SIZE (COMMS_HEADER_SIZE_BYTES + MAX_MESSAGE_PAYLOAD_SIZE_BYTES) +#define COMMS_DEVICE_ID_MASK 0x1FFFu // Mask to 13 bits +#define COMMS_DEVICE_ID_SHIFT 16u +#define COMMS_MSG_ID_MASK 0xFFFFu // Mask to 16 bits +#define COMMS_CAN_EXT_ID_MASK 0x1FFFFFFFu // Mask to 29 bits +#define COMMS_INVALID_DEVICE_ID 0x1FFFu // Set as default. + + +#define PING_RESERVED_1 0xBEEFu //idk +#define PING_RESERVED_2 0xABBAu //idk + +// If you need an enum inside your message (oppossed to adding lots of message types) +// You can add them here. +enum HardwareErrorType { + HARDWARE_ERROR_TYPE__UNKNOWN, + HARDWARE_ERROR_TYPE__CONFIGURATION_ERROR, // Software can be changed to fix the problem, without needing to reflash firmware + HARDWARE_ERROR_TYPE__INCORRECT_FIRMWARE_VERSION, // Firmware must be re flashed to fix the problem (some board is out of date) + HARDWARE_ERROR_TYPE__PHYSICALLY_FUCKED, // Something physical broke (wires severed, part overheated) Usually hard to detect +}; + + +// Message structures +// Every payload must be no more than max payload size (currently 8 bytes) + +typedef struct { + uint32_t reserved_1; + uint32_t reserved_2; +} Ping_t; + +// Similar to ping +typedef struct { + uint32_t timestamp_ms; +} HeartBeat_t; + +typedef struct { + uint8_t led_panel_index; + float panel_percent; + bool is_response = false; // A response is the LED controller responding with the currently set value +} LedPanelCMD_t; + +typedef struct { + uint8_t error_type; + uint8_t info; +} HardwareError_t; + + +typedef union { + Ping_t ping; + HeartBeat_t heartbeat; + LedPanelCMD_t led_panel_cmd; + HardwareError_t hardware_error; +} Payload_t; + +// C friendly comms helper functions +typedef struct msg_t +{ + uint16_t sender_id; // Only used when decoding messages, ignored when encoding + uint16_t type; + Payload_t payload; +} msg_t; + +// Why are encoded msg and can msg two different structs? -> in case we want to have big encoded messages and send them over multile can messages +// Currently, they are the same. +typedef struct encoded_msg_t +{ + uint32_t id; + uint8_t payload_len; + uint8_t payload[MAX_MESSAGE_PAYLOAD_SIZE_BYTES]; +} encoded_msg_t; + +typedef struct can_msg_t +{ + uint32_t id; // extended IDs + uint8_t payload_len; // length of payload (0 ... MAX_MESSAGE_PAYLOAD_SIZE_BYTES) + uint8_t payload[MAX_MESSAGE_PAYLOAD_SIZE_BYTES]; +} can_msg_t; + +typedef enum { + MSG_TYPE__UNKNOWN, // 0 INDEX IS RESERVED, DO NOT USE + MSG_TYPE__PING, // Just to ping a device. If the device has any lights, maybe this would make them flash + MSG_TYPE__HEARTBEAT, + MSG_TYPE__SET_LED_PWM, + MSG_TYPE__HARDWARE_ERROR, // Not really used rn + NUM_MSG_TYPES +} msg_type_t; + + +void comms_set_device_id(uint16_t device_id); +uint16_t comms_get_device_id(void); + +bool encode(const msg_t* msg_in, encoded_msg_t* encoded_msg_out); +bool decode(const encoded_msg_t* encoded_msg_in, msg_t* msg_out); + +bool convert_to_can (const encoded_msg_t* in, can_msg_t* out); +bool convert_from_can(const can_msg_t* in, encoded_msg_t* out); + +bool convert_to_raw (const encoded_msg_t* in, uint8_t* raw_out); +bool convert_from_raw(const uint8_t* raw_in, encoded_msg_t* out); +bool len_to_dlc(const uint8_t len, uint32_t* dlc_out); +bool dlc_to_len(const uint32_t dlc, uint8_t* len_out); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/common_libraries/comms/device_ids.h b/common_libraries/comms/device_ids.h new file mode 100644 index 0000000..6616b5a --- /dev/null +++ b/common_libraries/comms/device_ids.h @@ -0,0 +1,21 @@ +#pragma once + +// MAX DEVICE ID is 8191 decimal (max of 13 bit) +// 0x1FFF in hex + +// To set your device ID, +// void comms_set_device_id(YOUR_DEVICE_ID) + +#define DEVICE_ID__LED_PANEL_BASE 0x0100 +#define MAX_NUM_LED_PANELS 0xF // 0x0100 -> 0x010F are reserved for LED panels +// then, each individual panel gets an offset. +// OR just define each explicitly: +//#define DEVICE_ID__LED_PANEL_0 0x0100 +//#define DEVICE_ID__LED_PANEL_1 0x0101 +//#define DEVICE_ID__LED_PANEL_2 0x0102 +// ... + +// Will prob redefine these later +#define DEVICE_ID__ON_BOARD_NUC_NUBS 0x0C01 +#define DEVICE_ID__CONTROL_BASE_SUC 0x0C02 +#define DEVICE_ID__ON_BOARD_JETSON_JETSKI 0x0C03 diff --git a/common_libraries/comms/test/test_comms.cpp b/common_libraries/comms/test/test_comms.cpp new file mode 100644 index 0000000..0c0c6f4 --- /dev/null +++ b/common_libraries/comms/test/test_comms.cpp @@ -0,0 +1,12 @@ +#include +#include + +#include + +extern "C" { +#include "comms.h" +} + + + +//TODO \ No newline at end of file