From 1fa3c71567813d8266ff7812f7dafe224aaf6f51 Mon Sep 17 00:00:00 2001 From: Ahmed-Abdo-eng Date: Wed, 14 Oct 2026 18:37:48 +0300 Subject: [PATCH 1/3] Initialized gateway and protocol headers --- main/gp-gateway.h | 136 ++++++++++++++++++++++++++++++++++++++++++ main/gp-protocol.h | 144 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 280 insertions(+) create mode 100644 main/gp-gateway.h create mode 100644 main/gp-protocol.h diff --git a/main/gp-gateway.h b/main/gp-gateway.h new file mode 100644 index 0000000..4a36dc2 --- /dev/null +++ b/main/gp-gateway.h @@ -0,0 +1,136 @@ +#ifndef GATEWAY_H +#define GATEWAY_H + +/* + * gateway - ESP32 UART command-to-CAN forwarding (API revision 10). + * + * Depends on gp_protocol.h. Contains no UART or TWAI driver calls; + * hardware interaction is handled through the maintainer-provided + * nonblocking CanWriteFn callback. + */ + +#include +#include +#include "gp-protocol.h" + + +/* ---------- UART stream parser (ESP32 side) ---------- + * Maintains parser state across calls so a UART frame can be received + * in multiple chunks. Treat the structure fields as private. + */ +typedef struct { + uint8_t buf[GP_UART_FRAME_LEN]; + uint8_t count; +} UartRx; + +/* Reset the parser and discard any partially received frame. */ +void UartRx_Init(UartRx *rx); + +/* + * Feed the parser the next chunk of received UART bytes. + * + * Parameters: + * rx Persistent parser state. + * bytes Input byte buffer; may be NULL only when length is zero. + * length Number of available input bytes. + * consumed Receives the number of bytes consumed by this call. + * The caller advances its buffer offset accordingly and + * processes any remaining bytes in subsequent calls. + * out Receives the decoded command only when GP_OK is returned. + * Otherwise, its contents remain unchanged. + * + * Processing: + * Stops after one complete packet or one malformed packet. + * Handles frames split across chunks and multiple frames in a chunk. + * + * Returns: + * GP_OK A complete, valid command was decoded. + * GP_PENDING Input was consumed, but no complete command is available. + * GP_INVALID A malformed packet was detected; resynchronization to + * the A5 5A start marker is performed. + * GP_BAD_ARG A required argument is NULL. If consumed is usable, + * it is set to zero. + * + * For length == 0, bytes may be NULL and GP_PENDING is returned. + */ +GpResult UartRx_Feed(UartRx *rx, const uint8_t *bytes,size_t length, size_t *consumed, TargetSample *out); + + +/* + * Nonblocking CAN transmission callback provided by the maintainer. + * + * Parameters: + * user Application-provided context passed to Gateway_Init(). + * frame Pointer to the CAN frame to be queued. + * + * Returns: + * GP_OK The entire frame was copied into a bounded, + * maintainer-owned TX queue. + * GP_PENDING The queue is busy; no data was consumed. + * GP_IO_ERROR The queue operation failed. + * + * The callback must copy the frame before returning. It must not retain + * the supplied pointer for later DMA or transmission. + * + * Confirm with Ali whether the frame-based callback shown here is + * preferred over the alternative byte-buffer callback. + * + * GP_OK confirms queue acceptance only; it does not confirm CAN delivery + * or physical motor movement. + */ +typedef GpResult (*CanWriteFn)(void *user, const GpCanFrame *frame); + +typedef struct { + CanWriteFn write; + void *user; + uint32_t max_queue_ms; /* Maximum permitted command queue age. */ + uint16_t last_sequence; /* Last accepted sequence; valid only if + has_baseline is true. */ + bool has_baseline; /* Whether a sequence baseline is set. */ +} Gateway; + +/* + * Initialize the gateway's forwarding state. + * + * Stores the CAN callback, its application context, and the maximum + * permitted queue age. Rejects a NULL gateway or callback, a zero age + * limit, or a limit >= 2^31 ms with GP_BAD_ARG. + * + * Starts without an established sequence baseline. + */ +GpResult Gateway_Init(Gateway *gateway, CanWriteFn write, + void *user, uint32_t max_queue_ms); + +/* + * Validate and forward one decoded command. + * + * Parameters: + * gateway Persistent gateway state. + * command Decoded command to validate and forward. + * queued_ms Elapsed time since the ESP32 byte chunk was enqueued. + * + * Rejects commands that exceed the configured age limit or have + * duplicate/old sequence numbers. A sequence is considered fresh when + * its unsigned 16-bit delta from the last accepted sequence is 1..32767. + * + * The initial sequence baseline is accepted only from a valid neutral + * command with the arm request cleared. + * + * The sequence baseline is updated only when the CAN callback returns + * GP_OK. GP_PENDING leaves the command eligible for a bounded retry. + * This function must never block while waiting for transmission. + * + * A well-formed invalid neutral stop command with a fresh sequence must + * still be forwarded so the receiving controller can stop safely. + */ +GpResult Gateway_Forward(Gateway *gateway, + const TargetSample *command, uint32_t queued_ms); + +/* + * Clear sequence-tracking state while preserving the configured callback, + * callback context, and queue-age limit. The application maintainer is + * responsible for purging its own application queues. + */ +void Gateway_Reset(Gateway *gateway); + +#endif /* GATEWAY_H */ \ No newline at end of file diff --git a/main/gp-protocol.h b/main/gp-protocol.h new file mode 100644 index 0000000..411e49e --- /dev/null +++ b/main/gp-protocol.h @@ -0,0 +1,144 @@ +#ifndef GP_PROTOCOL_H +#define GP_PROTOCOL_H + +/* + * gp_protocol - Portable command encoding and decoding, API revision 10. + * + * Hardware-independent C library. + * Do not include ESP-IDF, STM32 HAL, UART, or TWAI driver headers here + * or in gp_protocol.c. + */ + +#include +#include + +#define GP_PROTOCOL_REV 10 + +/* ---------- Result codes ---------- */ +typedef enum { + GP_OK = 0, /* Operation succeeded or data was accepted. */ + GP_PENDING, /* More input is needed, or the TX queue is busy. */ + GP_INVALID, /* Malformed input or protocol constraint violation. */ + GP_BAD_ARG, /* Required argument is NULL or otherwise invalid. */ + GP_NO_SPACE, /* Output buffer capacity is insufficient. */ + GP_IO_ERROR /* Transmission callback or I/O operation failed. */ +} GpResult; + +/* ---------- UART wire format ---------- + * Frame size: 14 bytes. Multibyte fields are little-endian. + * + * Byte 0..1 Start marker: A5 5A + * Byte 2 Version: 2 + * Byte 3 Message kind: 1 + * Byte 4 Payload length: 7 + * Byte 5..6 Sequence: uint16_t + * Byte 7..8 Throttle: uint16_t, range 0..1000 + * Byte 9..10 Steering: int16_t, range -1000..1000 + * Byte 11 Flags + * Byte 12..13 CRC16-CCITT-FALSE over bytes 2..11. + * + * CRC16: polynomial 0x1021, initial value 0xFFFF, + * no reflection, final XOR 0x0000. + */ +#define GP_UART_FRAME_LEN 14u +#define GP_UART_SOF0 0xA5u +#define GP_UART_SOF1 0x5Au +#define GP_UART_VERSION 2u +#define GP_UART_KIND 1u +#define GP_UART_PAYLOAD_LEN 7u + +/* ---------- CAN wire format ---------- + * Classic CAN, 11-bit standard identifier, 500 kbit/s. + * + * Command ID: 0x105 + * DLC: 8 + * Byte 0..1 Sequence: uint16_t + * Byte 2..3 Throttle: uint16_t, range 0..1000 + * Byte 4..5 Steering: int16_t, range -1000..1000 + * Byte 6 Flags + * Byte 7 CRC8 over bytes 0..6. + * + * CRC8: polynomial 0x07, initial value 0x00, + * no reflection, final XOR 0x00. + */ +#define GP_CAN_ID_COMMAND 0x105u +#define GP_CAN_DLC 8u + +/* ---------- Command limits ---------- */ +#define GP_THROTTLE_MAX 1000u +#define GP_STEERING_MAX 1000 + +/* ---------- Command flags ---------- + * Bit 0: Valid + * Bit 1: Arm request + * Bit 2: Deadman + * Bits 3..7: Reserved; must be zero. + */ +#define GP_FLAG_VALID 0x01u +#define GP_FLAG_ARM 0x02u +#define GP_FLAG_DEADMAN 0x04u + +/* Request bits accepted from the command source: bits 1 and 2. */ +#define GP_FLAGS_REQUEST_MASK (GP_FLAG_ARM | GP_FLAG_DEADMAN) + +/* All defined flag bits: bits 0, 1, and 2. */ +#define GP_FLAGS_ALL_MASK (GP_FLAG_VALID | GP_FLAGS_REQUEST_MASK) + +/* ---------- Decoded command ---------- + * Common representation used independently of UART or CAN encoding. + */ +typedef struct { + uint16_t sequence; /* Sequence number; wraps modulo 65536. */ + uint16_t throttle_pm; /* Forward throttle demand: 0..1000. */ + int16_t steering_pm; /* Signed steering rate demand: -1000..1000. */ + uint8_t flags; /* Command flags defined above. */ +} TargetSample; + +/* + * Invalid-stop rule: + * When the valid flag is clear, throttle and steering must be zero + * and flags must equal zero. + * + * The stop command still carries its sequence number and CRC. + */ + +/* ---------- CAN frame ---------- + * Portable representation of one CAN frame. + */ +typedef struct { + uint32_t id; /* CAN identifier; command frames use 0x105. */ + uint8_t dlc; /* Data length; command frames use 8. */ + uint8_t data[8]; /* CAN payload. */ +} GpCanFrame; + +/* ---------- Portable codec API ---------- + * No hardware access. Inputs are not modified. + * Decoding functions leave the output command unchanged on failure. + */ + +/* + * Encode a command into a 14-byte UART frame. + * written is set to zero on failure. + * Returns GP_NO_SPACE if capacity is less than 14 bytes. + */ +GpResult GP_EncodeUart(const TargetSample *command, + uint8_t *out, size_t capacity, size_t *written); + +/* + * Decode exactly one 14-byte UART frame. + * Validate the start marker, version, kind, payload length, CRC, + * command ranges, and flags. + */ +GpResult GP_DecodeUart(const uint8_t *bytes, + size_t length, TargetSample *out); + +/* Encode one command into a caller-owned CAN frame. */ +GpResult GP_EncodeCan(const TargetSample *command, GpCanFrame *out); + +/* + * Decode a CAN command frame. + * Validate the identifier, DLC, CRC, command ranges, and flags. + */ +GpResult GP_DecodeCan(const GpCanFrame *frame, TargetSample *out); + +#endif /* GP_PROTOCOL_H */ From cb6443f72ede00967c3a75683423b58a2a97a1e1 Mon Sep 17 00:00:00 2001 From: Ahmed-Abdo-eng Date: Wed, 14 Oct 2026 19:08:34 +0300 Subject: [PATCH 2/3] restructered files and implemented protocol src file --- .../gateway/include/gateway.h} | 0 main/lib/gateway/src/gateway.c | 0 .../gp-protocol/include}/gp-protocol.h | 0 main/lib/gp-protocol/src/gp-protocol.c | 257 ++++++++++++++++++ 4 files changed, 257 insertions(+) rename main/{gp-gateway.h => lib/gateway/include/gateway.h} (100%) create mode 100644 main/lib/gateway/src/gateway.c rename main/{ => lib/gp-protocol/include}/gp-protocol.h (100%) create mode 100644 main/lib/gp-protocol/src/gp-protocol.c diff --git a/main/gp-gateway.h b/main/lib/gateway/include/gateway.h similarity index 100% rename from main/gp-gateway.h rename to main/lib/gateway/include/gateway.h diff --git a/main/lib/gateway/src/gateway.c b/main/lib/gateway/src/gateway.c new file mode 100644 index 0000000..e69de29 diff --git a/main/gp-protocol.h b/main/lib/gp-protocol/include/gp-protocol.h similarity index 100% rename from main/gp-protocol.h rename to main/lib/gp-protocol/include/gp-protocol.h diff --git a/main/lib/gp-protocol/src/gp-protocol.c b/main/lib/gp-protocol/src/gp-protocol.c new file mode 100644 index 0000000..763bb2e --- /dev/null +++ b/main/lib/gp-protocol/src/gp-protocol.c @@ -0,0 +1,257 @@ +#include "gp-protocol.h" +#include + +/* + * Portable GP revision 10 packet codec. + * + * This file intentionally has no MCU, UART, CAN/TWAI, or RTOS dependencies. + * All multibyte wire fields are serialized explicitly in little-endian order; + * native C structure layouts are never transmitted directly. + */ + +#define UART_CRC_OFFSET 12u +#define UART_CRC_INPUT_OFFSET 2u +#define UART_CRC_INPUT_LENGTH 10u +#define CAN_CRC_OFFSET 7u +#define CAN_CRC_INPUT_LENGTH 7u + +static uint16_t read_u16_le(const uint8_t *p) +{ + return (uint16_t)((uint16_t)p[0] | ((uint16_t)p[1] << 8)); +} + +static void write_u16_le(uint8_t *p, uint16_t value) +{ + p[0] = (uint8_t)(value & 0xFFu); + p[1] = (uint8_t)((value >> 8) & 0xFFu); +} + +static int16_t read_i16_le(const uint8_t *p) +{ + uint16_t raw = read_u16_le(p); + + /* Convert the two's-complement wire representation without relying on + an out-of-range unsigned-to-signed conversion. */ + if (raw <= (uint16_t)INT16_MAX) { + return (int16_t)raw; + } + return (int16_t)(-1 - (int32_t)(UINT16_MAX - raw)); +} + +static void write_i16_le(uint8_t *p, int16_t value) +{ + write_u16_le(p, (uint16_t)value); +} + +/* CRC-16/CCITT-FALSE: poly 0x1021, init 0xFFFF, no reflection, xor-out 0. */ +static uint16_t crc16_ccitt_false(const uint8_t *data, size_t length) +{ + uint16_t crc = 0xFFFFu; + size_t i; + + for (i = 0; i < length; ++i) { + unsigned bit; + crc ^= (uint16_t)((uint16_t)data[i] << 8); + for (bit = 0; bit < 8u; ++bit) { + if ((crc & 0x8000u) != 0u) { + crc = (uint16_t)((crc << 1) ^ 0x1021u); + } else { + crc = (uint16_t)(crc << 1); + } + } + } + return crc; +} + +/* CRC-8: poly 0x07, init 0, no reflection, xor-out 0. */ +static uint8_t crc8_gp(const uint8_t *data, size_t length) +{ + uint8_t crc = 0u; + size_t i; + + for (i = 0; i < length; ++i) { + unsigned bit; + crc ^= data[i]; + for (bit = 0; bit < 8u; ++bit) { + if ((crc & 0x80u) != 0u) { + crc = (uint8_t)((crc << 1) ^ 0x07u); + } else { + crc = (uint8_t)(crc << 1); + } + } + } + return crc; +} + +/* + * Accept either: + * - a valid command (VALID set; only VALID/ARM/DEADMAN bits may be set), or + * - the mandated invalid-stop packet (flags and both demands are zero). + */ +static int sample_is_valid(const TargetSample *command) +{ + if (command->throttle_pm > GP_THROTTLE_MAX) { + return 0; + } + + if (command->steering_pm < -GP_STEERING_MAX || + command->steering_pm > GP_STEERING_MAX) { + return 0; + } + + if ((command->flags & (uint8_t)~GP_FLAGS_ALL_MASK) != 0u) { + return 0; + } + + if ((command->flags & GP_FLAG_VALID) == 0u) { + return command->flags == 0u && + command->throttle_pm == 0u && + command->steering_pm == 0; + } + + return 1; +} + +GpResult GP_EncodeUart(const TargetSample *command, + uint8_t *out, + size_t capacity, + size_t *written) +{ + uint16_t crc; + + if (written != NULL) { + *written = 0u; + } + + if (command == NULL || out == NULL || written == NULL) { + return GP_BAD_ARG; + } + + if (!sample_is_valid(command)) { + return GP_INVALID; + } + + if (capacity < GP_UART_FRAME_LEN) { + return GP_NO_SPACE; + } + + out[0] = GP_UART_SOF0; + out[1] = GP_UART_SOF1; + out[2] = GP_UART_VERSION; + out[3] = GP_UART_KIND; + out[4] = GP_UART_PAYLOAD_LEN; + + write_u16_le(&out[5], command->sequence); + write_u16_le(&out[7], command->throttle_pm); + write_i16_le(&out[9], command->steering_pm); + out[11] = command->flags; + + crc = crc16_ccitt_false(&out[UART_CRC_INPUT_OFFSET], + UART_CRC_INPUT_LENGTH); + write_u16_le(&out[UART_CRC_OFFSET], crc); + + *written = GP_UART_FRAME_LEN; + return GP_OK; +} + +GpResult GP_DecodeUart(const uint8_t *bytes, + size_t length, + TargetSample *out) +{ + TargetSample decoded; + uint16_t received_crc; + uint16_t calculated_crc; + + if (bytes == NULL || out == NULL) { + return GP_BAD_ARG; + } + + if (length != GP_UART_FRAME_LEN) { + return GP_INVALID; + } + + if (bytes[0] != GP_UART_SOF0 || + bytes[1] != GP_UART_SOF1 || + bytes[2] != GP_UART_VERSION || + bytes[3] != GP_UART_KIND || + bytes[4] != GP_UART_PAYLOAD_LEN) { + return GP_INVALID; + } + + received_crc = read_u16_le(&bytes[UART_CRC_OFFSET]); + calculated_crc = crc16_ccitt_false(&bytes[UART_CRC_INPUT_OFFSET], + UART_CRC_INPUT_LENGTH); + if (received_crc != calculated_crc) { + return GP_INVALID; + } + + decoded.sequence = read_u16_le(&bytes[5]); + decoded.throttle_pm = read_u16_le(&bytes[7]); + decoded.steering_pm = read_i16_le(&bytes[9]); + decoded.flags = bytes[11]; + + if (!sample_is_valid(&decoded)) { + return GP_INVALID; + } + + *out = decoded; + return GP_OK; +} + +GpResult GP_EncodeCan(const TargetSample *command, GpCanFrame *out) +{ + GpCanFrame encoded; + + if (command == NULL || out == NULL) { + return GP_BAD_ARG; + } + + if (!sample_is_valid(command)) { + return GP_INVALID; + } + + encoded.id = GP_CAN_ID_COMMAND; + encoded.dlc = GP_CAN_DLC; + write_u16_le(&encoded.data[0], command->sequence); + write_u16_le(&encoded.data[2], command->throttle_pm); + write_i16_le(&encoded.data[4], command->steering_pm); + encoded.data[6] = command->flags; + encoded.data[CAN_CRC_OFFSET] = crc8_gp(encoded.data, CAN_CRC_INPUT_LENGTH); + + *out = encoded; + return GP_OK; +} + +GpResult GP_DecodeCan(const GpCanFrame *frame, TargetSample *out) +{ + TargetSample decoded; + + if (frame == NULL || out == NULL) { + return GP_BAD_ARG; + } + + if (frame->id != GP_CAN_ID_COMMAND || frame->dlc != GP_CAN_DLC) { + return GP_INVALID; + } + + if (frame->id > 0x7FFu) { + return GP_INVALID; + } + + if (frame->data[CAN_CRC_OFFSET] != + crc8_gp(frame->data, CAN_CRC_INPUT_LENGTH)) { + return GP_INVALID; + } + + decoded.sequence = read_u16_le(&frame->data[0]); + decoded.throttle_pm = read_u16_le(&frame->data[2]); + decoded.steering_pm = read_i16_le(&frame->data[4]); + decoded.flags = frame->data[6]; + + if (!sample_is_valid(&decoded)) { + return GP_INVALID; + } + + *out = decoded; + return GP_OK; +} From 478b23e600b2e841d68802c16f85b746d16f0016 Mon Sep 17 00:00:00 2001 From: Ahmed-Abdo-eng Date: Fri, 16 Oct 2026 05:01:15 +0300 Subject: [PATCH 3/3] finished gateway driver and added Uart and CAN configs to main --- .vscode/settings.json | 3 +- main/CMakeLists.txt | 19 ++- main/lib/gateway/include/gateway.h | 139 ++++------------ main/lib/gateway/src/gateway.c | 251 +++++++++++++++++++++++++++++ main/main.c | 90 ++++++++++- 5 files changed, 389 insertions(+), 113 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 7601759..0dbe14c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -9,5 +9,6 @@ "C_Cpp.default.compileCommands": "${workspaceFolder}/build/compile_commands.json", "files.associations": { "sdkconfig*": "properties" - } + }, + "idf.currentSetup": "C:\\esp\\v6.1\\esp-idf" } diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 01e432d..539440a 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1 +1,18 @@ -idf_component_register(SRCS "main.c" INCLUDE_DIRS "." REQUIRES freertos log) +idf_component_register( + SRCS + "main.c" + "lib/gateway/src/gateway.c" + "lib/gp-protocol/src/gp-protocol.c" + + INCLUDE_DIRS + "." + "lib/gateway/include" + "lib/gp-protocol/include" + + REQUIRES + freertos + log + esp_driver_gpio + esp_driver_uart + esp_driver_twai +) \ No newline at end of file diff --git a/main/lib/gateway/include/gateway.h b/main/lib/gateway/include/gateway.h index 4a36dc2..d460e6c 100644 --- a/main/lib/gateway/include/gateway.h +++ b/main/lib/gateway/include/gateway.h @@ -2,135 +2,56 @@ #define GATEWAY_H /* - * gateway - ESP32 UART command-to-CAN forwarding (API revision 10). + * ESP32-side UART stream parser and command-to-CAN forwarding logic. * - * Depends on gp_protocol.h. Contains no UART or TWAI driver calls; - * hardware interaction is handled through the maintainer-provided - * nonblocking CanWriteFn callback. + * This component does not create RTOS tasks or configure UART/TWAI hardware. + * The application/maintainer owns peripheral setup, UART reads, CAN TX queues, + * and scheduling. */ +#include #include -#include #include "gp-protocol.h" - -/* ---------- UART stream parser (ESP32 side) ---------- - * Maintains parser state across calls so a UART frame can be received - * in multiple chunks. Treat the structure fields as private. - */ typedef struct { - uint8_t buf[GP_UART_FRAME_LEN]; - uint8_t count; + uint8_t frame[GP_UART_FRAME_LEN]; + size_t used; } UartRx; -/* Reset the parser and discard any partially received frame. */ -void UartRx_Init(UartRx *rx); - /* - * Feed the parser the next chunk of received UART bytes. + * The callback must be nonblocking. It must copy the entire frame into a + * bounded, maintainer-owned TX queue before returning GP_OK. * - * Parameters: - * rx Persistent parser state. - * bytes Input byte buffer; may be NULL only when length is zero. - * length Number of available input bytes. - * consumed Receives the number of bytes consumed by this call. - * The caller advances its buffer offset accordingly and - * processes any remaining bytes in subsequent calls. - * out Receives the decoded command only when GP_OK is returned. - * Otherwise, its contents remain unchanged. - * - * Processing: - * Stops after one complete packet or one malformed packet. - * Handles frames split across chunks and multiple frames in a chunk. - * - * Returns: - * GP_OK A complete, valid command was decoded. - * GP_PENDING Input was consumed, but no complete command is available. - * GP_INVALID A malformed packet was detected; resynchronization to - * the A5 5A start marker is performed. - * GP_BAD_ARG A required argument is NULL. If consumed is usable, - * it is set to zero. - * - * For length == 0, bytes may be NULL and GP_PENDING is returned. - */ -GpResult UartRx_Feed(UartRx *rx, const uint8_t *bytes,size_t length, size_t *consumed, TargetSample *out); - - -/* - * Nonblocking CAN transmission callback provided by the maintainer. - * - * Parameters: - * user Application-provided context passed to Gateway_Init(). - * frame Pointer to the CAN frame to be queued. - * - * Returns: - * GP_OK The entire frame was copied into a bounded, - * maintainer-owned TX queue. - * GP_PENDING The queue is busy; no data was consumed. - * GP_IO_ERROR The queue operation failed. - * - * The callback must copy the frame before returning. It must not retain - * the supplied pointer for later DMA or transmission. - * - * Confirm with Ali whether the frame-based callback shown here is - * preferred over the alternative byte-buffer callback. - * - * GP_OK confirms queue acceptance only; it does not confirm CAN delivery - * or physical motor movement. + * GP_PENDING means busy and nothing was accepted. + * GP_IO_ERROR means a permanent/driver error. */ typedef GpResult (*CanWriteFn)(void *user, const GpCanFrame *frame); typedef struct { CanWriteFn write; - void *user; - uint32_t max_queue_ms; /* Maximum permitted command queue age. */ - uint16_t last_sequence; /* Last accepted sequence; valid only if - has_baseline is true. */ - bool has_baseline; /* Whether a sequence baseline is set. */ + void *user; + uint32_t max_queue_ms; + uint16_t last_sequence; + uint8_t have_sequence; } Gateway; -/* - * Initialize the gateway's forwarding state. - * - * Stores the CAN callback, its application context, and the maximum - * permitted queue age. Rejects a NULL gateway or callback, a zero age - * limit, or a limit >= 2^31 ms with GP_BAD_ARG. - * - * Starts without an established sequence baseline. - */ -GpResult Gateway_Init(Gateway *gateway, CanWriteFn write, - void *user, uint32_t max_queue_ms); +void UartRx_Init(UartRx *rx); + +GpResult UartRx_Feed(UartRx *rx, + const uint8_t *bytes, + size_t length, + size_t *consumed, + TargetSample *out); + +GpResult Gateway_Init(Gateway *gateway, + CanWriteFn write, + void *user, + uint32_t max_queue_ms); -/* - * Validate and forward one decoded command. - * - * Parameters: - * gateway Persistent gateway state. - * command Decoded command to validate and forward. - * queued_ms Elapsed time since the ESP32 byte chunk was enqueued. - * - * Rejects commands that exceed the configured age limit or have - * duplicate/old sequence numbers. A sequence is considered fresh when - * its unsigned 16-bit delta from the last accepted sequence is 1..32767. - * - * The initial sequence baseline is accepted only from a valid neutral - * command with the arm request cleared. - * - * The sequence baseline is updated only when the CAN callback returns - * GP_OK. GP_PENDING leaves the command eligible for a bounded retry. - * This function must never block while waiting for transmission. - * - * A well-formed invalid neutral stop command with a fresh sequence must - * still be forwarded so the receiving controller can stop safely. - */ GpResult Gateway_Forward(Gateway *gateway, - const TargetSample *command, uint32_t queued_ms); + const TargetSample *command, + uint32_t queued_ms); -/* - * Clear sequence-tracking state while preserving the configured callback, - * callback context, and queue-age limit. The application maintainer is - * responsible for purging its own application queues. - */ void Gateway_Reset(Gateway *gateway); -#endif /* GATEWAY_H */ \ No newline at end of file +#endif \ No newline at end of file diff --git a/main/lib/gateway/src/gateway.c b/main/lib/gateway/src/gateway.c index e69de29..8eef792 100644 --- a/main/lib/gateway/src/gateway.c +++ b/main/lib/gateway/src/gateway.c @@ -0,0 +1,251 @@ +#include "gateway.h" + +#include + +static void parser_resync(UartRx *rx) +{ + /* + * Preserve a trailing A5 because it may be the first byte + * of the next start-of-frame marker. + */ + if (rx->used > 0u && + rx->frame[rx->used - 1u] == GP_UART_SOF0) { + rx->frame[0] = GP_UART_SOF0; + rx->used = 1u; + } else { + rx->used = 0u; + } +} + +void UartRx_Init(UartRx *rx) +{ + if (rx != NULL) { + memset(rx, 0, sizeof(*rx)); + } +} + +GpResult UartRx_Feed(UartRx *rx, + const uint8_t *bytes, + size_t length, + size_t *consumed, + TargetSample *out) +{ + size_t i = 0u; + + if (consumed == NULL) { + return GP_BAD_ARG; + } + + *consumed = 0u; + + if (rx == NULL || + out == NULL || + (bytes == NULL && length != 0u)) { + return GP_BAD_ARG; + } + + if (length == 0u) { + return GP_PENDING; + } + + while (i < length) { + uint8_t byte = bytes[i++]; + + /* + * Search for the first SOF byte. + */ + if (rx->used == 0u) { + if (byte == GP_UART_SOF0) { + rx->frame[0] = byte; + rx->used = 1u; + } + + continue; + } + + /* + * The second SOF byte must follow. + */ + if (rx->used == 1u) { + if (byte == GP_UART_SOF1) { + rx->frame[1] = byte; + rx->used = 2u; + } else if (byte == GP_UART_SOF0) { + rx->frame[0] = byte; + rx->used = 1u; + } else { + rx->used = 0u; + } + + continue; + } + + rx->frame[rx->used++] = byte; + + /* + * Once the five-byte header is available, + * reject unsupported versions, kinds, or lengths. + */ + if (rx->used == 5u && + (rx->frame[2] != GP_UART_VERSION || + rx->frame[3] != GP_UART_KIND || + rx->frame[4] != GP_UART_PAYLOAD_LEN)) { + + parser_resync(rx); + *consumed = i; + + return GP_INVALID; + } + + /* + * Decode one complete UART frame. + */ + if (rx->used == GP_UART_FRAME_LEN) { + TargetSample candidate; + + GpResult result = GP_DecodeUart( + rx->frame, + GP_UART_FRAME_LEN, + &candidate + ); + + *consumed = i; + + if (result == GP_OK) { + rx->used = 0u; + *out = candidate; + + return GP_OK; + } + + parser_resync(rx); + + return result; + } + } + + *consumed = i; + + return GP_PENDING; +} + +static int is_valid_neutral_disarmed( + const TargetSample *command) +{ + return ((command->flags & GP_FLAG_VALID) != 0u) && + (command->throttle_pm == 0u) && + (command->steering_pm == 0) && + ((command->flags & GP_FLAG_ARM) == 0u); +} + +static int sequence_is_fresh( + uint16_t candidate, + uint16_t previous) +{ + /* + * Unsigned subtraction naturally handles uint16_t wraparound. + * A delta of 1..32767 represents a fresh sequence. + */ + uint16_t delta = (uint16_t)(candidate - previous); + + return (delta >= 1u) && (delta <= 32767u); +} + +GpResult Gateway_Init(Gateway *gateway, + CanWriteFn write, + void *user, + uint32_t max_queue_ms) +{ + if (gateway == NULL || write == NULL) { + return GP_BAD_ARG; + } + + if (max_queue_ms == 0u || + max_queue_ms >= 0x80000000u) { + return GP_INVALID; + } + + gateway->write = write; + gateway->user = user; + gateway->max_queue_ms = max_queue_ms; + gateway->last_sequence = 0u; + gateway->have_sequence = 0u; + + return GP_OK; +} + +GpResult Gateway_Forward(Gateway *gateway, + const TargetSample *command, + uint32_t queued_ms) +{ + GpCanFrame frame; + GpResult result; + + if (gateway == NULL || + command == NULL || + gateway->write == NULL) { + return GP_BAD_ARG; + } + + /* + * Reject commands that have exceeded the permitted queue age. + */ + if (queued_ms > gateway->max_queue_ms) { + return GP_INVALID; + } + + /* + * Validate and encode the command. + * Invalid-stop commands are permitted by the protocol + * when their throttle, steering, and flags are all zero. + */ + result = GP_EncodeCan(command, &frame); + + if (result != GP_OK) { + return result; + } + + /* + * Establish the initial sequence baseline only from + * a valid neutral command with ARM clear. + */ + if (!gateway->have_sequence) { + if (!is_valid_neutral_disarmed(command)) { + return GP_INVALID; + } + } else { + /* + * Reject duplicate, old, and out-of-order commands. + */ + if (!sequence_is_fresh( + command->sequence, + gateway->last_sequence)) { + return GP_INVALID; + } + } + + /* + * The callback must copy the complete frame into its + * bounded TX queue before returning GP_OK. + */ + result = gateway->write(gateway->user, &frame); + + /* + * Update sequence tracking only after queue acceptance. + * GP_PENDING leaves this command eligible for retry. + */ + if (result == GP_OK) { + gateway->last_sequence = command->sequence; + gateway->have_sequence = 1u; + } + + return result; +} + +void Gateway_Reset(Gateway *gateway) +{ + if (gateway != NULL) { + gateway->last_sequence = 0u; + gateway->have_sequence = 0u; + } +} \ No newline at end of file diff --git a/main/main.c b/main/main.c index b21368b..4807003 100644 --- a/main/main.c +++ b/main/main.c @@ -1,9 +1,95 @@ -#include +#include "gateway.h" -#include "esp_log.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" +#include "driver/gpio.h" +#include "driver/uart.h" +#include "esp_twai.h" +#include "esp_twai_onchip.h" +#include "esp_err.h" + +#include + +/* UART configuration */ +#define UART_PORT UART_NUM_1 +#define UART_TX_PIN GPIO_NUM_17 +#define UART_RX_PIN GPIO_NUM_16 +#define UART_BAUD_RATE 115200 + +/* TWAI / CAN configuration */ +#define CAN_TX_PIN GPIO_NUM_21 +#define CAN_RX_PIN GPIO_NUM_22 +#define CAN_BITRATE 500000 + +#define MAX_QUEUE_MS 100 + +/// Global instances of Gateway and UartRx +static Gateway gateway; +static UartRx uart_rx; + +// Initialize UART with the specified configuration +static esp_err_t UART_Init(void) +{ + const uart_config_t uart_config = { + .baud_rate = UART_BAUD_RATE, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .source_clk = UART_SCLK_DEFAULT + }; + + esp_err_t err = uart_driver_install( + UART_PORT, + 1024, + 0, + 0, + NULL, + 0 + ); + + if (err != ESP_OK) { + return err; + } + + err = uart_param_config(UART_PORT, &uart_config); + + if (err != ESP_OK) { + return err; + } + + return uart_set_pin( + UART_PORT, + UART_TX_PIN, + UART_RX_PIN, + UART_PIN_NO_CHANGE, + UART_PIN_NO_CHANGE + ); +} + +// Initialize TWAI (CAN) with the specified configuration +static twai_node_handle_t can_node = NULL; + +static esp_err_t CAN_Init(void) +{ + twai_onchip_node_config_t cfg = { + .io_cfg = { + .tx = CAN_TX_PIN, + .rx = CAN_RX_PIN, + .quanta_clk_out = -1, + .bus_off_indicator = -1, + }, + .bit_timing.bitrate = CAN_BITRATE, // your 500000 define + .tx_queue_depth = 10, + }; + + esp_err_t err = twai_new_node_onchip(&cfg, &can_node); + if (err != ESP_OK) return err; + + return twai_node_enable(can_node); +} + static const char *TAG = "sensing_node"; void app_main(void)