协议解析改进版20241128

于 2024-11-28 20:00:54 发布
阅读量207 收藏
点赞数 2
文章标签： c++ c语言 算法
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/qq_47598441/article/details/144119337
版权

接收协议、解析协议、存储文件

bsb.h

#ifndef _BSB_HEADER
#define _BSB_HEADER

#include <stdio.h>
#define ERRORLOG(msg)                                       \
	do                                                      \
	{                                                       \
		printf("%s:%s:%d\n", __FILE__, __func__, __LINE__); \
		perror(msg);                                        \
	} while (0);


#define FRAME_HEADER (unsigned short)0xAD7E // 帧头
#define FRAME_SIZE 79						// 每帧数据字节数(从1计数)
#define CHECK_BEGIN 2						// 校验区起始字节(从0计数)
#define CHECK_END 76						// 校验区结束字节(从0计数)
#define CHECK_SUM_BEGIN 77					// 校验字段起始字节(无符号16位校验和)

/* Private data structure */
typedef struct bsb
{
	unsigned char *buf;
	unsigned char *ptr;
	unsigned char *end;
} BSB;
#define BSB_SET_ERROR(b) ((b).end = NULL)
#define BSB_LENGTH(b) ((b).ptr - (b).buf)
#define BSB_REMAINING(b) ((b).end ? (b).end - (b).ptr : 0)
#define BSB_WORK_PTR(b) ((b).ptr)
// 初始化
#define BSB_INIT(b, buffer, size)                     \
	do                                                \
	{                                                 \
		(b).buf = (unsigned char *)buffer;            \
		(b).ptr = (unsigned char *)buffer;            \
		int s = (int)size;                            \
		if ((buffer == NULL) || (s < 0))              \
			(b).end = 0;                              \
		else                                          \
			(b).end = (unsigned char *)buffer + size; \
	} while (0)
/*通信双方字节序一致时BSB_IMPORT_byte、BSB_EXPORT_ptr*/
// 取size字节到x（buff）中
#define BSB_IMPORT_byte(b, x, size)    \
	do                                 \
	{                                  \
		if ((b).ptr + size <= (b).end) \
		{                              \
			memcpy(x, b.ptr, size);    \
			(b).ptr += size;           \
		}                              \
		else                           \
		{                              \
			BSB_SET_ERROR(b);          \
            (x)[0] = 0;                \
		}                              \
	} while (0)
// 从x中写入size字节到bsb.buf中
#define BSB_EXPORT_ptr(b, x, size)               \
	do                                            \
	{                                             \
		if ((b).ptr + size <= (b).end)            \
		{                                         \
			memcpy((b).ptr, x, size);             \
			(b).ptr += size;                      \
		}                                         \
		else if (((b).end != NULL))                \
		{                                         \
			memcpy((b).ptr, x, BSB_REMAINING(b)); \
			(b).ptr += BSB_REMAINING(b);          \
		}                                         \
	} while (0)
// b.ptr前进size字节
#define BSB_skip(b, size)                                    \
	do                                                              \
	{                                                               \
		if ((b).ptr + size <= (b).end && (b).ptr + size >= (b).buf) \
		{                                                           \
			(b).ptr += size;                                        \
			if ((b).ptr < (b).buf)                                  \
				(b).end = 0;                                        \
		}                                                           \
		else                                                        \
			BSB_SET_ERROR(b);                                       \
	} while (0)
// 后退size字节
#define BSB_rewind(b, size)                                  \
	do                                                              \
	{                                                               \
		if ((b).ptr - size <= (b).end && (b).ptr - size >= (b).buf) \
		{                                                           \
			(b).ptr -= size;                                        \
			if ((b).ptr < (b).buf)                                  \
				(b).end = 0;                                        \
		}                                                           \
		else                                                        \
		{                                                           \
			BSB_SET_ERROR(b);                                       \
		}                                                           \
	} while (0)
//交换字节序
// int a=0x12345678;
// a=BSB_SWAP_ENDIAN(a,sizeof(a),int);
#define BSB_SWAP_ENDIAN(value, size, type)                   \
    ({                                                       \
        type temp;                                           \
        unsigned char *ptr = (unsigned char *)&(value);      \
        for (size_t i = 0; i < (size); i++)                  \
        {                                                    \
            ((unsigned char *)&temp)[(size) - 1 - i] = ptr[i]; \
        }                                                    \
        temp;                                                \
    })   
#endif

.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/select.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <unistd.h>
#include "bsb.h"
#define BUFFER_SIZE 1024
int check_sum(char *buf);
int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop);

int check_sum(char *buf)
{
    unsigned short sum = 0;
    for (size_t i = CHECK_BEGIN; i <= CHECK_END; i++)
    {
        sum += buf[i];
    }
    if (*(unsigned short *)&buf[CHECK_SUM_BEGIN] == sum)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}
int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio, oldtio;
    if (tcgetattr(fd, &oldtio) != 0)
    {
        perror("SetupSerial 1");
        return -1;
    }

    bzero(&newtio, sizeof(newtio));
    newtio.c_cflag |= CLOCAL | CREAD;
    newtio.c_cflag = ~CSIZE;

    switch (nBits)
    {
    case 7:
        newtio.c_cflag |= CS7;
        break;
    case 8:
        newtio.c_cflag |= CS8;
        break;
    }

    switch (nEvent)
    {
    case 'O': // 奇校验
        newtio.c_cflag |= PARENB;
        newtio.c_cflag |= PARODD;
        newtio.c_iflag |= (INPCK | ISTRIP);
        break;
    case 'E': // 偶校验
        newtio.c_iflag |= (INPCK | ISTRIP);
        newtio.c_cflag |= PARENB;
        newtio.c_cflag = ~PARODD;
        break;
    case 'N': // 无校验
        newtio.c_cflag = ~PARENB;
        break;
    }

    switch (nSpeed)
    {
    case 300:
        cfsetispeed(&newtio, B300);
        cfsetospeed(&newtio, B300);
        break;
    case 600:
        cfsetispeed(&newtio, B600);
        cfsetospeed(&newtio, B600);
        break;
    case 1200:
        cfsetispeed(&newtio, B1200);
        cfsetospeed(&newtio, B1200);
        break;
    case 2400:
        cfsetispeed(&newtio, B2400);
        cfsetospeed(&newtio, B2400);
        break;
    case 4800:
        cfsetispeed(&newtio, B4800);
        cfsetospeed(&newtio, B4800);
        break;
    case 9600:
        cfsetispeed(&newtio, B9600);
        cfsetospeed(&newtio, B9600);
        break;
    case 19200:
        cfsetispeed(&newtio, B19200);
        cfsetospeed(&newtio, B19200);
        break;
    case 38400:
        cfsetispeed(&newtio, B38400);
        cfsetospeed(&newtio, B38400);
        break;
    case 57600:
        cfsetispeed(&newtio, B57600);
        cfsetospeed(&newtio, B57600);
        break;
    case 115200:
        cfsetispeed(&newtio, B115200);
        cfsetospeed(&newtio, B115200);
        break;
    default:
        return -2;
        break;
    }
    if (nStop == 1)
    {
        newtio.c_cflag &= ~CSTOPB;
    }
    else if (nStop == 2)
    {
        newtio.c_cflag |= CSTOPB;
    }
    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 0;
    tcflush(fd, TCIFLUSH);
    if ((tcsetattr(fd, TCSANOW, &newtio)) != 0)
    {
        perror("com set error");
        return -1;
    }
    printf("uart inint done!\n");
    return 1;
}

/*****************方式1解析代码****************************/
#if 0
// 动态数组结构
typedef struct
{
    char *buffer;    // 指向动态分配的数组
    size_t size;     // 数组中当前元素的数量
    size_t capacity; // 数组的总容量
} DynamicArray;
#define INITIAL_CAPACITY 256 // 初始容量,不能为0
void initArray(DynamicArray *a);
void freeArray(DynamicArray *a);
void ensureCapacity(DynamicArray *a, size_t additionalSize);
void addData(DynamicArray *a, const char *data, size_t dataSize);
void removeFrame(DynamicArray *a, size_t frameSize);
// 初始化动态数组
void initArray(DynamicArray *a)
{

    a->buffer = (char *)malloc(INITIAL_CAPACITY * sizeof(char));

    if (a->buffer == NULL)
    {
        ERRORLOG("malloc error\n");
    }

    a->size = 0;
    a->capacity = INITIAL_CAPACITY;
}

// 释放动态数组
void freeArray(DynamicArray *a)
{
    free(a->buffer);
    a->buffer = NULL;
    a->size = 0;
    a->capacity = 0;
}

// 根据缓存数据大小动态扩容数组
// 案例:当串口阻塞时一次性接收数据大于缓存数据的缓冲区大小(>INITIAL_CAPACITY)若不动态扩容,则数据溢出
void ensureCapacity(DynamicArray *a, size_t additionalSize)
{
    size_t newCapacity = a->capacity;
    while (newCapacity < a->size + additionalSize)
    {
        newCapacity *= 2;
    }
    // 这里即使realloc内存小于原来内存,即使用realloc缩容,由于原来内存未释放，仍可以接着用
    char *newBuffer = (char *)realloc(a->buffer, newCapacity);
    if (newBuffer == NULL)
    {
        ERRORLOG("Memory realloc error\n");
    }
    a->buffer = newBuffer;
    a->capacity = newCapacity; // 接着使用原来内存大小或新扩容的内存大小
}

// 添加数据到动态数组
void addData(DynamicArray *a, const char *data, size_t dataSize)
{
    ensureCapacity(a, dataSize);
    memcpy(a->buffer + a->size, data, dataSize);
    // printf("a->size1:%ld,dataSize:%ld\n",a->size,dataSize);
    a->size += dataSize;
    // printf("a->size2:%ld\n",a->size);
}
// 从动态数组中移除一帧数据
void removeFrame(DynamicArray *a, size_t frameSize)
{
    if (frameSize > a->size)
    {
        ERRORLOG("remove frame size is larger than array size\n");
    }
    memmove(a->buffer, a->buffer + frameSize, a->size - frameSize);
    // printf("a->size:%ld\n",a->size);
    a->size -= frameSize;
    // printf("a->size -:%ld\n",a->size);
}
int main()
{
    char *serial_device = "/dev/ttyUSB0";
    int baudrate = 115200;
    char *output_file_name = "test.txt";

    int serial_fd = open(serial_device, O_RDWR | O_NOCTTY | O_NDELAY);
    if (serial_fd == -1)
    {
        perror("Unable to open serial port");
        return 1;
    }

    int setret = 0;
    setret = set_opt(serial_fd, baudrate, 8, 'N', 1);

    if (setret == -1)
    {
        close(serial_fd);
        perror("set baudrate error\n");
    }

    // 打开输出文件
    FILE *output_file = fopen(output_file_name, "w");
    if (!output_file)
    {
        perror("Unable to open output file");
        close(serial_fd);
        return 1;
    }

    fd_set read_fds;
    DynamicArray array;

    initArray(&array);
    char readBuffer[BUFFER_SIZE] = {0}; // 用于每次存储读取的数据
    int fre = 0;                        // 帧计数,测试
    int total = 0;                      // 接收总字节数,测试
    while (1)
    {
        FD_ZERO(&read_fds);
        FD_SET(serial_fd, &read_fds);

        // 使用 select 等待数据
        int activity = select(serial_fd + 1, &read_fds, NULL, NULL, NULL);
        if (activity < 0)
        {
            perror("select error");
            break;
        }
        if (FD_ISSET(serial_fd, &read_fds))
        {
            int bytes_read = read(serial_fd, readBuffer, sizeof(readBuffer));

            if (bytes_read > 0)
            {
                total += bytes_read;
                addData(&array, readBuffer, bytes_read);
                memset(readBuffer, 0, sizeof(readBuffer));
            }
        }

        while (array.size >= FRAME_SIZE)
        {
            // 检查数组开头是否为帧头
            if (*(unsigned short *)array.buffer == FRAME_HEADER && check_sum(array.buffer))
            // if (*(unsigned short *)array.buffer==FRAME_HEADER)
            {
                // 测试
                BSB bsb;
                BSB_INIT(bsb, p, FRAME_SIZE);
                BSB_skip(bsb, 7);
                unsigned int time = 0;
                BSB_IMPORT_byte(bsb, &time, sizeof(time));
                // 转小端
                time = BSB_SWAP_ENDIAN(time, sizeof(unsigned int), unsigned int);
                printf("%d\n", time);
                fprintf(output_file, "%u\n", time);
                p += FRAME_SIZE;
                
                fre++;
                printf("array.size: %ld,fre: %d,total: %d\n",array.size,fre,total);
                // 移除这一帧数据
                removeFrame(&array, FRAME_SIZE);
            }
            else
            {
                // 不是帧头,移除第一个字节(继续从下一个字节开始判断)
                removeFrame(&array, 1);
            }
        }

        
    }
    // fclose(output_file);
    close(serial_fd);
    freeArray(&array);
    return 0;
}
#endif
int main()
{
    char *serial_device = "/dev/ttyUSB0";
    int baudrate = 115200;
    char *output_file_name = "test.txt";

    int serial_fd = open(serial_device, O_RDWR | O_NOCTTY | O_NDELAY);
    if (serial_fd == -1)
    {
        perror("Unable to open serial port");
        return 1;
    }

    int setret = 0;
    setret = set_opt(serial_fd, baudrate, 8, 'N', 1);

    if (setret == -1)
    {
        close(serial_fd);
        perror("set baudrate error\n");
    }

    // 打开输出文件
    FILE *output_file = fopen(output_file_name, "w");
    if (!output_file)
    {
        perror("Unable to open output file");
        close(serial_fd);
        return 1;
    }

    fd_set read_fds;
    char data_buf[1024] = {0};
    int logcount = 0;
    int writeflag = 0;
    BSB bsb;
    BSB_INIT(bsb, data_buf, sizeof(data_buf));

    char readBuffer[BUFFER_SIZE] = {0}; // 用于每次存储读取的数据

    while (1)
    {
        FD_ZERO(&read_fds);
        FD_SET(serial_fd, &read_fds);
        // 使用 select 等待数据
        int activity = select(serial_fd + 1, &read_fds, NULL, NULL, NULL);
        if (activity < 0)
        {
            perror("select error");
            break;
        }
        if (FD_ISSET(serial_fd, &read_fds))
        {
            int bytes_read = read(serial_fd, readBuffer, sizeof(readBuffer));

            if (bytes_read > 0)
            {
                // 缓存收到的数据,缓存结束清除缓冲区
                BSB_EXPORT_ptr(bsb, readBuffer, bytes_read);
                memset(readBuffer, 0, sizeof(readBuffer));
                if (BSB_LENGTH(bsb) >= 2 * FRAME_SIZE) // 至少有一帧数据
                {
                    int bsb_len = BSB_LENGTH(bsb);
                    int i = 0;
                    // 找到帧头  预期结果,开头即帧头
                    for (i = 0; i < bsb_len; i++)
                    {
                        if (*((unsigned short *)&bsb.buf[i]) == FRAME_HEADER)
                        {
                            break;
                        }
                    }
                    // 处理开头不是帧头情况
                    if (i != 0)
                    {
                        memmove(data_buf, &data_buf[i], bsb_len - i);
                        BSB_rewind(bsb, i);
                    }

                    bsb_len = BSB_LENGTH(bsb);

                    int bsb_for = bsb_len;

                    int parse_bytes = 0; // 记录解析完整帧的字节数。
                    char *p = bsb.buf;
                    // 解析缓存的数据
                    for (bsb_for; bsb_for >= FRAME_SIZE; bsb_for -= FRAME_SIZE)
                    {
                        // 解析一帧数据
                        if (*((unsigned short *)p) == FRAME_HEADER && check_sum(p))
                        {
                            // 测试
                            BSB bsb;
                            BSB_INIT(bsb, p, FRAME_SIZE);
                            BSB_skip(bsb, 7);
                            unsigned int time = 0;
                            BSB_IMPORT_byte(bsb, &time, sizeof(time));
                            // 转小端
                            time = BSB_SWAP_ENDIAN(time, sizeof(unsigned int), unsigned int);
                            printf("%d\n", time);
                            fprintf(output_file, "%u\n", time);
                            p += FRAME_SIZE;
                        }
                        parse_bytes += FRAME_SIZE;
                    }
                    // 循环退出时缓存的剩余不到一帧数据,缓存该半帧数据
                    if ((BSB_LENGTH(bsb) - parse_bytes) != 0)
                    {
                        int haf_ofset = 0;
                        haf_ofset = (BSB_LENGTH(bsb) - parse_bytes);
                        memmove(data_buf, &bsb.buf[parse_bytes], haf_ofset);
                    }
                    BSB_EXPORT_rewind(bsb, parse_bytes);
                }
            }
        }
    }
    fclose(output_file);
    close(serial_fd);
    return 0;
}
// 已知通信双方大小端一样时,无论什么类型数据,存取直接无脑BSB_IMPORT_byte、BSB_EXPORT_ptr
// 帧字段
// typedef struct
// {
// 	unsigned short head;
// 	unsigned char Fre_len;
// 	unsigned int Cnt;
// 	unsigned short GPS_week;
// 	double GPS_Seconds;
// 	double Imu_lon;
// 	double Imu_lat;
// 	float Imu_h;
// 	float Att_h;
// 	float Att_p;
// 	float Att_r;
// 	float Vel_e;
// 	float Vel_n;
// 	float Vel_u;
// 	float Meter;
// 	float Deviation;
// 	float BiasX;
// 	float BiasY;
// 	unsigned short check_sum;
// } Fre;
// BSB bsb;
// BSB_INIT(bsb, send_buf, FRAME_SIZE);
// BSB_EXPORT_ptr(bsb, &fre.head, sizeof(fre.head));               // 2 bytes for unsigned short
// BSB_EXPORT_ptr(bsb, &fre.Fre_len, sizeof(fre.Fre_len));         // 1 byte for unsigned char
// BSB_EXPORT_ptr(bsb, &fre.Cnt, sizeof(fre.Cnt));                 // 4 bytes for unsigned int
// BSB_EXPORT_ptr(bsb, &fre.GPS_week, sizeof(fre.GPS_week));       // 2 bytes for unsigned short
// BSB_EXPORT_ptr(bsb, &fre.GPS_Seconds, sizeof(fre.GPS_Seconds)); // 8 bytes for double
// BSB_EXPORT_ptr(bsb, &fre.Imu_lon, sizeof(fre.Imu_lon));         // 8 bytes for double
// BSB_EXPORT_ptr(bsb, &fre.Imu_lat, sizeof(fre.Imu_lat));         // 8 bytes for double
// BSB_EXPORT_ptr(bsb, &fre.Imu_h, sizeof(fre.Imu_h));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Att_h, sizeof(fre.Att_h));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Att_p, sizeof(fre.Att_p));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Att_r, sizeof(fre.Att_r));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Vel_e, sizeof(fre.Vel_e));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Vel_n, sizeof(fre.Vel_n));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Vel_u, sizeof(fre.Vel_u));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Meter, sizeof(fre.Meter));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.Deviation, sizeof(fre.Deviation));     // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.BiasX, sizeof(fre.BiasX));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.BiasY, sizeof(fre.BiasY));             // 4 bytes for float
// BSB_EXPORT_ptr(bsb, &fre.check_sum, sizeof(fre.check_sum));     // 2 bytes for unsigned short

// BSB bsb;
// BSB_INIT(bsb, array.buffer, FRAME_SIZE);
// BSB_IMPORT_byte(bsb, &fre->head, sizeof(fre->head));		  // 2 bytes for unsigned short
// BSB_IMPORT_byte(bsb, &fre->Fre_len, sizeof(fre->Fre_len)); // 1 byte for unsigned char
// BSB_IMPORT_byte(bsb, &fre->Cnt, sizeof(fre->Cnt));				  // 4 bytes for unsigned int
// BSB_IMPORT_byte(bsb, &fre->GPS_week, sizeof(fre->GPS_week));		  // 2 bytes for unsigned short
// BSB_IMPORT_byte(bsb, &fre->GPS_Seconds, sizeof(fre->GPS_Seconds)); // 8 bytes for double
// BSB_IMPORT_byte(bsb, &fre->Imu_lon, sizeof(fre->Imu_lon)); // 8 bytes for double
// BSB_IMPORT_byte(bsb, &fre->Imu_lat, sizeof(fre->Imu_lat)); // 8 bytes for double
// BSB_IMPORT_byte(bsb, &fre->Imu_h, sizeof(fre->Imu_h));	  // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Att_h, sizeof(fre->Att_h)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Att_p, sizeof(fre->Att_p)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Att_r, sizeof(fre->Att_r)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Vel_e, sizeof(fre->Vel_e)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Vel_n, sizeof(fre->Vel_n)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Vel_u, sizeof(fre->Vel_u)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Meter, sizeof(fre->Meter));		  // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->Deviation, sizeof(fre->Deviation)); // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->BiasX, sizeof(fre->BiasX));		  // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->BiasY, sizeof(fre->BiasY));		  // 4 bytes for float
// BSB_IMPORT_byte(bsb, &fre->check_sum, sizeof(fre->check_sum)); // 2 bytes for unsigned short

菜白★
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值