机器人数据协议解析

落日映苍穹つ 2023-02-21 11:50 15阅读 0赞

#include "algorithm.h"
    
    
    unsigned char xueya_init[6]={ 0xfd,0,0,0,0,0};
    
    /**********在一个字符串中判断是否有另外一个字符串******/
    /*************/
    /* arr1[]:被查询的大数组 length1：大数组的长度 */
    /* arr2[]:要查找的小数组 length1：小数组的长度 */
    /* 返回值 ： -1 表示没找到 */
    /* 其他值表示找到了，放回index */
    ///
    int find_arr(unsigned char *arr1, int length1,unsigned char *arr2, int length2)
    { 
    	int i = 0, j = 0;
    	
    	if(length1<length2)
    	{ 
    		return -1 ;
    	}
    	for (j = 0; j < length2; )
    	{ 
    		if (arr1[i] == arr2[j])
    		{ 
    			j++;
    			i++;
    			if (i > length1)
    				return -1;
    		}
    		else
    		{ 
    			j = 0;
    			i++;
    		}
    	}
    	return i - length2;
    }
    
    /**********拷贝一个字符串到obj数组中******/
    /*************/
    /* source[]:被拷贝的大数组 length1：大数组的长度 */
    /* arr2[]:要拷贝存放的小数组 length：要拷贝的长度 */
    /* 返回值 ： -1 表示没找到 */
    /* 其他值表示找到了，返回index */
    ///
    int str_copy(unsigned char *source ,unsigned char *obj , int length)
    { 
    		int i=0 ;
    		for(i=0;i<length;i++)
    		{ 
    			obj[i]=source[i];
    		}
    		return 1 ;
    }
    //
    //
    // 获取的数字无效
    struct Health_DATA health_data = {  0 }; //定义一个健康数据结构
    struct Leida_Distance_DATA leida_distance={ 0};
    
    
    // arr1:被解析的字符串 length：被解析的字符串长度 */
    // 数据获取和解析函数 */
    // 生物雷数据获取 */
    // 超声波雷达数据获取 */
    //返回值：return ： -1 表示数据握手失败 */
    // 其他,数据接收成功 */
    
    int data_Read(unsigned char *arr1, int length)
    { 
    	int index = -1;
    	unsigned char*arr = arr1;
    	unsigned char data_header[2] = {  0x66,0xcc };//起始包头
    	if (length < 3)
    	{ 
    		return -1;
    	}
    	index = find_arr(arr, length, data_header, 2);//找到数组中的包头
    	if (index != -1) 
    	{ 
    		printf("握手成功！\r\n");
    	}
    	else
    	{ 
    		printf("握手失败！\r\n");
    		return -1 ;
    	}
    	arr = arr+index;//数据地址偏移
    	
    	int package_length = arr[index + 2];
    	if (length < 3 + package_length + 1)//数据包不完整，接收的数据出错
    	{ 
    		return -1;
    	}
    	int package_ID = arr[3];
    	if (package_ID == 0x01) //生物雷达数据
    	{ 
    		health_data.heartBeat = arr[4];
    		health_data.breath = arr[5];
    		health_data.temperature = arr[6] * 256 + arr[7] * 0.1;
    		health_data.bloodPressure_H = arr[8];
    		health_data.bloodPressure_L = arr[9];
    		health_data.bloodOxygen = arr[10];
    		printf("生物雷达数据更新成功！\r\n");
    		printf("%d %d %d %d %d %d \r\n", health_data.heartBeat, health_data.breath, 
    		health_data.temperature, health_data.bloodPressure_H, health_data.bloodPressure_L, health_data.bloodOxygen);
    	}
    	else if(package_ID==0x02) //超声波数据
    	{ 
    		leida_distance.dis_Left =arr[4]*256+arr[5];
    		leida_distance.dis_Right=arr[6]*256+arr[7];
    		leida_distance.dis_Ahead=arr[8]*256+arr[9];
    		leida_distance.dis_Back	=arr[10]*256+arr[11];
    		printf("超声波雷达数据更新成功");
    		printf("%d %d %d %d \r\n",leida_distance.dis_Left,leida_distance.dis_Right,leida_distance.dis_Ahead,leida_distance.dis_Back);
    	}
    	return 1;
    }

.h文件

#ifndef __ALGORITHM_H
    #define __ALGORITHM_H
    
    #include "stm32f1xx_hal.h"
    #include "uart.h"
    
    struct Health_DATA
    { 
    	uint8_t heartBeat ;
    	uint8_t breath		;
    	uint16_t temperature;
    	uint8_t bloodPressure_H;
    	uint8_t bloodPressure_L;
    	uint8_t bloodOxygen;
    };
    struct Leida_Distance_DATA
    { 
    	uint16_t dis_Left;
    	uint16_t dis_Right;
    	uint16_t dis_Ahead;
    	uint16_t dis_Back;
    };
    
    extern unsigned char xueya_init[6] ;
    
    extern struct Health_DATA health_data ; //定义一个健康数据结构
    extern struct Leida_Distance_DATA leida_distance;//定义一个超声波数据结构
    
    
    int find_arr(unsigned char *arr1, int length1, unsigned char *arr2, int length2);
    int str_copy(unsigned char *source ,unsigned char *obj , int length);
    int data_Read(unsigned char *arr1, int length);
    
    #endif