基于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);
		}
	}
	else
		sbg_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 up
	u16 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 axis
   u8    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变量就是我们要的值。