1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/flight/pios/common/pios_ms5611.c

353 lines
9.7 KiB
C
Raw Normal View History

/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MS5611 MS5611 Functions
* @brief Hardware functions to deal with the altitude pressure sensor
* @{
*
* @file pios_ms5611.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief MS5611 Pressure Sensor Routines
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_MS5611
#define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
/* Glocal Variables */
ConversionTypeTypeDef CurrentRead;
/* Local Variables */
MS5611CalibDataTypeDef CalibData;
/* Straight from the datasheet */
static uint32_t RawTemperature;
static uint32_t RawPressure;
static int64_t Pressure;
static int64_t Temperature;
static int32_t lastConversionStart;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
// Move into proper driver structure with cfg stored
static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
static int32_t i2c_id;
/**
* Initialise the MS5611 sensor
*/
int32_t ms5611_read_flag;
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
{
i2c_id = i2c_device;
oversampling = cfg->oversampling;
dev_cfg = cfg; // Store cfg before enabling interrupt
PIOS_MS5611_WriteCommand(MS5611_RESET);
PIOS_DELAY_WaitmS(20);
uint8_t data[2];
// reset temperature compensation values
compensation_t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
CalibData.C[i] = (data[0] << 8) | data[1];
}
}
/**
* Start the ADC conversion
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
* \return 0 for success, -1 for failure (conversion completed and not read)
*/
int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
{
/* Start the conversion */
if (Type == TemperatureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
continue;
}
} else if (Type == PressureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
continue;
}
}
lastConversionStart = PIOS_DELAY_GetRaw();
CurrentRead = Type;
return 0;
}
/**
* @brief Return the delay for the current osr
*/
int32_t PIOS_MS5611_GetDelay()
{
switch (oversampling) {
case MS5611_OSR_256:
return 1;
case MS5611_OSR_512:
return 2;
case MS5611_OSR_1024:
return 3;
case MS5611_OSR_2048:
return 5;
case MS5611_OSR_4096:
return 10;
default:
break;
}
return 10;
}
/**
* @brief Return the delay for the current osr in uS
*/
uint32_t PIOS_MS5611_GetDelayUs()
{
switch (oversampling) {
case MS5611_OSR_256:
return 600;
case MS5611_OSR_512:
return 1170;
case MS5611_OSR_1024:
return 2280;
case MS5611_OSR_2048:
return 4540;
case MS5611_OSR_4096:
return 9040;
default:
break;
}
return 10;
}
/**
* Read the ADC conversion value (once ADC conversion has completed)
* \return 0 if successfully read the ADC, -1 if failed
*/
int32_t PIOS_MS5611_ReadADC(void)
{
uint8_t Data[3];
Data[0] = 0;
Data[1] = 0;
Data[2] = 0;
while (PIOS_MS5611_GetDelayUs() > PIOS_DELAY_DiffuS(lastConversionStart)) {
vTaskDelay(0);
}
static int64_t deltaTemp;
/* Read and store the 16bit result */
if (CurrentRead == TemperatureConv) {
/* Read the temperature conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
}
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
// Difference between actual and reference temperature
// dT = D2 - TREF = D2 - C5 * 2^8
deltaTemp = ((int32_t)RawTemperature) - (CalibData.C[4] * POW2(8));
// Actual temperature (-40…85°C with 0.01°C resolution)
// TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23));
} else {
int64_t Offset;
int64_t Sens;
// used for second order temperature compensation
int64_t Offset2 = 0;
2013-12-10 01:44:13 +01:00
int64_t Sens2 = 0;
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
}
// check if temperature is less than 20°C
2013-12-10 01:44:13 +01:00
if (Temperature < 2000) {
// Apply compensation
// T2 = dT^2 / 2^31
// OFF2 = 5 ⋅ (TEMP 2000)^2/2
// SENS2 = 5 ⋅ (TEMP 2000)^2/2^2
int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
Offset2 = (5 * tcorr) / 2;
Sens2 = (5 * tcorr) / 4;
compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
2013-12-10 01:44:13 +01:00
if (Temperature < -1500) {
// OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
// SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
Offset2 += 7 * tcorr2;
Sens2 += (11 * tcorr2) / 2;
}
} else {
compensation_t2 = 0;
Offset2 = 0;
Sens2 = 0;
}
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
// Offset at actual temperature
// OFF = OFFT1 + TCO * dT = C2 * 2^16 + (C4 * dT) / 2^7
Offset = ((int64_t)CalibData.C[1]) * POW2(16) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(7) - Offset2;
// Sensitivity at actual temperature
// SENS = SENST1 + TCS * dT = C1 * 2^15 + (C3 * dT) / 2^8
Sens = ((int64_t)CalibData.C[0]) * POW2(15) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(8) - Sens2;
// Temperature compensated pressure (10…1200mbar with 0.01mbar resolution)
// P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15);
}
return 0;
}
/**
* Return the most recently computed temperature in kPa
*/
float PIOS_MS5611_GetTemperature(void)
{
// Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
}
/**
* Return the most recently computed pressure in kPa
*/
float PIOS_MS5611_GetPressure(void)
{
return ((float)Pressure) / 1000.0f;
}
/**
* Reads one or more bytes into a buffer
* \param[in] the command indicating the address to read
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = MS5611_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = 1,
.buf = &address,
}
,
{
.info = __func__,
.addr = MS5611_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
}
/**
* Writes one or more bytes to the MS5611
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_MS5611_WriteCommand(uint8_t command)
{
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = MS5611_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = 1,
.buf = &command,
}
,
};
return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Run self-test operation.
* \return 0 if self-test succeed, -1 if failed
*/
int32_t PIOS_MS5611_Test()
{
// TODO: Is there a better way to test this than just checking that pressure/temperature has changed?
int32_t cur_value = 0;
cur_value = Temperature;
PIOS_MS5611_StartADC(TemperatureConv);
PIOS_DELAY_WaitmS(5);
PIOS_MS5611_ReadADC();
if (cur_value == Temperature) {
return -1;
}
cur_value = Pressure;
PIOS_MS5611_StartADC(PressureConv);
PIOS_DELAY_WaitmS(26);
PIOS_MS5611_ReadADC();
if (cur_value == Pressure) {
return -1;
}
return 0;
}
#endif /* PIOS_INCLUDE_MS5611 */
/**
* @}
* @}
*/