基于STM32读取SBG Ellipse A型号惯导数据

之前在飞控上用的都是mpu6050，但AUV在执行任务时主要在水下环境，收不到GPS信号，因此对INS的精度要求较高，在姿态解算时要考虑地球自转等因素，因此需要一款惯导器件能够感受到地球自转，经测量，SBG公司生产的Ellipse A并不能满足要求。下面介绍一下采集系统的软件架构。

组合导航系统需要同时采集多种传感器，进行多传感器融合，以提高导航系统精度。初步准备用SBG和GPS进行融合，将GPS的更新频率降低到1Hz以模拟USBL的水环境状况，本篇文章仅介绍读取SBG部分。

SBG传感器使用的接口是RS232，说到此惭愧不已，因为学术不精，最开始直接用USB转TTL线来读数据，结果在串口助手上看到的全是乱码。

虽然都是串口设备，RS-232的通讯电平：逻辑1是-15V~-3V，逻辑0是+3V~+15V，而TTL的通讯电平：逻辑1是2.4V~5V，逻辑0是0~0.5V。从电压等级也可以看出RS-232的远距离传输和抗干扰能力更强一些。

下面看一下sbg的协议格式

 除了帧头以外，协议规定了消息ID和消息类，简单的说，消息类用于区分消息内容是传感器数据还是需要执行的命令，消息ID中将数据进行了分类，我只需要加速度计测得的加速度值和陀螺仪测得的角速度值。

读取数据协议的策略是这样的，当传感器向stm32的串口发送数据时，就会使能相应的串口中断，每中断一次接收1个字节，因此在中断函数中需要一个预处理部分，将需要解读的一组完整的imu数据存入缓存区。中断函数如下：

void USART2_IRQHandler(void)
{uint8_t ch;if(USART_GetITStatus(USART2,USART_IT_RXNE)) {USART_ClearITPendingBit(USART2, USART_IT_RXNE);	ch=USART_ReceiveData(USART2);SBG_Data_Receive_Prepare(ch);}	 
}

最初我犯了一个低级错误，将读取数据分成了两部分，一部分是将数据存入缓存区，第二部分是将缓存区数据按协议解析。这本没有问题，但是我将第一部分放在了串口中断中，将第二部分放在了定时器中断中每5ms解析一次。虽听起来不错，但有个严重的问题，当协议解析的时候并不能保证缓存区中是一组完整的数据，这就导致了读出来的数每隔几十个就会有组极大值。

正确的做法是将两部分写在同一流程中，也就是一起放到串口中断中，待数组存放完全，再去解析，而定时器则每5ms调用一次解析出的数据进行融合等操作，这样就没有问题了~

下面为两部分的函数实现：

数据预处理

extern uint8_t SBG_Data[100];
u8 sbg_data_state = 0;
u16 _data_len = 0,_data_cnt = 0;
void SBG_Data_Receive_Prepare(u8 data)
{//static u16 _data_len = 0,_data_cnt = 0;//static u8 sbg_data_state = 0;if(sbg_data_state==0&&data==0xFF) {sbg_data_state=1;_data_cnt = 0;SBG_Data[_data_cnt]=data;//0_data_cnt++;//1}else if(sbg_data_state==1&&data==0x5A) {sbg_data_state=2;SBG_Data[_data_cnt]=data;//1_data_cnt++;//2}else if(sbg_data_state==2&&data==0x03) {sbg_data_state=3;SBG_Data[_data_cnt]=data;//2_data_cnt++;//3}else if(sbg_data_state==3&&data==0x00) {sbg_data_state=4;SBG_Data[_data_cnt]=data;//3_data_cnt++;//4}else if(sbg_data_state==4&&_data_cnt<67) {SBG_Data[_data_cnt]=data;_data_cnt++;if(_data_cnt==67) {GET_SBG_DATA(SBG_Data);}}elsesbg_data_state = 0;
}

union 
{float a;uint8_t b[4];
} hex_receive;
float hex2flo(uint8_t *buffer)
{int i = 0;for(i = 0; i < 4; i++) hex_receive.b[i] = *(buffer+i);return hex_receive.a;
}/*!
* Compute a CRC for a specified buffer.
* \param[in] pBuffer Read only buffer to compute the CRC on.
* \param[in] bufferSize Buffer size in bytes.
* \return The computed 16 bit CRC.
*/
u16 calcCRC(const void *pBuffer, u16 bufferSize)
{const u8 *pBytesArray = (const u8*)pBuffer;u16 poly = 0x8408;u16 crc = 0;u8 carry;u8 i_bits;u16 j;for (j =0; j < bufferSize; j++){crc = crc ^ pBytesArray[j];for (i_bits = 0; i_bits < 8; i_bits++){carry = crc & 1;crc = crc / 2;if (carry){crc = crc^poly;}}}return crc;
}
/******************»ñÈ¡SBGÊý¾Ý******************/u16 SBG_Cnt = 0;u32 TIME_STAMP; //Time since sensor is powered upu16 IMU_STATUS; //IMU Status bitmask
float ACCEL_X, ACCEL_Y, ACCEL_Z;   //Filtered Accelerometer - X axis
float GYRO_X, GYRO_Y, GYRO_Z;  //Filtered Gyroscope - X axis
float ACCEL_X_past, ACCEL_Y_past, ACCEL_Z_past;   //Filtered Accelerometer - X axis
float GYRO_X_past, GYRO_Y_past, GYRO_Z_past;  //Filtered Gyroscope - X axisu8    state_ins = 0;//0±íʾpastδ¸üÐÂ
float TEMP;  //Internal Temperature
float DELTA_VEL_X, DELTA_VEL_Y, DELTA_VEL_Z;  //Sculling output - X axis
float DELTA_ANGLE_X, DELTA_ANGLE_Y, DELTA_ANGLE_Z;   //Coning output - X axis
float ROLL, PITCH, YAW;
float Q0, Q1, Q2, Q3;
float MAG_X, MAG_Y, MAG_Z;u16 CRC_Num;u16 CRC_Receive;u16 Data_Len;
void GET_SBG_DATA(uint8_t *data_buf)//´«ÈëSBG_Data[0]
{/*IEEE754: 1λ·ûºÅ룬8λָÊý£¬23λβÊý(СÊý²¿·Ö)*/if(!(*(data_buf+0)==0xFF && *(data_buf+1)==0x5A)) return;Data_Len = ((vs16)(*(data_buf+5)<<8)|*(data_buf+4));if(*(data_buf+6+Data_Len+2)!=0x33) return;/******************************************************************************** @name    SBG_ECOM_LOG_IMU_DATA* @MSGid   0x03* @brief   Includes IMU status, acc., gyro, temp delta speeds and delta angles values* @ps      Just DATA, no CRC, ETX*******************************************************************************/if(*(data_buf+2)==0x03 && *(data_buf+3)==0x00) {CRC_Num       =  calcCRC((data_buf+6), Data_Len);CRC_Receive   =  ((vs16)(*(data_buf+65)<<8)|*(data_buf+64));//		if(CRC_Num == CRC_Receive) {TIME_STAMP    =  ((vs32)(*(data_buf+9)<<24)|(*(data_buf+8)<<16)|(*(data_buf+7)<<8)|(*(data_buf+6)));IMU_STATUS    =  ((vs16)(*(data_buf+11)<<8)|*(data_buf+10));ACCEL_X       =  hex2flo(data_buf+12);ACCEL_Y       =  hex2flo(data_buf+16);ACCEL_Z       =  hex2flo(data_buf+20);GYRO_X        =  hex2flo(data_buf+24);GYRO_Y        =  hex2flo(data_buf+28);GYRO_Z        =  hex2flo(data_buf+32);TEMP          =  hex2flo(data_buf+36);DELTA_VEL_X   =  hex2flo(data_buf+40);DELTA_VEL_Y   =  hex2flo(data_buf+44);DELTA_VEL_Z   =  hex2flo(data_buf+48);DELTA_ANGLE_X =  hex2flo(data_buf+52);DELTA_ANGLE_Y =  hex2flo(data_buf+56);DELTA_ANGLE_Z =  hex2flo(data_buf+60);}/******************************************************************************** @name    SBG_ECOM_LOG_EKF_EULER* @MSGid   0x06* @brief   Includes roll, pitch, yaw and their accuracies on each axis*******************************************************************************/else if(*(data_buf+2)==0x06 && *(data_buf+3)==0xFF) {//LEN:data_buf+4   data_buf+5 //TIME_STAMP = ((vs32)(*(data_buf+6)<<24)|(*(data_buf+7)<<16)|(*(data_buf+8)<<8)|(*(data_buf+9)));//IMU_STATUS = ((vs16)(*(data_buf+10)<<8)|*(data_buf+11));ROLL   =  hex2flo(data_buf+10);PITCH  =  hex2flo(data_buf+14);YAW    =  hex2flo(data_buf+18);}/******************************************************************************** @name    SBG_ECOM_LOG_EKF_QUAT* @MSGid   0x07* @brief   Includes the 4 quaternions values*******************************************************************************/else if(*(data_buf+2)==0x07 && *(data_buf+3)==0xFF) {Q0 = hex2flo(data_buf+10);Q1 = hex2flo(data_buf+14);Q2 = hex2flo(data_buf+18);Q3 = hex2flo(data_buf+22);}/******************************************************************************** @name    SBG_ECOM_LOG_MAG* @MSGid   0x04* @brief   Magnetic data with associated accelerometer on each axis*******************************************************************************/else if(*(data_buf+2)==0x04 && *(data_buf+3)==0xFF) {MAG_X = hex2flo(data_buf+12);MAG_Y = hex2flo(data_buf+16);MAG_Z = hex2flo(data_buf+20);}else {SBG_Cnt = 0;}}

值得注意的是，在解析时，我们应该注意大小端，即传感器先发送的字节是高8位还是低8位。第二个需要注意的点就是，SBG传输的float数据格式是IEEE754格式，因此并不能用位操作去直接合并，一个小技巧是使用union联合体，在联合体中定义一个float和一个有4个元素的字符数组，这样当我们把收到的字节依次传入数组中后，float变量就是我们要的值。