Implement native ESP-IDF radar firmware

main/radar_protocol.c

@@ -0,0 +1,198 @@
+#include "radar_protocol.h"
+
+#include <string.h>
+
+const uint8_t rd03d_frame_header[RD03D_FRAME_HEADER_SIZE] = {
+    0xAAU, 0xFFU, 0x03U, 0x00U,
+};
+
+const uint8_t rd03d_frame_tail[RD03D_FRAME_TAIL_SIZE] = {
+    0x55U, 0xCCU,
+};
+
+const uint8_t rd03d_cmd_enable[RD03D_CMD_ENABLE_SIZE] = {
+    0xFDU, 0xFCU, 0xFBU, 0xFAU, 0x04U, 0x00U, 0xFFU,
+    0x00U, 0x01U, 0x00U, 0x04U, 0x03U, 0x02U, 0x01U,
+};
+
+const uint8_t rd03d_cmd_multi[RD03D_CMD_MULTI_SIZE] = {
+    0xFDU, 0xFCU, 0xFBU, 0xFAU, 0x02U, 0x00U,
+    0x90U, 0x00U, 0x04U, 0x03U, 0x02U, 0x01U,
+};
+
+const uint8_t rd03d_cmd_end[RD03D_CMD_END_SIZE] = {
+    0xFDU, 0xFCU, 0xFBU, 0xFAU, 0x02U, 0x00U,
+    0xFEU, 0x00U, 0x04U, 0x03U, 0x02U, 0x01U,
+};
+
+static uint16_t decode_u16_le(const uint8_t *bytes)
+{
+    return (uint16_t)((uint16_t)bytes[0] |
+                      (uint16_t)((uint16_t)bytes[1] << 8U));
+}
+
+int16_t rd03d_decode_sign_magnitude(uint8_t low, uint8_t high)
+{
+    const uint16_t raw = (uint16_t)((uint16_t)low |
+                                    (uint16_t)((uint16_t)high << 8U));
+    const int16_t magnitude = (int16_t)(raw & UINT16_C(0x7FFF));
+
+    return (raw & UINT16_C(0x8000)) != 0U ? magnitude : (int16_t)-magnitude;
+}
+
+bool rd03d_decode_frame(const uint8_t frame[RD03D_FRAME_SIZE], rd03d_frame_t *out)
+{
+    size_t target_index;
+
+    if (frame == NULL || out == NULL) {
+        return false;
+    }
+    if (memcmp(frame, rd03d_frame_header, RD03D_FRAME_HEADER_SIZE) != 0 ||
+        memcmp(frame + RD03D_FRAME_SIZE - RD03D_FRAME_TAIL_SIZE,
+               rd03d_frame_tail,
+               RD03D_FRAME_TAIL_SIZE) != 0) {
+        return false;
+    }
+
+    memset(out, 0, sizeof(*out));
+    for (target_index = 0U; target_index < RD03D_MAX_TARGETS; ++target_index) {
+        const uint8_t *payload = frame + RD03D_FRAME_HEADER_SIZE +
+                                 target_index * RD03D_TARGET_DATA_SIZE;
+        rd03d_target_t *target = &out->targets[target_index];
+        uint8_t combined = 0U;
+        size_t byte_index;
+
+        for (byte_index = 0U; byte_index < RD03D_TARGET_DATA_SIZE; ++byte_index) {
+            combined |= payload[byte_index];
+        }
+        if (combined == 0U) {
+            continue;
+        }
+
+        target->x_mm = rd03d_decode_sign_magnitude(payload[0], payload[1]);
+        target->y_mm = rd03d_decode_sign_magnitude(payload[2], payload[3]);
+        target->speed_cm_s = rd03d_decode_sign_magnitude(payload[4], payload[5]);
+        target->resolution_mm = decode_u16_le(payload + 6U);
+        target->valid = true;
+    }
+
+    return true;
+}
+
+void rd03d_parser_init(rd03d_parser_t *parser)
+{
+    if (parser != NULL) {
+        memset(parser, 0, sizeof(*parser));
+    }
+}
+
+static void parser_seek_header(rd03d_parser_t *parser, uint8_t byte)
+{
+    if (byte == rd03d_frame_header[parser->header_matched]) {
+        ++parser->header_matched;
+        if (parser->header_matched == RD03D_FRAME_HEADER_SIZE) {
+            memcpy(parser->buffer, rd03d_frame_header, RD03D_FRAME_HEADER_SIZE);
+            parser->buffered = RD03D_FRAME_HEADER_SIZE;
+            parser->header_matched = 0U;
+        }
+        return;
+    }
+
+    /* The header has only one non-empty proper prefix/suffix: a single AA. */
+    parser->header_matched = byte == rd03d_frame_header[0] ? 1U : 0U;
+}
+
+static void parser_resynchronise(rd03d_parser_t *parser)
+{
+    size_t offset;
+    size_t suffix_length;
+
+    /* Prefer a complete header already present inside the rejected window. */
+    for (offset = 1U;
+         offset + RD03D_FRAME_HEADER_SIZE <= RD03D_FRAME_SIZE;
+         ++offset) {
+        if (memcmp(parser->buffer + offset,
+                   rd03d_frame_header,
+                   RD03D_FRAME_HEADER_SIZE) == 0) {
+            parser->buffered = RD03D_FRAME_SIZE - offset;
+            memmove(parser->buffer, parser->buffer + offset, parser->buffered);
+            parser->header_matched = 0U;
+            return;
+        }
+    }
+
+    /* Otherwise preserve a partial header at the end for a split next frame. */
+    parser->buffered = 0U;
+    parser->header_matched = 0U;
+    for (suffix_length = RD03D_FRAME_HEADER_SIZE - 1U;
+         suffix_length > 0U;
+         --suffix_length) {
+        if (memcmp(parser->buffer + RD03D_FRAME_SIZE - suffix_length,
+                   rd03d_frame_header,
+                   suffix_length) == 0) {
+            parser->header_matched = suffix_length;
+            return;
+        }
+    }
+}
+
+bool rd03d_parser_push(rd03d_parser_t *parser, uint8_t byte, rd03d_frame_t *out)
+{
+    rd03d_frame_t decoded;
+
+    if (parser == NULL) {
+        return false;
+    }
+
+    ++parser->bytes_received;
+    if (parser->buffered == 0U) {
+        parser_seek_header(parser, byte);
+        return false;
+    }
+
+    parser->buffer[parser->buffered++] = byte;
+    if (parser->buffered < RD03D_FRAME_SIZE) {
+        return false;
+    }
+
+    if (rd03d_decode_frame(parser->buffer, &decoded)) {
+        parser->buffered = 0U;
+        parser->header_matched = 0U;
+        ++parser->frames_decoded;
+        if (out != NULL) {
+            *out = decoded;
+        }
+        return true;
+    }
+
+    ++parser->malformed_frames;
+    parser_resynchronise(parser);
+    return false;
+}
+
+size_t rd03d_parser_feed(rd03d_parser_t *parser,
+                         const uint8_t *data,
+                         size_t length,
+                         rd03d_frame_callback_t callback,
+                         void *context)
+{
+    size_t index;
+    size_t decoded_count = 0U;
+
+    if (parser == NULL || (data == NULL && length != 0U)) {
+        return 0U;
+    }
+
+    for (index = 0U; index < length; ++index) {
+        rd03d_frame_t frame;
+
+        if (rd03d_parser_push(parser, data[index], &frame)) {
+            ++decoded_count;
+            if (callback != NULL) {
+                callback(&frame, context);
+            }
+        }
+    }
+
+    return decoded_count;
+}