STM32实现水下四旋翼（七）传感任务3——MS5837水深传感器读水深（超详细）

一. MS5837-30BA水深传感器解读

1. 简介

无人机实现定高飞行要依靠高度数据，那么水下航行器想要实现定深航行同样需要垂直方向的位置数据，也就是深度，我们使用MS5837-30BA这款压力传感器来测量水深。

关于MS5837的使用网上有很多教程，其实都不用看，看数据手册就行了，写的非常非常全面。什么？看不懂英文？好的我已经把它翻译了一遍，还加上了博主的解说，可以说是非常非常详细了，相信我，你看了我的数据手册，就会写驱动程序了。中英文数据手册和驱动代码已上传，可点击下载.

2. 性能参数

电气特性，性能特征等请看手册，我们只关注算法。

MS5837可以配置成不同的ADC转换精度，用OSR表示，精度越高转换越慢，下表显示了不同精度对应的转换时间。每次读ADC转换数据前要先发送转换命令，必须要按照最大的转换时间去写一个延时，延时时间够了才能继续发送读转换完成的数据，不然数据是错的。比如OSR=4096时转换时间是7.40-9.04，那么延时时间可以设置成10ms.

3. 典型电路

MS5837使用IIC通讯，典型应用电路如下图。经典的IIC通讯电路。

4. 深度数据解算算法

（1） 一阶算法

数据解算算法说明书里给了流程图

翻译成算法语言：

1） 开始。限定了工作条件，压力测量值0~ 30Bar，温度值范围-20~85℃，参考温度20℃。这个参考温度会用到。

2） 从PROM中读校准参数。一共六个值C1-C6，这组值出厂已经校准过了。其物理含义嘛请看英文表述，不懂也没关系，我们会计算就行了。后面给了推荐的数据类型都为unsigned int16，数据位数16位，数值范围0~65536，最后一列是示例值。这组校准参数是每次上电后必须从传感器内部的PROM中读取一次的，这6个数据C1-C6后面解算要用。

3）读数字压力D1与数字温度和D2，这是MS5837输出的24位模数转换值。推荐数据类型unsigned int32。这两个数据是原始数据，还不准，需要经过C1-C6这组校准参数去校准。

4） 计算温度。

计算DT（测量值与参考值之差）：$dT=D2-T_{REF}=D2-C5\cdot 2^8$

计算实际温度TEMP：$TEMP=20+dT\cdot TEMPSENS=2000+dT\cdot C6/2^{23}$

好的，到这里读到校准后的温度值了。

5） 计算温度补偿后的压力。

计算实际温度下的偏差：$OFF=OF{F}_{T1}+TCO\cdot dT=C2\cdot 2^{16}+(C4\cdot dT)/2^7$

计算实际温度下的灵敏度：$SENS=SEN{S}_{T1}+TCS\cdot dT=C1\cdot 2^{15}+(C3\cdot dT)/2^8$

计算温度补偿后的压力：$P=D1\cdot SENS-OFF=(D1\cdot SENS/2^{21}-OFF)/2^{13}$

提到了一堆物理命名，不用管，应用公式去计算就行了。好的，到这里我们得到温度补偿后的压力值了。

（2） 二阶算法

上面使用的是一阶温度补偿算法，说明书里给了二阶温度补偿后的算法流程图：

相应算法在框图中表示的很清楚哈，命名都为你命好了。最后得到的结果就是TEMP2和P2，也就是二阶算法计算的温度和压力。

5. IIC读数据的寄存器与时序

仔细看看，算法其实很明了，数据的类型，命名都按说明书里推荐的来就行了。那么现在我们只关注怎么去读数据，IIC读数据的关键，一是寄存器地址，二是通信时序。 说明书里写的很详细。

（1）MS5837命令

MS5837只有5个基本命令：

1. 复位
2. 读取出厂校准值C1-C6
3. 数据D1转换（压力值数据）
4. 数据D2转换（温度值数据）
5. 读取ADC的转换结果（24位温度值与压力值）

每条I2C通信消息以启动条件开始，以停止条件结束。MS5837-30BA地址是1110110x(写:x=0，读:x=1)。即写0xEC，读0xED.

（2）寄存器地址

寄存器地址在下表给出

五个命令对应的寄存器地址显示的很详细了。

1. 复位命令寄存器是0x1E ;
2. 读取出厂校准值C1-C6，看最后一栏，bit4-bit6为000-111,C1-C6对应的地址为0xA2-0xAC;
3. 数据D1转换（压力值数据），注意这是个写命令。这里不同的精度对应地址不一样，对应选择就行，比如OSR=4096，地址为0x48;
4. 数据D2转换（温度值数据），注意这是个写命令。这里不同的精度对应地址不一样，对应选择就行，比如OSR=4096，地址为0x58;
5. 读转换结果，地址0x00.

（3）通信时序

进一步，继续看通信时序，这里一步都不能错，包括有没有应答信号。

复位命令

写代码的时候按照其时序进行就可以了，示例如下：

IIC_Start();
IIC_Send_Byte(0xEC); // 发送写命令，读命令是0xEC+1=0xED
IIC_Wait_Ack();
IIC_Send_Byte(0x1E); //发送复位命令
IIC_Wait_Ack();
IIC_Stop();

读PROM指令（读校准系数C1-C6）

PROM的读取命令由用户在复位后执行一次。校准参数存储在7个地址中，占据112bits的内存空间（每个数据16位）。读PROM命令是8bits，返回数据16bits（MSB模式，也就是高位在低地址）。PROM读指令包含两步，第一步设置系统进入PROM读模式，第二步读返回结果。

以读C3为例：寄存器地址0xA0+3*2

MS5837返回数据位16位：

这一段同样的，写代码的时候按照其时序进行就可以了，注意前面提到了校准系数有6个，依次读六次就行了。示例如下：

for (i = 1; i <= 6; i++)
{
     IIC_Start();
     IIC_Send_Byte(0xEC);
     IIC_Wait_Ack();
     IIC_Send_Byte(0xA0 + (i * 2));  
     IIC_Wait_Ack();
     IIC_Stop();
     delay_us(5);
     IIC_Start();
     IIC_Send_Byte(0xEC + 0x01); //进入接收模式
     delay_us(1);
     IIC_Wait_Ack();
     inth = IIC_Read_Byte(1); //带ACK的读数据
     delay_us(1);
     intl = IIC_Read_Byte(0); //最后一个字节NACK
     IIC_Stop();
     Cal_C[i] = (((uint16_t)inth << 8) | intl);  
}

好的，现在我们会读PROM中的校准系数了。

数据D1/D2转换

转换命令用于启动未补偿的压力(D1)或未补偿的温度(D2)转换。经过转换之后，使用ADC读命令，结果以MSB模式输出。注意了，每次必须先发送转换命令，而且需要等转换完成，再发送读ADC命令，这样就能读到正确的D1或D2数据。

比如 分辨率=4096，数据类型=D1，

IIC指令启动压力转换（地址0x48）

读ADC数据

等待10ms等转换完成（转换时间查表），转换完成之后发送ADC读命令（0x00）

MS5837的返回数据为24位：

好的，用代码表示上面三步的时序

    IIC_Start();
    IIC_Send_Byte(0xEC); //写地址
    IIC_Wait_Ack();
    IIC_Send_Byte(0x48); //写转换命令，0x48是读D1，0x58是读D2
    IIC_Wait_Ack();
    IIC_Stop();

    delay_ms(10); // 等待装换完成

    IIC_Start();
    IIC_Send_Byte(0xEC); //写地址
    IIC_Wait_Ack();
    IIC_Send_Byte(0); // start read sequence
    IIC_Wait_Ack();
    IIC_Stop();

    IIC_Start();
    IIC_Send_Byte(0xEC + 0x01); //进入接收模式
    IIC_Wait_Ack();
    temp[0] = IIC_Read_Byte(1); //带ACK的读数据  bit 23-16
    temp[1] = IIC_Read_Byte(1); //带ACK的读数据  bit 8-15
    temp[2] = IIC_Read_Byte(0); //带NACK的读数据 bit 0-7
    IIC_Stop();

好的，现在我们会读D1和D2了，结合前面读到的C1-C6，就可以开心的使用算法流程用一阶算法或二阶算法计算温度与压力了，知道了压力就可以计算深度了。

二. STM32读取水深传感器驱动程序

经过上面的解读，相信你已经啥都会了，现在一起来写一下驱动代码。

建立一个MS5837.h和MS5837.c的文件（IIC的驱动就略去不贴了，这里我用的IIC0用以与IIC区分）

MS5837.h文件内容：

#ifndef __MS5837_H_
#define __MS5837_H_
#include "sys.h"

void MS5837_init(void);
void MS5837_Getdata(float * outTemp, float * outPress);

#endif

MS5837.h文件先按照说明书定义一些变量：

#include "MS5837.h"
#include "IIC0.h"
#include "delay.h"

#if SYSTEM_SUPPORT_OS
#include "includes.h" //os 使用
#endif


/*
C1 压力灵敏度 SENS|T1
C2  压力补偿  OFF|T1
C3	温度压力灵敏度系数 TCS
C4	温度系数的压力补偿 TCO
C5	参考温度 T|REF
C6 	温度系数的温度 TEMPSENS
*/
uint32_t Cal_C[7]; //用于存放PROM中的6组数据C1-C6

double OFF_;
float Aux;
/*
dT 实际和参考温度之间的差异
Temperature 实际温度	
*/
uint64_t dT, Temperature;
/*
OFF 实际温度补偿
SENS 实际温度灵敏度
*/
uint64_t SENS;
uint32_t D1_Pres, D2_Temp;		 // 数字压力值,数字温度值
uint32_t TEMP2, T2, OFF2, SENS2; //温度校验值

uint32_t Pressure;			  //气压
uint32_t Depth;
float Atmdsphere_Pressure; //大气压

然后添加初始换、复位、读数据等函数

/*******************************************************************************
  * @函数名称	MS583730BA_RESET
  * @函数说明   复位MS5611
  * @输入参数   无
  * @输出参数   无
  * @返回参数   无
*******************************************************************************/
void MS583703BA_RESET(void)
{
	IIC0_Start();
	IIC0_Send_Byte(0xEC); //CSB接地，主机地址：0XEE，否则 0X77
	IIC0_Wait_Ack();
	IIC0_Send_Byte(0x1E); //发送复位命令
	IIC0_Wait_Ack();
	IIC0_Stop();
}

/*******************************************************************************
  * @函数名称	MS5611_init
  * @函数说明   初始化5611
  * @输入参数  	无
  * @输出参数   无
  * @返回参数   无
*******************************************************************************/
void MS5837_init(void)
{
	u8 inth, intl;
	u8 i;
	float air_temp, air_press, air_depth;

	IIC0_Init();
	MS583703BA_RESET(); // Reset Device  复位MS5837
	delay_ms(40);		//复位后延时（注意这个延时是一定必要的，可以缩短但似乎不能少于20ms）

	for (i = 1; i <= 6; i++)
	{
		IIC0_Start();
		IIC0_Send_Byte(0xEC);
		IIC0_Wait_Ack();
		IIC0_Send_Byte(0xA0 + (i * 2));
		IIC0_Wait_Ack();
		IIC0_Stop();
		delay_us(5);
		IIC0_Start();
		IIC0_Send_Byte(0xEC + 0x01); //进入接收模式
		delay_us(1);
		IIC0_Wait_Ack();
		inth = IIC0_Read_Byte(1); //带ACK的读数据
		delay_us(1);
		intl = IIC0_Read_Byte(0); //最后一个字节NACK
		IIC0_Stop();
		Cal_C[i] = (((uint16_t)inth << 8) | intl);
	}

	// for (i = 0; i < 5; i++)
	// {
	// 	delay_ms(1);
	// 	MS5837_Getdata(&air_temp, &air_press, &air_depth); //获取大气压
	// 	Atmdsphere_Pressure += air_press;
	// 	//printf("%d\t", Pressure); //串口输出原始数据
	// }
	// Atmdsphere_Pressure = Atmdsphere_Pressure / 5.0f;
	//printf("Atmdsphere_Pressure:%d\r\n", Atmdsphere_Pressure); //串口输出原始数据
}

/**************************实现函数********************************************
*函数原型:unsigned long MS561101BA_getConversion(void)
*功　　能:    读取 MS5837 的转换结果 
*******************************************************************************/
uint32_t MS583703BA_getConversion(uint8_t command)
{
	
	uint32_t  conversion = 0;
	u8 temp[3];

	
	IIC0_Start();
	IIC0_Send_Byte(0xEC); //写地址
	IIC0_Wait_Ack();
	IIC0_Send_Byte(command); //写转换命令
	IIC0_Wait_Ack();
	IIC0_Stop();
	
	//delay_ms(10);
	delay_us(10000);			// 等待AD转换完成
	
	IIC0_Start();
	IIC0_Send_Byte(0xEC); //写地址
	IIC0_Wait_Ack();
	IIC0_Send_Byte(0); // start read sequence
	IIC0_Wait_Ack();
	IIC0_Stop();

	IIC0_Start();
	IIC0_Send_Byte(0xEC + 0x01); //进入接收模式
	IIC0_Wait_Ack();
	temp[0] = IIC0_Read_Byte(1); //带ACK的读数据  bit 23-16
	temp[1] = IIC0_Read_Byte(1); //带ACK的读数据  bit 8-15
	temp[2] = IIC0_Read_Byte(0); //带NACK的读数据 bit 0-7
	IIC0_Stop();
	
	conversion = ((uint32_t)temp[0] << 16) | ((uint32_t)temp[1] << 8) | temp[2];

	return conversion;
}

///***********************************************
//  * @brief  读取气压
//  * @param  None
//  * @retval None
//************************************************/
void MS5837_Getdata(float * outTemp, float * outPress)
{
	D1_Pres = MS583703BA_getConversion(0x48);
	D2_Temp = MS583703BA_getConversion(0x58);

	if (D2_Temp > (((uint32_t)Cal_C[5]) * 256))
	{
		dT = D2_Temp - (((uint32_t)Cal_C[5]) * 256);
		Temperature = 2000 + dT * ((uint32_t)Cal_C[6]) / 8388608;
		OFF_ = (uint32_t)Cal_C[2] * 65536 + ((uint32_t)Cal_C[4] * dT) / 128;
		SENS = (uint32_t)Cal_C[1] * 32768 + ((uint32_t)Cal_C[3] * dT) / 256;
	}
	else
	{
		dT = (((uint32_t)Cal_C[5]) * 256) - D2_Temp;
		Temperature = 2000 - dT * ((uint32_t)Cal_C[6]) / 8388608;
		OFF_ = (uint32_t)Cal_C[2] * 65536 - ((uint32_t)Cal_C[4] * dT) / 128;
		SENS = (uint32_t)Cal_C[1] * 32768 - ((uint32_t)Cal_C[3] * dT) / 256;
	}
	
	if (Temperature < 2000) // low temp
	{
		Aux = (2000 - Temperature) * (2000 - Temperature);
		T2 = 3 * (dT * dT) / 8589934592;
		OFF2 = 3 * Aux / 2;
		SENS2 = 5 * Aux / 8;
	}
	else
	{
		Aux = (Temperature - 2000) * (Temperature - 2000);
		T2 = 2 * (dT * dT) / 137438953472;
		OFF2 = 1 * Aux / 16;
		SENS2 = 0;
	}
	OFF_ = OFF_ - OFF2;
	SENS = SENS - SENS2;
	
	*outTemp = (float)(Temperature - T2) / 100.0f;
	*outPress =(float)((D1_Pres * SENS / 2097152 - OFF_) / 8192) / 10.0f;
	//*outDepth = 0.983615 * (*outPress - Atmdsphere_Pressure);
}

通过调用 void MS5837_Getdata(float * outTemp, float * outPress)就可以读到经过二阶算法解算的温度与压力值，当然了这个值是绝对压力值，与大气压结合经过计算就能算出深度了。

三. 传感任务增加读水深的应用程序

现在我们继续封装，回到我们之前创建的sensor.h和sensor.c文件，在sensor.c文件中添加以下代码

void sensorReadMS5837(float *TEMPERATURE,float *PRESSURE, float *DEPTH)
{
	static float air_pressure = 985.0f;		// 默认大气压（正常是990-1010之间），保证算出来的是个正值，初始时刻是不是零深度并不重要
    MS5837_Getdata(TEMPERATURE,PRESSURE); 		//获取当前气压
	*DEPTH =  (*PRESSURE - air_pressure) / 0.983615;
}

这个地方我人为的给大气压赋了一个比正常偏小的值，这样算出来的深度永远为正的，对于定深控制是没有影响的。当然了你可以初始化时用MS5837将大气压读出来，再作为测量的大气压值。

当然了，可以滤一下波，还是滑动平均滤波

// 深度值滤波参数
float filterDepth[10];
float sumDepth;
u8 count_depth = 0;

// FIR滤波
void sensorReadDepth(float *water_depth)
{
	float w_depth,w_temperature,w_pressure;
	float temp;
	sensorReadMS5837(&w_temperature,&w_pressure,&w_depth);
	temp = filterDepth[count_depth];
	filterDepth[count_depth] = w_depth;
	sumDepth += filterDepth[count_depth] - temp;
	*water_depth = sumDepth / 10.0f;

	count_depth++;
	if (count_depth == 10) count_depth = 0;
}

现在，封装完毕，来到我们的main函数，在之前创建的 sensor_task中添加读深度的代码，更新后如下（建议与上一章对照看）

u8 sensor_task(void *p_arg)
{
	OS_ERR err;
	CPU_SR_ALLOC();

	float Gyro[3], Angle[3];
	float wDepth;
	u8 count = 20;

	//滤波初始化
	while (count--)
	{
		sensorReadAngle(Gyro, Angle);
		sensorReadDepth(&wDepth);
	}

	// 初始化之后，所有期望值复制为实际值
	state.realAngle.roll = Angle[0];
	state.realAngle.pitch = Angle[1];
	state.realAngle.yaw = Angle[2];
	state.realDepth = wDepth;
	setstate.expectedAngle.roll = state.realAngle.roll;
	setstate.expectedAngle.pitch = state.realAngle.pitch;
	setstate.expectedAngle.yaw = state.realAngle.yaw; //初始化之后将当前的姿态角作为期望姿态角初值
	setstate.expectedDepth = state.realDepth;

	while (1)
	{
		/********************************************** 获取期望值与测量值*******************************************/
		sensorReadAngle(Gyro, Angle);
		sensorReadDepth(&wDepth);
		//反馈值
		state.realAngle.roll = Angle[0];
		state.realAngle.pitch = Angle[1];
		state.realAngle.yaw = Angle[2];
		tate.realDepth = wDepth;
		state.realRate.roll = Gyro[0];
		state.realRate.pitch = Gyro[1];
		state.realRate.yaw = Gyro[2];
		
		delay_ms(5); // 水深传感器单次读取需要20ms+, 所以这里的延时小一点
	}
}

到这里读水深的任务就完成啦。