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_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
