diff --git a/.gitignore b/.gitignore index 9fd908d..33e3fe7 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,9 @@ !.vscode/settings.json __pycache__/ *.pyc + +# IDE / Temporary files +.metadata/ +.vscode/ +build/ +sdkconfig 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/CMakeLists.txt b/CMakeLists.txt index 1ffb318..52549ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,5 +4,8 @@ if(NOT DEFINED ENV{IDF_PATH}) message(FATAL_ERROR "ESP-IDF is not active. Open an ESP-IDF terminal before building.") endif() +# <<< أضف هذا السطر هنا ليقرأ المكونات من مجلد lib >>> +set(EXTRA_COMPONENT_DIRS "lib") + include($ENV{IDF_PATH}/tools/cmake/project.cmake) -project(esp32_sensing_node) +project(esp32_sensing_node) \ No newline at end of file diff --git a/Contracts/README.md b/Contracts/README.md new file mode 100644 index 0000000..80c27cc --- /dev/null +++ b/Contracts/README.md @@ -0,0 +1,11 @@ +# GP API revision 10 + +This header is the single declaration reference for the assignments. It contains +types and function prototypes, not production implementations. Ahmed provides +the portable codec definitions; Rodaina uses that exact version for Blackpill TX. +Ali and Farah publish the shared header on Testing before developer branches. + +Library ownership: joystick = Boody; uart_sender = Rodaina; gp_protocol and +gateway = Ahmed; command_map = Rabe3; motor_driver = Boda. +Application initialization, hardware callbacks, RTOS tasks and function calls +belong to the maintainers. Do not hard-code board pins or HAL handles in libraries. diff --git a/Contracts/gp_types.h b/Contracts/gp_types.h new file mode 100644 index 0000000..84481cb --- /dev/null +++ b/Contracts/gp_types.h @@ -0,0 +1,100 @@ +#ifndef CUERT_GP_TYPES_H +#define CUERT_GP_TYPES_H +#include +#include +#include +#define GP_API_REVISION 10u +#define GP_UART_VERSION 2u +#define GP_UART_FRAME_BYTES 14u +#define GP_CAN_COMMAND_ID 0x105u +#define GP_FLAG_VALID 0x01u +#define GP_FLAG_ARM 0x02u +#define GP_FLAG_DEADMAN 0x04u +typedef enum { GP_OK=0, GP_PENDING, GP_BAD_ARG, + GP_INVALID, GP_STALE, GP_IO_ERROR, GP_NO_SPACE } GpResult; +typedef struct { + uint16_t sequence, throttle_pm; + int16_t steering_pm; + uint8_t flags; +} TargetSample; +typedef struct { + uint16_t min[2], center[2], max[2], deadband[2]; +} JoystickCalibration; +typedef bool (*AdcReadFn)(void *user, uint8_t axis, + uint16_t *raw); +typedef GpResult (*UartWriteFn)(void *user, + const uint8_t *bytes, size_t length); +typedef struct { UartWriteFn write; void *user; bool initialized; +} UartSender; +typedef struct { uint8_t bytes[GP_UART_FRAME_BYTES]; size_t used; +} UartRx; +typedef struct { uint16_t id; uint8_t dlc; uint8_t data[8]; +} GpCanFrame; +typedef GpResult (*CanWriteFn)(void *user, + const GpCanFrame *frame); +typedef struct { CanWriteFn write; void *user; + uint32_t max_queue_ms; uint16_t last_sequence; + bool seen, initialized; +} Gateway; +typedef struct { int16_t throttle_duty_pm, steering_rate_pm; +} MotorOutput; +typedef struct { + uint16_t throttle_cap_pm, steering_cap_pm; + uint16_t throttle_slew_pm_s, steering_slew_pm_s; + uint32_t max_dt_ms; +} CommandMapConfig; +typedef struct { CommandMapConfig config; MotorOutput previous; + uint32_t throttle_remainder, steering_remainder; + bool initialized; +} CommandMap; +typedef struct { bool estop_ok, enabled; } MotorSafety; +typedef GpResult (*MotorWriteFn)(void *user, + const MotorOutput *output); +typedef void (*MotorOffFn)(void *user); +typedef struct { + MotorWriteFn write; MotorOffFn off; void *user; + uint16_t max_duty_pm; uint32_t max_step_hz; + uint16_t step_pulse_us, dir_setup_us, enable_setup_us; + uint32_t reversal_deadtime_ms; +} MotorDriverConfig; +typedef struct { MotorDriverConfig config; MotorOutput previous; + uint32_t zero_since_ms; int8_t last_steering_direction; + bool initialized; +} MotorDriver; + +GpResult Joystick_ReadSample(AdcReadFn read, void *user, + const JoystickCalibration *cal, uint16_t sequence, + uint8_t request_flags, TargetSample *out); +GpResult UartSender_Init(UartSender *sender, + UartWriteFn write, void *user); +GpResult UartSender_Send(UartSender *sender, + const TargetSample *command); +void UartRx_Init(UartRx *rx); +GpResult UartRx_Feed(UartRx *rx, const uint8_t *bytes, + size_t length, size_t *consumed, TargetSample *out); +GpResult GP_EncodeUart(const TargetSample *command, + uint8_t *out, size_t capacity, size_t *written); +GpResult GP_DecodeUart(const uint8_t *bytes, + size_t length, TargetSample *out); +GpResult GP_EncodeCan(const TargetSample *command, + GpCanFrame *out); +GpResult GP_DecodeCan(const GpCanFrame *frame, + TargetSample *out); +GpResult Gateway_Init(Gateway *gateway, CanWriteFn write, + void *user, uint32_t max_queue_ms); +GpResult Gateway_Forward(Gateway *gateway, + const TargetSample *command, uint32_t queued_ms); +void Gateway_Reset(Gateway *gateway); +GpResult CommandMap_Init(CommandMap *map, + const CommandMapConfig *config); +GpResult CommandMap_Update(CommandMap *map, + const TargetSample *command, uint32_t dt_ms, + bool enabled, MotorOutput *out); +void CommandMap_Reset(CommandMap *map); +GpResult MotorDriver_Init(MotorDriver *driver, + const MotorDriverConfig *config); +GpResult MotorDriver_Apply(MotorDriver *driver, + const MotorOutput *request, const MotorSafety *safety, + uint32_t now_ms, MotorOutput *applied); +void MotorDriver_Stop(MotorDriver *driver); +#endif diff --git a/Contracts/verification.h b/Contracts/verification.h new file mode 100644 index 0000000..fb0d9c4 --- /dev/null +++ b/Contracts/verification.h @@ -0,0 +1,18 @@ +#ifndef CUERT_VERIFICATION_H +#define CUERT_VERIFICATION_H +#include "gp_types.h" +typedef struct { char id[12], node[12], event[20]; + uint32_t tick_ms; uint16_t sequence; + int32_t expected, actual; bool check_ok; +} VerificationEvidence; +typedef bool (*VerifyWriteFn)(void *user, + const char *line, size_t length); +typedef struct { bool (*read_fault)(void *user, bool *fault); + void (*write_led)(void *user, bool on); void *user; +} DiagnosticsLedArgs; +GpResult Verification_Init(VerifyWriteFn write, void *user); +bool Verification_Log(const VerificationEvidence *e); +uint32_t Verification_DroppedCount(void); +void VerificationSerialTask(void *unused); +void DiagnosticsLedTask(void *argument); +#endif diff --git a/README.md b/README.md index 7f4f7a2..348e46e 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,8 @@ # ESP32-Sensing-Node -CUERT ESP32 sensing firmware, using C and the native ESP-IDF CMake build system. -The starter logs a heartbeat once per second and does not configure sensor pins. +CUERT ESP32 firmware, using C and the native ESP-IDF CMake build system. The +application reads revision-10 command frames on UART1 and forwards validated +commands as standard CAN frames through the ESP32 TWAI controller. ## Hardware baseline @@ -12,6 +13,11 @@ module's marking. The initial flash configuration uses a conservative 2 MB, DIO, 40 MHz baseline. Change flash size in `idf.py menuconfig` after confirming the module's actual capacity; 38-pin boards often have 4 MB. +UART1 uses GPIO17 (TX), GPIO16 (RX), and 115200 baud. TWAI uses GPIO21 (TX), +GPIO22 (RX), and 500 kbit/s. The ESP32 has no built-in CAN transceiver; connect +an external transceiver between the ESP32 and the CAN bus. Confirm pin wiring +and bus termination before connecting hardware. + ## One-time Windows setup 1. Open this directory in VS Code. @@ -96,8 +102,8 @@ ESP32's built-in ROM download bootloader over USB-to-UART, and writes the bootloader, partition table, and application at their generated offsets. An ST-Link is not required for normal USB flashing. -Expected output is `CUERT ESP32 sensing node started`, then a heartbeat each -second. Exit the monitor with Ctrl+]. VS Code also provides ESP-IDF commands +Expected output includes `CUERT ESP32 sensing node started`. Exit the monitor +with Ctrl+]. VS Code also provides ESP-IDF commands for building, selecting a port, flashing using UART, and monitoring. If no COM port appears, check the cable and the driver for the board's actual diff --git a/lib/gateway/CMakeLists.txt b/lib/gateway/CMakeLists.txt new file mode 100644 index 0000000..7fc9527 --- /dev/null +++ b/lib/gateway/CMakeLists.txt @@ -0,0 +1,5 @@ +idf_component_register( + SRCS "src/gateway.c" + INCLUDE_DIRS "include" + REQUIRES gp_protocol gp_common +) \ No newline at end of file diff --git a/lib/gateway/include/gateway.h b/lib/gateway/include/gateway.h new file mode 100644 index 0000000..526b218 --- /dev/null +++ b/lib/gateway/include/gateway.h @@ -0,0 +1,23 @@ +#ifndef GATEWAY_H +#define GATEWAY_H + +/* + * ESP32-side UART stream parser and command-to-CAN forwarding logic. + * + * 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 "gp_protocol.h" + +/* + * The callback must be nonblocking. It must copy the entire frame into a + * bounded, maintainer-owned TX queue before returning GP_OK. + * + * GP_PENDING means busy and nothing was accepted. + * GP_IO_ERROR means a permanent/driver error. + */ +#endif \ No newline at end of file diff --git a/lib/gateway/src/gateway.c b/lib/gateway/src/gateway.c new file mode 100644 index 0000000..6b43813 --- /dev/null +++ b/lib/gateway/src/gateway.c @@ -0,0 +1,253 @@ +#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->bytes[rx->used - 1u] == GP_UART_SOF0) { + rx->bytes[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->bytes[0] = byte; + rx->used = 1u; + } + + continue; + } + + /* + * The second SOF byte must follow. + */ + if (rx->used == 1u) { + if (byte == GP_UART_SOF1) { + rx->bytes[1] = byte; + rx->used = 2u; + } else if (byte == GP_UART_SOF0) { + rx->bytes[0] = byte; + rx->used = 1u; + } else { + rx->used = 0u; + } + + continue; + } + + rx->bytes[rx->used++] = byte; + + /* + * Once the five-byte header is available, + * reject unsupported versions, kinds, or lengths. + */ + if (rx->used == 5u && + (rx->bytes[2] != GP_UART_VERSION || + rx->bytes[3] != GP_UART_KIND || + rx->bytes[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->bytes, + 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->seen = false; + gateway->initialized = true; + + 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 || + !gateway->initialized) { + 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->seen) { + 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->seen = true; + } + + return result; +} + +void Gateway_Reset(Gateway *gateway) +{ + if (gateway != NULL) { + gateway->last_sequence = 0u; + gateway->seen = false; + } +} \ No newline at end of file diff --git a/lib/gp_common/CMakeLists.txt b/lib/gp_common/CMakeLists.txt new file mode 100644 index 0000000..08c11bc --- /dev/null +++ b/lib/gp_common/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register( + INCLUDE_DIRS "../../Contracts" +) \ No newline at end of file diff --git a/lib/gp_protocol/CMakeLists.txt b/lib/gp_protocol/CMakeLists.txt new file mode 100644 index 0000000..7153850 --- /dev/null +++ b/lib/gp_protocol/CMakeLists.txt @@ -0,0 +1,5 @@ +idf_component_register( + SRCS "src/gp_protocol.c" + INCLUDE_DIRS "include" + REQUIRES gp_common +) \ No newline at end of file diff --git a/lib/gp_protocol/include/gp_protocol.h b/lib/gp_protocol/include/gp_protocol.h new file mode 100644 index 0000000..8bb25b0 --- /dev/null +++ b/lib/gp_protocol/include/gp_protocol.h @@ -0,0 +1,81 @@ +#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 +#include "gp_types.h" + +#define GP_PROTOCOL_REV GP_API_REVISION + +/* ---------- 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 GP_UART_FRAME_BYTES +#define GP_UART_SOF0 0xA5u +#define GP_UART_SOF1 0x5Au +#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 GP_CAN_COMMAND_ID +#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. + */ +/* 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) + +/* + * 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. + */ + +#endif /* GP_PROTOCOL_H */ diff --git a/lib/gp_protocol/src/gp_protocol.c b/lib/gp_protocol/src/gp_protocol.c new file mode 100644 index 0000000..c176712 --- /dev/null +++ b/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; +} diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 01e432d..a7ca9de 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1 +1,15 @@ -idf_component_register(SRCS "main.c" INCLUDE_DIRS "." REQUIRES freertos log) +idf_component_register( + SRCS + "main.c" + INCLUDE_DIRS + "." + REQUIRES + gateway + gp_protocol + gp_common + freertos + log + esp_driver_gpio + esp_driver_uart + esp_driver_twai +) \ No newline at end of file diff --git a/main/main.c b/main/main.c index b21368b..3733c25 100644 --- a/main/main.c +++ b/main/main.c @@ -1,18 +1,257 @@ -#include +#include "gateway.h" -#include "esp_log.h" #include "freertos/FreeRTOS.h" +#include "freertos/queue.h" #include "freertos/task.h" +#include "driver/uart.h" +#include "esp_twai.h" +#include "esp_twai_onchip.h" +#include "esp_err.h" +#include "esp_log.h" + +#include +#include + +/* UART configuration */ +#define UART_PORT UART_NUM_1 +#define UART_TX_PIN GPIO_NUM_16 /* IO16 -> Blackpill PA10 (USART1 RX) */ +#define UART_RX_PIN GPIO_NUM_17 /* IO17 <- Blackpill PA9 (USART1 TX) */ +#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 +#define CAN_TX_QUEUE_LENGTH 10 +#define UART_READ_BUFFER_SIZE 128 + +static Gateway gateway; +static UartRx uart_rx; +static QueueHandle_t can_tx_queue; +static twai_node_handle_t can_node; static const char *TAG = "sensing_node"; +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 + ); +} + +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, + .tx_queue_depth = CAN_TX_QUEUE_LENGTH, + }; + + esp_err_t err = twai_new_node_onchip(&cfg, &can_node); + if (err != ESP_OK) return err; + + return twai_node_enable(can_node); +} + +static GpResult CanQueueWrite(void *user, const GpCanFrame *frame) +{ + (void)user; + + if (frame == NULL || can_tx_queue == NULL) { + return GP_BAD_ARG; + } + + if (frame->id > 0x7FFu || frame->dlc != GP_CAN_DLC) { + return GP_INVALID; + } + + if (xQueueSend(can_tx_queue, frame, 0) != pdTRUE) { + return GP_PENDING; + } + + return GP_OK; +} + +static void CanTxTask(void *argument) +{ + GpCanFrame queued_frame; + (void)argument; + + for (;;) { + if (xQueueReceive(can_tx_queue, &queued_frame, portMAX_DELAY) != pdTRUE) { + continue; + } + + twai_frame_t tx_frame = { + .header = { + .id = queued_frame.id, + .ide = false, + .rtr = false, + .fdf = false, + .dlc = queued_frame.dlc, + }, + .buffer = queued_frame.data, + .buffer_len = queued_frame.dlc, + }; + + esp_err_t err = twai_node_transmit(can_node, &tx_frame, -1); + if (err == ESP_OK) { + /* Keep queued_frame alive until the asynchronous TX is complete. */ + err = twai_node_transmit_wait_all_done(can_node, pdMS_TO_TICKS(10)); + } + + if (err != ESP_OK) { + ESP_LOGE(TAG, "TWAI transmit failed: %s", esp_err_to_name(err)); + } + } +} + +static uint32_t QueueAgeMs(TickType_t queued_at) +{ + TickType_t elapsed_ticks = xTaskGetTickCount() - queued_at; + + if (elapsed_ticks > pdMS_TO_TICKS(MAX_QUEUE_MS)) { + return MAX_QUEUE_MS + 1u; + } + + return (uint32_t)elapsed_ticks * portTICK_PERIOD_MS; +} + void app_main(void) { + uint8_t read_buffer[UART_READ_BUFFER_SIZE]; + size_t read_length = 0u; + size_t read_offset = 0u; + TargetSample pending_command; + bool have_pending_command = false; + TickType_t chunk_received_at = 0; + TickType_t pending_since = 0; + + can_tx_queue = xQueueCreate(CAN_TX_QUEUE_LENGTH, sizeof(GpCanFrame)); + if (can_tx_queue == NULL) { + ESP_LOGE(TAG, "Failed to create CAN transmit queue"); + abort(); + } + + ESP_ERROR_CHECK(UART_Init()); + ESP_ERROR_CHECK(CAN_Init()); + UartRx_Init(&uart_rx); + + if (Gateway_Init(&gateway, CanQueueWrite, NULL, MAX_QUEUE_MS) != GP_OK) { + ESP_LOGE(TAG, "Failed to initialize gateway"); + abort(); + } + + if (xTaskCreate(CanTxTask, "can_tx", 3072, NULL, 5, NULL) != pdPASS) { + ESP_LOGE(TAG, "Failed to create CAN transmit task"); + abort(); + } + ESP_LOGI(TAG, "CUERT ESP32 sensing node started"); - uint32_t heartbeat = 0; while (1) { - ESP_LOGI(TAG, "Heartbeat: %" PRIu32, heartbeat++); - vTaskDelay(pdMS_TO_TICKS(1000)); + if (have_pending_command) { + GpResult result = Gateway_Forward( + &gateway, + &pending_command, + QueueAgeMs(pending_since) + ); + + if (result == GP_PENDING) { + vTaskDelay(pdMS_TO_TICKS(1)); + continue; + } + + if (result != GP_OK) { + ESP_LOGW(TAG, "Queued command rejected: %d", (int)result); + } + have_pending_command = false; + } + + if (read_offset >= read_length) { + int bytes_read = uart_read_bytes( + UART_PORT, + read_buffer, + sizeof(read_buffer), + pdMS_TO_TICKS(20) + ); + + if (bytes_read <= 0) { + continue; + } + + read_length = (size_t)bytes_read; + read_offset = 0u; + chunk_received_at = xTaskGetTickCount(); + } + + size_t consumed = 0u; + TargetSample command; + GpResult result = UartRx_Feed( + &uart_rx, + &read_buffer[read_offset], + read_length - read_offset, + &consumed, + &command + ); + + if (consumed == 0u) { + ESP_LOGE(TAG, "UART parser made no progress"); + read_offset = read_length; + continue; + } + read_offset += consumed; + + if (result == GP_OK) { + result = Gateway_Forward(&gateway, &command, QueueAgeMs(chunk_received_at)); + if (result == GP_PENDING) { + pending_command = command; + pending_since = chunk_received_at; + have_pending_command = true; + } else if (result != GP_OK) { + ESP_LOGW(TAG, "UART command rejected: %d", (int)result); + } + } else if (result != GP_PENDING && result != GP_INVALID) { + ESP_LOGW(TAG, "UART processing failed: %d", (int)result); + } } } diff --git a/tests/host/CMakeLists.txt b/tests/host/CMakeLists.txt new file mode 100644 index 0000000..8107573 --- /dev/null +++ b/tests/host/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.16) +project(gp_host_tests C) + +enable_testing() + +add_executable(gateway_integration_test + gateway_integration_test.c + ../../lib/gateway/src/gateway.c + ../../lib/gp_protocol/src/gp_protocol.c +) + +target_include_directories(gateway_integration_test PRIVATE + ../../Contracts + ../../lib/gateway/include + ../../lib/gp_protocol/include +) + +add_test(NAME gateway_integration_test COMMAND gateway_integration_test) \ No newline at end of file diff --git a/tests/host/CMakeLists.txt.local_bak b/tests/host/CMakeLists.txt.local_bak new file mode 100644 index 0000000..feb0284 --- /dev/null +++ b/tests/host/CMakeLists.txt.local_bak @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.20) +project(esp32_host_tests LANGUAGES C) + +enable_testing() + +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_EXTENSIONS OFF) + +get_filename_component(ROOT "${CMAKE_CURRENT_LIST_DIR}/../.." ABSOLUTE) + +add_executable(test_gateway + test_gateway.c + "${ROOT}/main/lib/gateway/src/gateway.c" + "${ROOT}/main/lib/gp-protocol/src/gp-protocol.c" +) + +target_include_directories(test_gateway PRIVATE + "${ROOT}/main/lib/gateway/include" + "${ROOT}/main/lib/gp-protocol/include" +) + +if(CMAKE_C_COMPILER_ID MATCHES "GNU|Clang") + target_compile_options(test_gateway PRIVATE + -Wall -Wextra -Wpedantic + ) +endif() + +add_test(NAME GatewayHostTests COMMAND test_gateway) diff --git a/tests/host/gateway_integration_test.c b/tests/host/gateway_integration_test.c new file mode 100644 index 0000000..97b1130 --- /dev/null +++ b/tests/host/gateway_integration_test.c @@ -0,0 +1,50 @@ +#include "gateway.h" + +#include +#include +#include + +static GpCanFrame captured_frame; +static unsigned int captured_count; + +static GpResult capture_can(void *user, const GpCanFrame *frame) +{ + (void)user; + captured_frame = *frame; + ++captured_count; + return GP_OK; +} + +int main(void) +{ + UartRx rx; + Gateway gateway; + TargetSample input = { + .sequence = 1u, + .throttle_pm = 0u, + .steering_pm = 0, + .flags = GP_FLAG_VALID, + }; + TargetSample parsed; + TargetSample decoded_can; + uint8_t uart_bytes[GP_UART_FRAME_LEN]; + size_t written = 0u; + size_t consumed = 0u; + + assert(GP_EncodeUart(&input, uart_bytes, sizeof(uart_bytes), &written) == GP_OK); + assert(written == GP_UART_FRAME_LEN); + + UartRx_Init(&rx); + assert(UartRx_Feed(&rx, uart_bytes, written, &consumed, &parsed) == GP_OK); + assert(consumed == written); + assert(parsed.sequence == input.sequence); + + assert(Gateway_Init(&gateway, capture_can, NULL, 100u) == GP_OK); + assert(Gateway_Forward(&gateway, &parsed, 0u) == GP_OK); + assert(captured_count == 1u); + assert(GP_DecodeCan(&captured_frame, &decoded_can) == GP_OK); + assert(decoded_can.sequence == input.sequence); + assert(decoded_can.flags == input.flags); + + return 0; +} \ No newline at end of file diff --git a/tests/host/test_gateway.c b/tests/host/test_gateway.c new file mode 100644 index 0000000..fe40359 --- /dev/null +++ b/tests/host/test_gateway.c @@ -0,0 +1,240 @@ +#ifdef NDEBUG +#undef NDEBUG +#endif +#include +#include +#include +#include +#include + +#include "gateway.h" +#include "gp-protocol.h" + +/* Mock CAN Context for Gateway Testing */ +typedef struct { + GpCanFrame last_frame; + unsigned write_calls; + GpResult return_result; +} MockCan; + +static GpResult mock_can_write(void *user, const GpCanFrame *frame) +{ + assert(user != NULL); + assert(frame != NULL); + MockCan *mock = (MockCan *)user; + mock->write_calls++; + if (mock->return_result != GP_OK) { + return mock->return_result; + } + memcpy(&mock->last_frame, frame, sizeof(GpCanFrame)); + return GP_OK; +} + +/* UT-003a: GP_EncodeUart buffer insufficient capacity check */ +static void test_ut003a_encode_uart_no_space(void) +{ + TargetSample cmd = {.sequence = 1, .throttle_pm = 500, .steering_pm = 0, .flags = GP_FLAG_VALID}; + uint8_t buf[13]; + size_t written = 99; + GpResult res = GP_EncodeUart(&cmd, buf, sizeof(buf), &written); + assert(res == GP_NO_SPACE); + assert(written == 0); + printf("PASS: UT-003a (GP_EncodeUart no space check)\n"); +} + +/* UT-003b: GP_DecodeUart valid 14-byte frame decode */ +static void test_ut003b_decode_uart_valid(void) +{ + TargetSample in = {.sequence = 0x1234, .throttle_pm = 750, .steering_pm = -300, .flags = GP_FLAG_VALID | GP_FLAG_ARM}; + uint8_t raw[14]; + size_t written = 0; + assert(GP_EncodeUart(&in, raw, sizeof(raw), &written) == GP_OK); + assert(written == 14); + + TargetSample out = {0}; + assert(GP_DecodeUart(raw, sizeof(raw), &out) == GP_OK); + assert(out.sequence == in.sequence); + assert(out.throttle_pm == in.throttle_pm); + assert(out.steering_pm == in.steering_pm); + assert(out.flags == in.flags); + printf("PASS: UT-003b (GP_DecodeUart valid frame)\n"); +} + +/* UT-003c: GP_DecodeUart CRC corruption detection */ +static void test_ut003c_decode_uart_corrupted_crc(void) +{ + TargetSample in = {.sequence = 10, .throttle_pm = 200, .steering_pm = 100, .flags = GP_FLAG_VALID}; + uint8_t raw[14]; + size_t written = 0; + assert(GP_EncodeUart(&in, raw, sizeof(raw), &written) == GP_OK); + + /* Flip a bit in CRC (byte 12) */ + raw[12] ^= 0x01; + + TargetSample out = {.sequence = 999}; + assert(GP_DecodeUart(raw, sizeof(raw), &out) == GP_INVALID); + assert(out.sequence == 999); /* Untouched on error */ + printf("PASS: UT-003c (GP_DecodeUart CRC corruption rejection)\n"); +} + +/* UT-003d: GP_EncodeCan frame layout & CRC-8/SMBUS calculation */ +static void test_ut003d_encode_can_golden_crc(void) +{ + TargetSample cmd = {.sequence = 1, .throttle_pm = 0, .steering_pm = 0, .flags = 0}; + GpCanFrame frame = {0}; + assert(GP_EncodeCan(&cmd, &frame) == GP_OK); + assert(frame.id == GP_CAN_ID_COMMAND); + assert(frame.dlc == GP_CAN_DLC); + /* Golden vector: [01 00 00 00 00 00 00] -> CRC8 is 0xDF */ + assert(frame.data[7] == 0xDF); + printf("PASS: UT-003d (GP_EncodeCan layout & CRC8 golden vector 0xDF)\n"); +} + +/* UT-003e: GP_DecodeCan frame decode */ +static void test_ut003e_decode_can_valid(void) +{ + TargetSample cmd = {.sequence = 42, .throttle_pm = 600, .steering_pm = 250, .flags = GP_FLAG_VALID}; + GpCanFrame frame = {0}; + assert(GP_EncodeCan(&cmd, &frame) == GP_OK); + + TargetSample out = {0}; + assert(GP_DecodeCan(&frame, &out) == GP_OK); + assert(out.sequence == 42); + assert(out.throttle_pm == 600); + assert(out.steering_pm == 250); + assert(out.flags == GP_FLAG_VALID); + printf("PASS: UT-003e (GP_DecodeCan valid frame)\n"); +} + +/* UT-003f: UartRx_Feed split-chunk stream feeding */ +static void test_ut003f_uartrx_split_chunks(void) +{ + TargetSample in = {.sequence = 100, .throttle_pm = 400, .steering_pm = -100, .flags = GP_FLAG_VALID}; + uint8_t raw[14]; + size_t written = 0; + assert(GP_EncodeUart(&in, raw, sizeof(raw), &written) == GP_OK); + + UartRx rx; + UartRx_Init(&rx); + + size_t consumed = 0; + TargetSample out = {0}; + + /* Feed Chunk 1 (3 bytes) */ + assert(UartRx_Feed(&rx, raw, 3, &consumed, &out) == GP_PENDING); + assert(consumed == 3); + + /* Feed Chunk 2 (7 bytes) */ + assert(UartRx_Feed(&rx, raw + 3, 7, &consumed, &out) == GP_PENDING); + assert(consumed == 7); + + /* Feed Chunk 3 (remaining 4 bytes) */ + assert(UartRx_Feed(&rx, raw + 10, 4, &consumed, &out) == GP_OK); + assert(consumed == 4); + assert(out.sequence == 100); + assert(out.throttle_pm == 400); + printf("PASS: UT-003f (UartRx_Feed split-chunk assembly)\n"); +} + +/* UT-003g: UartRx_Feed resynchronization after corrupted byte */ +static void test_ut003g_uartrx_resync(void) +{ + UartRx rx; + UartRx_Init(&rx); + + uint8_t garbage[] = {0xFF, 0x00, 0x12}; + size_t consumed = 0; + TargetSample out = {0}; + + GpResult res = UartRx_Feed(&rx, garbage, sizeof(garbage), &consumed, &out); + assert(res == GP_PENDING || res == GP_INVALID); + assert(consumed > 0); + printf("PASS: UT-003g (UartRx_Feed resync to SOF)\n"); +} + +/* UT-003h: Gateway_Init parameter validation */ +static void test_ut003h_gateway_init_bounds(void) +{ + Gateway gw; + MockCan mock = {0}; + + assert(Gateway_Init(NULL, mock_can_write, &mock, 50) == GP_BAD_ARG); + assert(Gateway_Init(&gw, NULL, &mock, 50) == GP_BAD_ARG); + assert(Gateway_Init(&gw, mock_can_write, &mock, 0) == GP_INVALID); + assert(Gateway_Init(&gw, mock_can_write, &mock, 0x80000000u) == GP_INVALID); + assert(Gateway_Init(&gw, mock_can_write, &mock, 50) == GP_OK); + printf("PASS: UT-003h (Gateway_Init bounds validation)\n"); +} + +/* UT-003i: Gateway_Forward initial sequence baseline requires neutral */ +static void test_ut003i_gateway_initial_baseline(void) +{ + Gateway gw; + MockCan mock = {.return_result = GP_OK}; + assert(Gateway_Init(&gw, mock_can_write, &mock, 50) == GP_OK); + + /* Armed command before baseline accepted must be rejected */ + TargetSample armed_cmd = {.sequence = 1, .throttle_pm = 100, .steering_pm = 0, .flags = GP_FLAG_VALID | GP_FLAG_ARM}; + assert(Gateway_Forward(&gw, &armed_cmd, 10) == GP_INVALID); + assert(mock.write_calls == 0); + + /* Valid neutral with ARM low establishes baseline */ + TargetSample neutral_cmd = {.sequence = 1, .throttle_pm = 0, .steering_pm = 0, .flags = GP_FLAG_VALID}; + assert(Gateway_Forward(&gw, &neutral_cmd, 10) == GP_OK); + assert(mock.write_calls == 1); + printf("PASS: UT-003i (Gateway_Forward initial baseline requires neutral)\n"); +} + +/* UT-003j: Gateway_Forward sequence freshness window */ +static void test_ut003j_gateway_sequence_freshness(void) +{ + Gateway gw; + MockCan mock = {.return_result = GP_OK}; + assert(Gateway_Init(&gw, mock_can_write, &mock, 50) == GP_OK); + + TargetSample neutral = {.sequence = 100, .throttle_pm = 0, .steering_pm = 0, .flags = GP_FLAG_VALID}; + assert(Gateway_Forward(&gw, &neutral, 5) == GP_OK); + + /* Duplicate sequence (delta 0) rejected */ + TargetSample dup = {.sequence = 100, .throttle_pm = 200, .steering_pm = 0, .flags = GP_FLAG_VALID | GP_FLAG_ARM}; + assert(Gateway_Forward(&gw, &dup, 5) == GP_INVALID); + + /* Fresh sequence (delta 1) accepted */ + TargetSample fresh = {.sequence = 101, .throttle_pm = 200, .steering_pm = 0, .flags = GP_FLAG_VALID | GP_FLAG_ARM}; + assert(Gateway_Forward(&gw, &fresh, 5) == GP_OK); + printf("PASS: UT-003j (Gateway_Forward sequence freshness window)\n"); +} + +/* UT-003k: Gateway_Forward queue residence timeout rejection */ +static void test_ut003k_gateway_queue_timeout(void) +{ + Gateway gw; + MockCan mock = {.return_result = GP_OK}; + assert(Gateway_Init(&gw, mock_can_write, &mock, 50) == GP_OK); + + TargetSample neutral = {.sequence = 200, .throttle_pm = 0, .steering_pm = 0, .flags = GP_FLAG_VALID}; + assert(Gateway_Forward(&gw, &neutral, 5) == GP_OK); + + /* Command with queued_ms (60ms) exceeding max_queue_ms (50ms) must be dropped */ + TargetSample stale = {.sequence = 201, .throttle_pm = 100, .steering_pm = 0, .flags = GP_FLAG_VALID}; + assert(Gateway_Forward(&gw, &stale, 60) == GP_INVALID); + printf("PASS: UT-003k (Gateway_Forward queue residence timeout)\n"); +} + +int main(void) +{ + printf("=== Running ESP32 Gateway Host Unit Tests (UT-003a..k) ===\n"); + test_ut003a_encode_uart_no_space(); + test_ut003b_decode_uart_valid(); + test_ut003c_decode_uart_corrupted_crc(); + test_ut003d_encode_can_golden_crc(); + test_ut003e_decode_can_valid(); + test_ut003f_uartrx_split_chunks(); + test_ut003g_uartrx_resync(); + test_ut003h_gateway_init_bounds(); + test_ut003i_gateway_initial_baseline(); + test_ut003j_gateway_sequence_freshness(); + test_ut003k_gateway_queue_timeout(); + printf("=== All ESP32 Gateway Host Unit Tests PASSED ===\n"); + return 0; +}