236 lines
6.8 KiB
C
236 lines
6.8 KiB
C
#include "ms5607.h"
|
||
#include "i2c.h"
|
||
#include "filter.h"
|
||
#include "anemometer_dev.h"
|
||
#include "uart_dev.h"
|
||
|
||
static void MS56XX_Reset(void)
|
||
{
|
||
uint8_t _cmd[] = {0x1e};
|
||
HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff);
|
||
osDelay(5);
|
||
}
|
||
|
||
static void MS56XX_Read_PromData(uint16_t *VAL_C)
|
||
{
|
||
uint8_t _cmd[] = {0xa0};
|
||
uint8_t temp_buff[2] = {0};
|
||
for(uint8_t i = 0; i < 8; i++)
|
||
{
|
||
HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff);
|
||
HAL_I2C_Master_Receive(&hi2c3, MS5607_ADDRESS, temp_buff, 2, 0xff);
|
||
*(VAL_C + i) = temp_buff[0];
|
||
*(VAL_C + i) <<= 8;
|
||
*(VAL_C + i) += temp_buff[1];
|
||
_cmd[0] += 0x02;
|
||
}
|
||
}
|
||
|
||
static uint16_t MS56XX_C_Value[8] = {0};
|
||
void MS56XX_Init(void)
|
||
{
|
||
MS56XX_Reset();
|
||
MS56XX_Read_PromData(MS56XX_C_Value);
|
||
}
|
||
|
||
static HAL_StatusTypeDef MS56XX_ReadD1_Press(uint32_t *D1_Value)
|
||
{
|
||
uint8_t D1_Value_Buff[3] = {0xFF, 0xFF, 0xFF};
|
||
uint8_t _cmd[] = {0x48};
|
||
uint8_t _addr[] = {0x00};
|
||
if(HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff) != HAL_OK)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
osDelay(10);
|
||
if(HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _addr, 1, 0xff) != HAL_OK)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
if(HAL_I2C_Master_Receive(&hi2c3, MS5607_ADDRESS, D1_Value_Buff, 3, 0xff) != HAL_OK)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
|
||
uint32_t press;
|
||
press = D1_Value_Buff[0];
|
||
press <<= 8;
|
||
press += D1_Value_Buff[1];
|
||
press <<= 8;
|
||
press += D1_Value_Buff[2];
|
||
*D1_Value = press;
|
||
return HAL_OK;
|
||
}
|
||
|
||
static HAL_StatusTypeDef MS56XX_ReadD2_Temp(uint32_t *D2_Value_)
|
||
{
|
||
uint8_t D2_Value_Buff[3] = {0x00, 0x00, 0x00};
|
||
uint8_t _cmd[] = {0x58};
|
||
uint8_t _addr[] = {0x00};
|
||
if(HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff) != HAL_OK)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
osDelay(10);
|
||
if(HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _addr, 1, 0xff) != HAL_OK)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
if(HAL_I2C_Master_Receive(&hi2c3, MS5607_ADDRESS, D2_Value_Buff, 3, 0xff) != HAL_OK)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
|
||
uint32_t Temp;
|
||
Temp = D2_Value_Buff[0];
|
||
Temp <<= 8;
|
||
Temp += D2_Value_Buff[1];
|
||
Temp <<= 8;
|
||
Temp += D2_Value_Buff[2];
|
||
*D2_Value_ = Temp;
|
||
return HAL_OK;
|
||
}
|
||
|
||
static HAL_StatusTypeDef MS56XX_GetTemperature (int32_t *dT, int32_t *MS56XX_Temp) //<2F><><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>
|
||
{
|
||
uint32_t D2_Value;
|
||
if(MS56XX_ReadD2_Temp(&D2_Value) != HAL_OK) //ѭ<><D1AD><EFBFBD><EFBFBD>ȡ D2
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
// term_printf("D2:%d\r\n", D2_Value);
|
||
|
||
if(D2_Value > (MS56XX_C_Value[5] * 256))
|
||
{
|
||
*dT = D2_Value - MS56XX_C_Value[5] * 256; //<2F><>ʽ dT = D2 - TREF = D2 - C5 * 2^8
|
||
}else
|
||
{
|
||
*dT = MS56XX_C_Value[5] * 256 - D2_Value; //<2F><>ʽ dT = D2 - TREF = D2 - C5 * 2^8
|
||
*dT *= -1;
|
||
}
|
||
|
||
// term_printf("dT:%d\r\n", *dT);
|
||
*MS56XX_Temp = 2000 + *dT * ((int32_t)MS56XX_C_Value[6]/8388608.0); //<2F><><EFBFBD><EFBFBD><EFBFBD>¶<EFBFBD>ֵ<EFBFBD><D6B5>100<30><30><EFBFBD><EFBFBD>2001<30><31>ʾ20.01<EFBFBD><EFBFBD> <20><>ʽTEMP =20<32><30>C + dT * TEMPSENS =2000 + dT * C6 / 2^23
|
||
return HAL_OK;
|
||
}
|
||
|
||
static HAL_StatusTypeDef MS56XX_GetPressureTemp(float *Temp, float *Press) //<2F><><EFBFBD><EFBFBD><EFBFBD>¶Ȳ<C2B6><C8B2><EFBFBD>ѹ<EFBFBD><D1B9>
|
||
{
|
||
int32_t dT;
|
||
int32_t MS56XX_Temperature;
|
||
uint32_t D1_Value;
|
||
if(MS56XX_ReadD1_Press(&D1_Value) != HAL_OK) //ѭ<><D1AD><EFBFBD><EFBFBD>ȡ D1
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
|
||
if(MS56XX_GetTemperature(&dT, &MS56XX_Temperature) != HAL_OK)//<2F><>ȡdT<64><54>Temp
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
// term_printf("TEMP:%d\r\n", MS56XX_Temperature);
|
||
|
||
// term_printf("dT:%d\r\n", dT);
|
||
// term_printf("TEMP:%d\r\n", MS56XX_Temperature);
|
||
|
||
int64_t OFF = ((int64_t)MS56XX_C_Value[2] * 131072) + ((int64_t)MS56XX_C_Value[4] * dT )/ 64.0; //<2F><>ʽ OFF = OFFT1 + TCO *dT = C2 *2^17 +(C4 *dT )/ 2^6
|
||
int64_t SENS = ((int64_t)MS56XX_C_Value[1] * 65536) + ((int64_t)MS56XX_C_Value[3] * dT )/ 128.0; //<2F><>ʽSENS = SENST1 + TCS* dT= C1 * 2^16 + (C3 * dT )/ 2^7
|
||
|
||
// term_printf("OFF:%d\r\n", OFF);
|
||
// term_printf("SENS:%d\r\n", SENS);
|
||
|
||
//<2F>¶Ȳ<C2B6><C8B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><EFBFBD><DFBC><EFBFBD>оƬ<D0BE>ֲ<EFBFBD>
|
||
int64_t T2 = 0;
|
||
int64_t OFF2 = 0;
|
||
int64_t SENS2 = 0;
|
||
if (MS56XX_Temperature < 2000 ) // second order temperature compensation when under 20 degrees C
|
||
{
|
||
T2 = (dT * dT) / 2147483648.0;
|
||
OFF2 = 61 * ((MS56XX_Temperature - 2000) * (MS56XX_Temperature - 2000)) / 16;
|
||
SENS2 = 2 * ((MS56XX_Temperature - 2000) * (MS56XX_Temperature - 2000)) ;
|
||
if (MS56XX_Temperature < -1500)
|
||
{
|
||
OFF2 = OFF2 + 15 * ((MS56XX_Temperature + 1500) * (MS56XX_Temperature + 1500));
|
||
SENS2 = SENS2 + 8 * ((MS56XX_Temperature + 1500) * (MS56XX_Temperature + 1500));
|
||
}
|
||
}
|
||
|
||
// term_printf("T2:%d\r\n", T2);
|
||
MS56XX_Temperature = MS56XX_Temperature - T2;
|
||
OFF = OFF - OFF2;
|
||
SENS = SENS - SENS2;
|
||
|
||
|
||
int32_t Tmp_Pressure = ((int32_t)D1_Value * SENS/2097152 - OFF) / 32768; //<2F><>ʽ P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
|
||
|
||
// term_printf("\r\nC1-C6:%d %d %d %d %d %d\r\n", MS56XX_C_Value[1], MS56XX_C_Value[2], MS56XX_C_Value[3], MS56XX_C_Value[4], MS56XX_C_Value[5], MS56XX_C_Value[6]);
|
||
// term_printf("D1:%d\r\n", D1_Value);
|
||
//
|
||
// term_printf("dT:%d\r\n", dT);
|
||
//
|
||
// term_printf("OFF:%d\r\n", OFF);
|
||
// term_printf("SENS:%d\r\n", SENS);
|
||
// term_printf("P:%d\r\n", Tmp_Pressure);
|
||
// term_printf("TEMP:%d\r\n", MS56XX_Temperature);
|
||
// term_printf("PRESS:%d\r\n", Tmp_Pressure);
|
||
if(MS56XX_Temperature<-6000)
|
||
{
|
||
MS56XX_Temperature=-6000;
|
||
return HAL_ERROR;
|
||
}
|
||
if(MS56XX_Temperature>8500)
|
||
{
|
||
MS56XX_Temperature=8500;
|
||
return HAL_ERROR;
|
||
}
|
||
if(Tmp_Pressure<1000)
|
||
{
|
||
// Tmp_Pressure=1000;
|
||
*Press = 0;
|
||
return HAL_ERROR;
|
||
}
|
||
if(Tmp_Pressure>120000)
|
||
{
|
||
// Tmp_Pressure=120000;
|
||
*Press = 0;
|
||
return HAL_ERROR;
|
||
}
|
||
*Temp = ((float)MS56XX_Temperature)/100;
|
||
*Press = ((float)Tmp_Pressure)/100;
|
||
return HAL_OK;
|
||
}
|
||
|
||
static float calculateAverage(float arr[], int avgLength) {
|
||
float sum = 0;
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>飨<EFBFBD><E9A3A8><EFBFBD><EFBFBD>10<31><30>Ԫ<EFBFBD>أ<EFBFBD><D8A3><EFBFBD><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD><EFBFBD><EFBFBD>ֱֵ<D6B5><D6B1><EFBFBD>ﵽָ<EFB5BD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
for (int i = 0; i < 10; ++i) {
|
||
sum += arr[i];
|
||
}
|
||
|
||
// <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
||
float average = sum / avgLength;
|
||
return average;
|
||
}
|
||
|
||
#define COLLECT_DATA_NUM 10
|
||
HAL_StatusTypeDef MS56XX_GetPressure(float *Press)
|
||
{
|
||
float temp_buff;
|
||
float pressure_buff[COLLECT_DATA_NUM] = {0};
|
||
uint8_t ret_falt = 0;
|
||
for(uint8_t i = 0;i<COLLECT_DATA_NUM;i++)
|
||
{
|
||
if(MS56XX_GetPressureTemp(&temp_buff, &pressure_buff[i]) != HAL_OK)
|
||
{
|
||
ret_falt++;
|
||
pressure_buff[i] = 0;
|
||
}
|
||
}
|
||
if(ret_falt >= COLLECT_DATA_NUM)
|
||
{
|
||
return HAL_ERROR;
|
||
}
|
||
*Press = calculateAverage( pressure_buff,COLLECT_DATA_NUM - ret_falt);
|
||
return HAL_OK;
|
||
} |