insert yesense utc timestamp

yesense_utc.c

//
// Created by hvt on 2021/11/3.
//
#include <stddef.h>
#include <stdio.h>

enum PARSE_STATES {
    FIND_HEAD_0,
    FIND_HEAD_1,
    FIND_LENGTH,
    FIND_END
};

unsigned char ys_frame[512];
unsigned char ys_frame_length = 0;

enum PARSE_STATES ys_state = FIND_HEAD_0;
unsigned int ys_received_buffer_size = 0;


/**
 * yesense报文校验
 * @param data 校验起始指针
 * @param len 校验长度
 * @param CK1 校验结果位1
 * @param CK2 校验结果位2
 * @return
 */
int crc_ys_ck1_ck2(const unsigned char *const data, const unsigned char len, unsigned char *CK1, unsigned char *CK2) {
    *CK1 = *CK2 = 0;
    unsigned short i;

    for (i = 0; i < len; i++) {
        *CK1 += data[i];
        *CK2 += *CK1;
    }
//    *checksum = ((unsigned short) (check_a << 8) | check_b);

    return 0;
}

/**
 * 在YS报文中UTC时间戳
 * @param imu_frame 完整的yesense报文
 * @param secs UTC秒
 * @param microsecs UTC微秒
 * @return 新的帧长度
 */
unsigned char insert_utc_timestamp(unsigned char *imu_frame, const unsigned int secs, const unsigned int microsecs) {
    unsigned char *length_field = imu_frame + 4;
    unsigned char *data_length = length_field;

    // utc插入位置
    int insert_position = 2 + 2 + 1 + *data_length;

    // utc秒代替采样时间
    imu_frame[insert_position] = 0x51;
    imu_frame[insert_position + 1] = 0x04;
    *((unsigned int *) &(imu_frame[insert_position + 2])) = secs;

    // utc微秒代替同步输出时间
    imu_frame[insert_position + 6] = 0x52;
    imu_frame[insert_position + 7] = 0x04;
    *((unsigned int *) &(imu_frame[insert_position + 8])) = microsecs;

    // 修改data字段的length
    *data_length += 12;

    // 重新计算crc
    crc_ys_ck1_ck2(&imu_frame[2], 2 + 1 + *data_length,
                   &imu_frame[insert_position + 12], &imu_frame[insert_position + 13]);

    // 返回最新的帧长度
    return 2 + 2 + 1 + *data_length + 2;
}

/**
 * 处理收到的一个byte
 * @param byte
 */
void process_recv_ys_byte(unsigned char byte) {
    if (ys_state == FIND_HEAD_0) {
        if (byte == 0x59) {
            ys_received_buffer_size = 0;
            ys_frame_length = 0;
            ys_frame[ys_received_buffer_size++] = byte;
            ys_state = FIND_HEAD_1;
        } else {
            ys_state = FIND_HEAD_0;
        }
    } else if (ys_state == FIND_HEAD_1) {
        if (byte == 0x53) {
            ys_state = FIND_LENGTH;
            ys_frame[ys_received_buffer_size++] = byte;
        } else {
            ys_state = FIND_HEAD_0;
        }
    } else if (ys_state == FIND_LENGTH) {
        ys_frame[ys_received_buffer_size++] = byte;
        if (ys_received_buffer_size == 5) {
            // 帧头2+ID2+长度1+数据+校验2
            ys_frame_length = 2 + 2 + 1 + byte + 2;
            ys_state = FIND_END;
        }
    } else if (ys_state == FIND_END) {
        ys_frame[ys_received_buffer_size++] = byte;
        if (ys_received_buffer_size == ys_frame_length) {
            // =======================
            // TODO 1. 插入utc时间戳, 并自动更新帧长度
            ys_frame_length = insert_utc_timestamp(ys_frame, 88888, 99999);
            // TODO 2. 数据发往上位机
            // ......
            // =======================
            ys_state = FIND_HEAD_0;
        }
    }
}

int main() {

    unsigned char fake_ys_data[] =
            {
                    0x59, 0x53, 0xBC, 0x93, 0x80, 0x10, 0x0C, 0x65, 0x07, 0xFF, 0xFF, 0x7B, 0xFD, 0x00, 0x00, 0x41,
                    0xFB, 0x94, 0x00, 0x20, 0x0C, 0x67, 0xDE, 0x01, 0x00, 0x2A, 0x3E, 0x00,
                    0x00, 0x01, 0xE2, 0xFF, 0xFF, 0x30, 0x0C, 0xDC, 0xA7, 0xC8, 0xEF, 0xA8, 0x72, 0x4D, 0x0B, 0x18,
                    0xCD, 0x4C, 0xF1, 0x31, 0x0C, 0x42, 0xD9, 0xFB, 0xFF, 0xB9, 0xE4, 0x02,
                    0x00, 0x9F, 0x3C, 0xFC, 0xFF, 0x40, 0x0C, 0x9C, 0x18, 0x16, 0x00, 0x69, 0x9F, 0xF8, 0xFF, 0x61,
                    0x1E, 0x40, 0x00, 0x41, 0x10, 0x45, 0x3F, 0x0F, 0x00, 0xB9, 0xED, 0xFF,
                    0xFF, 0xB9, 0x30, 0x00, 0x00, 0x67, 0x8F, 0x00, 0x00, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x98, 0xBD, 0xFF, 0xFF, 0x70, 0x0C, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x51, 0x04, 0x7C, 0x2A, 0x76, 0x2D, 0x52,
                    0x04, 0xEC, 0x31, 0x76, 0x2D, 0x53, 0x54
            };

    for (int i = 0; i < sizeof(fake_ys_data) / sizeof(unsigned char); ++i) {
        // ==========================
        // TODO 每次收到一个串口的byte就调用一次process_recv_ys_byte
        process_recv_ys_byte(fake_ys_data[i]);
    }

    for (int i = 0; i < ys_frame_length; ++i) {
        printf("%02x ", ys_frame[i]);
    }
    printf("\n");

    return 0;
}