Radick/main/radar_protocol.c

#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;
}