nav982学习（2）

最新推荐文章于 2022-08-25 10:49:38 发布

火柴的初心

最新推荐文章于 2022-08-25 10:49:38 发布

阅读量624

点赞数 1

分类专栏： ROS

本文链接：https://blog.csdn.net/He3he3he/article/details/82418298

版权

ROS 专栏收录该内容

15 篇文章 1 订阅

订阅专栏

1.原始数据包

1.1原始数据包的结构体

typedef struct
{
	float accelerometers[3];
	float gyroscopes[3];
	float magnetometers[3];
	float imu_temperature;
	float pressure;
	float pressure_temperature;
} raw_sensors_packet_t;

其中：

accelerometers：加速度计	gyroscopes：陀螺仪	magnetometers：磁力计
imu_temperature：最低温度	pressure_temperature：压力温度	pressure：压力

2. 串口和波特率的选择

	if (OpenComport(argv[1], atoi(argv[2])))
	{
		printf("Could not open serial port\n");
		exit(EXIT_FAILURE);
	}

第一个参数为指向字符串的指针，第二个参数为整型的波特率

int OpenComport(char *comport, int baudrate)
{
  int baudr;

  switch(baudrate)/*选择波特率*/
  {
    case      50 : baudr = B50;
                   break;
    case      75 : baudr = B75;
                   break;
    case     110 : baudr = B110;
                   break;
    case     134 : baudr = B134;
                   break;
    case     150 : baudr = B150;
                   break;
    case     200 : baudr = B200;
                   break;
    case     300 : baudr = B300;
                   break;
    case     600 : baudr = B600;
                   break;
    case    1200 : baudr = B1200;
                   break;
    case    1800 : baudr = B1800;
                   break;
    case    2400 : baudr = B2400;
                   break;
    case    4800 : baudr = B4800;
                   break;
    case    9600 : baudr = B9600;
                   break;
    case   19200 : baudr = B19200;
                   break;
    case   38400 : baudr = B38400;
                   break;
    case   57600 : baudr = B57600;
                   break;
    case  115200 : baudr = B115200;
                   break;
    case  230400 : baudr = B230400;
                   break;
    case  460800 : baudr = B460800;
                   break;
    case  500000 : baudr = B500000;
                   break;
    case  576000 : baudr = B576000;
                   break;
    case  921600 : baudr = B921600;
                   break;
    case 1000000 : baudr = B1000000;
                   break;
    default      : printf("invalid baudrate\n");
                   return(1);
                   break;
  }

  Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY);
  if(Cport==-1)
  {
    perror("unable to open comport ");
    return(1);
  }

  error = tcgetattr(Cport, &old_port_settings);
  if(error==-1)
  {
    close(Cport);
    perror("unable to read portsettings ");
    return(1);
  }
  memset(&new_port_settings, 0, sizeof(new_port_settings));  /* clear the new struct */

  new_port_settings.c_cflag = baudr | CS8 | CLOCAL | CREAD;
  new_port_settings.c_iflag = IGNPAR;
  new_port_settings.c_oflag = 0;
  new_port_settings.c_lflag = 0;
  new_port_settings.c_cc[VMIN] = 0;      /* block untill n bytes are received */
  new_port_settings.c_cc[VTIME] = 0;     /* block untill a timer expires (n * 100 mSec.) */
  error = tcsetattr(Cport, TCSANOW, &new_port_settings);
  if(error==-1)
  {
    close(Cport);
    perror("unable to adjust portsettings ");
    return(1);
  }

  return(0);
}

3.解码初始化

an_decoder_initialise(&an_decoder);/*解码初始化*/

void an_decoder_initialise(an_decoder_t *an_decoder)
{
	an_decoder->buffer_length = 0;
	an_decoder->crc_errors = 0;
}

typedef struct
{
	uint8_t buffer[AN_DECODE_BUFFER_SIZE];
	uint16_t buffer_length;
	uint32_t crc_errors;
} an_decoder_t;

其中：形参为一个指向结构体的指针，传入的为&an_decoder（地址），an_decoder->buffer_length相当于（*an_decoder）.buffer_length。

注释：由于点（.）的优先级高于*，所以要加括号。

an_decoder_t：解码包

AN_DECODE_BUFFER_SIZE：25

4.while循环

while (1)
{    /*返回缓存区接受到的字节数*/
	if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0)
	{
		/* 通过接收的字节数来增加解码缓冲区长度 */
		an_decoder_increment(&an_decoder, bytes_received);
			
		/* 对缓冲区中的所有数据包进行解码 */
		while ((an_packet = an_packet_decode(&an_decoder)) != NULL)
		{
			if (an_packet->id == packet_id_system_state) /* system state packet */
			{
				/* copy all the binary data into the typedef struct for the packet */
				/* this allows easy access to all the different values             */
				if(decode_system_state_packet(&system_state_packet, an_packet) == 0)
				{
					printf("System State Packet:\n");
					printf("\tLatitude = %f, Longitude = %f, Height = %f\n", system_state_packet.latitude * RADIANS_TO_DEGREES, system_state_packet.longitude * RADIANS_TO_DEGREES, system_state_packet.height);
					printf("\tRoll = %f, Pitch = %f, Heading = %f\n", system_state_packet.orientation[0] * RADIANS_TO_DEGREES, system_state_packet.orientation[1] * RADIANS_TO_DEGREES, system_state_packet.orientation[2] * RADIANS_TO_DEGREES);
				}
			}
			else if (an_packet->id == packet_id_raw_sensors) /* raw sensors packet */
			{
				/* copy all the binary data into the typedef struct for the packet */
				/* this allows easy access to all the different values             */
				if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0)
				{
					printf("Raw Sensors Packet:\n");
					printf("\tAccelerometers X: %f Y: %f Z: %f\n", raw_sensors_packet.accelerometers[0], raw_sensors_packet.accelerometers[1], raw_sensors_packet.accelerometers[2]);
					printf("\tGyroscopes X: %f Y: %f Z: %f\n", raw_sensors_packet.gyroscopes[0] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[1] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[2] * RADIANS_TO_DEGREES);
				}
			}
			else
			{
				printf("Packet ID %u of Length %u\n", an_packet->id, an_packet->length);
			}
				
			/* Ensure that you free the an_packet when your done with it or you will leak memory */
			an_packet_free(&an_packet);
		}
	}
}

4.1 pollcomport函数(rs232.h)

int PollComport(unsigned char *buf, int size)
{
  int n;

#ifndef __STRICT_ANSI__                       /* __STRICT_ANSI__ is defined when the -ansi option is used for gcc */
  if(size>SSIZE_MAX)  size = (int)SSIZE_MAX;  /* SSIZE_MAX is defined in limits.h */
#else
  if(size>4096)  size = 4096;
#endif

  n = read(Cport, buf, size);

  return(n);
}

其中：

1.Buf是指向缓冲区的指针，并以字节为单位调整缓冲区的大小，返回缓冲区中接收的字符数。

2.这可能小于缓存区大小或则为零！它不会阻塞或等待，无论是否收到任何字符，它都会立即返回。

3.成功打开COM端口后，将此功能连接到计时器，计时器的间隔应约为：100毫秒。

4.在关闭COM端口之前，不要忘记停止计时器。

5.实际参数为两个宏定义，如下所示：

#define an_decoder_pointer(an_decoder) &(an_decoder)->buffer[(an_decoder)->buffer_length]
#define an_decoder_size(an_decoder) (sizeof((an_decoder)->buffer) - (an_decoder)->buffer_length)

第一个宏定义传入的buffer数组的首地址，第二个宏定义为缓存区的大小。

4.2 an_decoder_increment函数

#define an_decoder_increment(an_decoder, bytes_received) (an_decoder)->buffer_length += bytes_received

其中：

1.该函数的功能是通过接收的字节数来增加解码缓冲区长度

2.实际参数为an_decoder的地址，因此(an_decoder)->buffer_length相当于*（&an_decoder).buffer_length。

4.3 an_packet_decode()解原始数据包函数

an_packet_t *an_packet_decode(an_decoder_t *an_decoder)
{
	uint16_t decode_iterator = 0;
	an_packet_t *an_packet = NULL;
	uint8_t header_lrc, id, length;
	uint16_t crc;

	while (decode_iterator + AN_PACKET_HEADER_SIZE <= an_decoder->buffer_length)
	{
		header_lrc = an_decoder->buffer[decode_iterator++];
		if (header_lrc == calculate_header_lrc(&an_decoder->buffer[decode_iterator]))
		{
			id = an_decoder->buffer[decode_iterator++];
			length = an_decoder->buffer[decode_iterator++];
			crc = an_decoder->buffer[decode_iterator++];
			crc |= an_decoder->buffer[decode_iterator++] << 8;

			if (decode_iterator + length > an_decoder->buffer_length)
			{
				decode_iterator -= AN_PACKET_HEADER_SIZE;
				break;
			}

			if (crc == calculate_crc16(&an_decoder->buffer[decode_iterator], length))
			{
				an_packet = an_packet_allocate(length, id);
				if (an_packet != NULL)
				{
					memcpy(an_packet->header, &an_decoder->buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE * sizeof(uint8_t));
					memcpy(an_packet->data, &an_decoder->buffer[decode_iterator], length * sizeof(uint8_t));
				}
				decode_iterator += length;
				break;
			}
			else
			{
				decode_iterator -= (AN_PACKET_HEADER_SIZE - 1);
				an_decoder->crc_errors++;
			}
		}
	}
	if (decode_iterator < an_decoder->buffer_length)
	{
		if (decode_iterator > 0)
		{
			memmove(&an_decoder->buffer[0], &an_decoder->buffer[decode_iterator], (an_decoder->buffer_length - decode_iterator) * sizeof(uint8_t));
			an_decoder->buffer_length -= decode_iterator;
		}
	}
	else an_decoder->buffer_length = 0;

	return an_packet;
}

其中：

1.该函数为指针函数，功能是将原始数据解码为一个包，并且返回包的地址。

2.如果没有解码包，将返回NULL。

4.4 decode_system_state_packet（）解系统状态包函数

int decode_system_state_packet(system_state_packet_t *system_state_packet, an_packet_t *an_packet)
{
	if(an_packet->id == packet_id_system_state && an_packet->length == 100)
	{
		memcpy(&system_state_packet->system_status, &an_packet->data[0], sizeof(uint16_t));
		memcpy(&system_state_packet->filter_status, &an_packet->data[2], sizeof(uint16_t));
		memcpy(&system_state_packet->unix_time_seconds, &an_packet->data[4], sizeof(uint32_t));
		memcpy(&system_state_packet->microseconds, &an_packet->data[8], sizeof(uint32_t));
		memcpy(&system_state_packet->latitude, &an_packet->data[12], sizeof(double));
		memcpy(&system_state_packet->longitude, &an_packet->data[20], sizeof(double));
		memcpy(&system_state_packet->height, &an_packet->data[28], sizeof(double));
		memcpy(&system_state_packet->velocity[0], &an_packet->data[36], 3 * sizeof(float));
		memcpy(&system_state_packet->body_acceleration[0], &an_packet->data[48], 3 * sizeof(float));
		memcpy(&system_state_packet->g_force, &an_packet->data[60], sizeof(float));
		memcpy(&system_state_packet->orientation[0], &an_packet->data[64], 3 * sizeof(float));
		memcpy(&system_state_packet->angular_velocity[0], &an_packet->data[76], 3 * sizeof(float));
		memcpy(&system_state_packet->standard_deviation[0], &an_packet->data[88], 3 * sizeof(float));
		return 0;
	}
	else return 1;
}

其中：

1.将所有二进制数据复制到该包中。

2.系统状态数据包的ID为20

火柴的初心

关注

1
点赞
踩
3

收藏

觉得还不错? 一键收藏
打赏
1
评论
nav982学习（2）

1.原始数据包 1.1原始数据包的结构体typedef struct{ float accelerometers[3]; float gyroscopes[3]; float magnetometers[3]; float imu_temperature; float pressure; float pressure_temperature;} raw_sensors...
复制链接

扫一扫