hpm6750/controller_yy_app/vklink/um482.c

#include "um482.h"
#include <stdio.h>
#include <string.h>

#define CRC32_POLYNOMIAL 0xEDB88320

/* 消息类型 */
enum
{
    UM482_MSG_TYPE_BINARY = 0,
    UM482_MSG_TYPE_ASCII = 1,
    UM482_MSG_TYPE_ASCII_SIMPLE = 2
};

/* 消息解析阶段 */
enum
{
    UM482_PARSE_STATE_UNINIT = 0,
    UM482_PARSE_STATE_IDLE = 1,
    UM482_PARSE_STATE_GOT_HEADER = 2,
    UM482_PARSE_STATE_GOT_DATA = 3,
    UM482_PARSE_STATE_GOT_CRC = 4
};

/* msg id 枚举定义 */
enum
{
    UM482_MSG_BESTPOS_ID = 42,
    UM482_MSG_BESTVEL_ID = 99,
    UM482_MSG_HEADING_ID = 971,
};

unsigned long _CRC32Value(int i)
{
    int j;
    unsigned long ulCRC;
    ulCRC = i;
    for (j = 8; j > 0; j--)
    {
        if (ulCRC & 1)
            ulCRC = (ulCRC >> 1) ^ CRC32_POLYNOMIAL;
        else
            ulCRC >>= 1;
    }
    return ulCRC;
}

unsigned long _CalculateBlockCRC32(unsigned char *ucBuffer,
                                   unsigned long ulCount, uint32_t initValue)
{
    unsigned long ulTemp1;
    unsigned long ulTemp2;
    unsigned long ulCRC = initValue;
    while (ulCount-- != 0)
    {
        ulTemp1 = (ulCRC >> 8) & 0x00FFFFFF;
        ulTemp2 = _CRC32Value(((int)ulCRC ^ *ucBuffer++) & 0xff);
        ulCRC = ulTemp1 ^ ulTemp2;
    }
    return (ulCRC);
}

void um482_rxmsg_init(um482_msg_t *msg)
{
    msg->_parse_state = UM482_PARSE_STATE_IDLE;
    msg->_head_rx_index = 0;
    msg->_data_rx_index = 0;
    msg->_check_rx_index = 0;
}

void um482_rxmsg_deinit(um482_msg_t *msg)
{
    msg->_parse_state = UM482_PARSE_STATE_UNINIT;
}

int8_t um482_rxmsg_parse_char(uint8_t c, um482_msg_t *msg)
{
    int8_t ret_val = 0;
    if (msg->_parse_state == UM482_PARSE_STATE_UNINIT)
    {
        um482_rxmsg_init(msg);
    }

    switch (msg->_parse_state)
    {
    case UM482_PARSE_STATE_IDLE:
        switch (msg->_head_rx_index)
        {
        case 0:
            if (c == 0xAA)
            {
                msg->header.sync[0] = c;
                msg->_head_rx_index++;
            }
            else
            {
                um482_rxmsg_init(msg);
            }

            break;

        case 1:
            if (c == 0x44)
            {
                msg->header.sync[1] = c;
                msg->_head_rx_index++;
            }
            else
            {
                um482_rxmsg_init(msg);
            }
            break;

        case 2:
            if (c == 0x12)
            {
                msg->header.sync[2] = c;
                msg->_head_rx_index++;
            }
            else
            {
                um482_rxmsg_init(msg);
            }
            break;

        case 3:
            if (c == sizeof(msg->header))
            {
                msg->header.header_len = c;
                msg->_head_rx_index++;
            }
            else
            {
                um482_rxmsg_init(msg);
            }
            break;

        default:
            if (msg->_head_rx_index < sizeof(msg->header))
            {
                uint8_t *p = (uint8_t *)&msg->header;
                *(p + msg->_head_rx_index) = c;
                msg->_head_rx_index++;
                if (msg->_head_rx_index >= sizeof(msg->header))
                {
                    if (msg->header.msg_len <= sizeof(msg->data))
                    {
                        msg->_parse_state = UM482_PARSE_STATE_GOT_HEADER;
                        msg->_head_rx_index = 0;
                        msg->_data_rx_index = 0;
                    }
                    else
                    {
                        um482_rxmsg_init(msg);
                    }
                }
            }
            else
            {
                um482_rxmsg_init(msg);
            }
            break;
        }
        break;

    case UM482_PARSE_STATE_GOT_HEADER:
        if (msg->_data_rx_index < msg->header.msg_len)
        {
            msg->data[msg->_data_rx_index] = c;
            msg->_data_rx_index++;
            if (msg->_data_rx_index >= msg->header.msg_len)
            {
                msg->_parse_state = UM482_PARSE_STATE_GOT_DATA;
                msg->_check_rx_index = 0;
                msg->crc32_check = 0;
            }
        }
        else
        {
            um482_rxmsg_init(msg);
        }
        break;

    case UM482_PARSE_STATE_GOT_DATA:
        if (msg->_check_rx_index < sizeof(msg->crc32_check))
        {
            msg->crc32_check += c << (msg->_check_rx_index * 8);
            msg->_check_rx_index++;
            if (msg->_check_rx_index >= sizeof(msg->crc32_check))
            {
                uint32_t check = 0;
                /* 计算校验 */
                check = _CalculateBlockCRC32((uint8_t *)&msg->header,
                                             sizeof(msg->header), 0);
                check =
                    _CalculateBlockCRC32(msg->data, msg->header.msg_len, check);

                if (check == msg->crc32_check)
                {
                    ret_val = 1;
                }
                else
                {
                    ret_val = -1;
                }
                um482_rxmsg_init(msg);
            }
        }
        else
        {
            um482_rxmsg_init(msg);
        }
        break;

    default:
        break;
    }

    return ret_val;
}

void um482_rxmsg_decode(um482_msg_t *msg, um482_rx_data_t *pdata)
{

    switch (msg->header.msg_id)
    {
    case UM482_MSG_BESTPOS_ID:
        pdata->bestpos_rx_count++;
        if (pdata->bestpos_rx_count == 0)
            pdata->bestpos_rx_count = 1;
        if (pdata->bestpos_data_link_status != COMP_CLOSED)
            pdata->bestpos_data_link_status = COMP_NORMAL;
        memcpy(&pdata->bestpos, msg->data, sizeof(pdata->bestpos));
        if (pdata->bestpos_rx_callback)
            pdata->bestpos_rx_callback(pdata);
        break;

    case UM482_MSG_BESTVEL_ID:
        pdata->bestvel_rx_count++;
        if (pdata->bestvel_rx_count == 0)
            pdata->bestvel_rx_count = 1;
        if (pdata->bestvel_data_link_status != COMP_CLOSED)
            pdata->bestvel_data_link_status = COMP_NORMAL;
        memcpy(&pdata->bestvel, msg->data, sizeof(pdata->bestvel));
        if (pdata->bestvel_rx_callback)
            pdata->bestvel_rx_callback(pdata);
        break;

    case UM482_MSG_HEADING_ID:
        pdata->heading_rx_count++;
        if (pdata->heading_rx_count == 0)
            pdata->heading_rx_count = 1;
        if (pdata->heading_data_link_status != COMP_CLOSED)
            pdata->heading_data_link_status = COMP_NORMAL;
        memcpy(&pdata->heading, msg->data, sizeof(pdata->heading));
        if (pdata->heading_rx_callback)
            pdata->heading_rx_callback(pdata);
        break;

    default:
        break;
    }
}

void um482_rxmsg_link_check(um482_rx_data_t *pdata)
{
    /* printf("%d %d %d\r\n", pdata->bestvel_rx_count, pdata->bestpos_rx_count,
           pdata->heading_rx_count); */

    pdata->rx_count = pdata->bestvel_rx_count + pdata->bestpos_rx_count +
                      pdata->heading_rx_count;
    if (pdata->bestvel_data_link_status == COMP_NORMAL)
    {
        if (pdata->bestvel_rx_count == 0)
        {
            pdata->bestvel_data_link_status = COMP_LOST;
        }
        pdata->bestvel_rx_count = 0;
    }

    if (pdata->bestpos_data_link_status == COMP_NORMAL)
    {
        if (pdata->bestpos_rx_count == 0)
        {
            pdata->bestpos_data_link_status = COMP_LOST;
        }

        pdata->bestpos_rx_count = 0;
    }

    if (pdata->heading_data_link_status == COMP_NORMAL)
    {
        if (pdata->heading_rx_count == 0)
        {
            pdata->heading_data_link_status = COMP_LOST;
        }

        pdata->heading_rx_count = 0;
    }
}