mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
535 lines
16 KiB
C
535 lines
16 KiB
C
/**
|
||
******************************************************************************
|
||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||
* @{
|
||
* @addtogroup PIOS_MS56XX MS56XX Functions
|
||
* @brief Hardware functions to deal with the altitude pressure sensor
|
||
* @{
|
||
*
|
||
* @file pios_ms56xx.c
|
||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||
* @brief MS56XX 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_MS56XX
|
||
#include <pios_ms56xx.h>
|
||
#define POW2(x) (1 << x)
|
||
|
||
// Command addresses
|
||
#define MS56XX_RESET 0x1E
|
||
#define MS56XX_CALIB_ADDR 0xA2 /* First sample is factory stuff */
|
||
#define MS56XX_CALIB_LEN 16
|
||
#define MS56XX_ADC_READ 0x00
|
||
#define MS56XX_PRES_ADDR 0x40
|
||
#define MS56XX_TEMP_ADDR 0x50
|
||
|
||
// Option to change the interleave between Temp and Pressure conversions
|
||
// Undef for normal operation
|
||
#define PIOS_MS56XX_SLOW_TEMP_RATE 20
|
||
#ifndef PIOS_MS56XX_SLOW_TEMP_RATE
|
||
#define PIOS_MS56XX_SLOW_TEMP_RATE 1
|
||
#endif
|
||
// Running moving average smoothing factor
|
||
#define PIOS_MS56XX_TEMP_SMOOTHING 10
|
||
|
||
#define PIOS_MS56XX_INIT_DELAY_US 1000000
|
||
#define PIOS_MS56XX_RESET_DELAY_US 20000
|
||
|
||
/* Local Types */
|
||
typedef struct {
|
||
uint16_t C[6];
|
||
} MS56XXCalibDataTypeDef;
|
||
|
||
typedef enum {
|
||
MS56XX_CONVERSION_TYPE_None = 0,
|
||
MS56XX_CONVERSION_TYPE_PressureConv,
|
||
MS56XX_CONVERSION_TYPE_TemperatureConv
|
||
} ConversionTypeTypeDef;
|
||
|
||
typedef enum {
|
||
MS56XX_FSM_INIT = 0,
|
||
MS56XX_FSM_CALIBRATION,
|
||
MS56XX_FSM_TEMPERATURE,
|
||
MS56XX_FSM_PRESSURE,
|
||
MS56XX_FSM_CALCULATE,
|
||
} MS56XX_FSM_State;
|
||
|
||
static ConversionTypeTypeDef CurrentRead = MS56XX_CONVERSION_TYPE_None;
|
||
|
||
static MS56XXCalibDataTypeDef CalibData;
|
||
|
||
static uint8_t temp_press_interleave_count = 1;
|
||
|
||
/* Straight from the datasheet */
|
||
static uint32_t RawTemperature;
|
||
static uint32_t RawPressure;
|
||
static int64_t Pressure;
|
||
static int64_t Temperature;
|
||
static int64_t FilteredTemperature = INT32_MIN;
|
||
|
||
static uint32_t lastCommandTimeRaw;
|
||
static uint32_t commandDelayUs;
|
||
|
||
static uint32_t conversionDelayUs;
|
||
|
||
// Second order temperature compensation. Temperature offset
|
||
static int64_t compensation_t2;
|
||
|
||
// Move into proper driver structure with cfg stored
|
||
static enum pios_ms56xx_version version;
|
||
static uint8_t ms56xx_address;
|
||
static uint32_t oversampling;
|
||
static int32_t i2c_id;
|
||
static PIOS_SENSORS_1Axis_SensorsWithTemp results;
|
||
static bool hw_error = false;
|
||
|
||
// private functions
|
||
static int32_t PIOS_MS56xx_Read_I2C(uint8_t address, uint8_t *buffer, uint8_t len);
|
||
static int32_t PIOS_MS56xx_WriteCommand(uint8_t command, uint32_t delayuS);
|
||
static uint32_t PIOS_MS56xx_GetDelayUs(void);
|
||
static void PIOS_MS56xx_ReadCalibrationData(void);
|
||
|
||
// sensor driver interface
|
||
bool PIOS_MS56xx_driver_Test(uintptr_t context);
|
||
void PIOS_MS56xx_driver_Reset(uintptr_t context);
|
||
void PIOS_MS56xx_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
|
||
void PIOS_MS56xx_driver_fetch(void *, uint8_t size, uintptr_t context);
|
||
bool PIOS_MS56xx_driver_poll(uintptr_t context);
|
||
|
||
const PIOS_SENSORS_Driver PIOS_MS56xx_Driver = {
|
||
.test = PIOS_MS56xx_driver_Test,
|
||
.poll = PIOS_MS56xx_driver_poll,
|
||
.fetch = PIOS_MS56xx_driver_fetch,
|
||
.reset = PIOS_MS56xx_driver_Reset,
|
||
.get_queue = NULL,
|
||
.get_scale = PIOS_MS56xx_driver_get_scale,
|
||
.is_polled = true,
|
||
};
|
||
/**
|
||
* Initialise the MS56XX sensor
|
||
*/
|
||
void PIOS_MS56xx_Init(const struct pios_ms56xx_cfg *cfg, int32_t i2c_device)
|
||
{
|
||
i2c_id = i2c_device;
|
||
|
||
ms56xx_address = cfg->address;
|
||
version = cfg->version;
|
||
oversampling = cfg->oversampling;
|
||
conversionDelayUs = PIOS_MS56xx_GetDelayUs();
|
||
}
|
||
/**
|
||
* 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), -2 if failure occurred
|
||
*/
|
||
int32_t PIOS_MS56xx_StartADC(ConversionTypeTypeDef Type)
|
||
{
|
||
/* Start the conversion */
|
||
|
||
if (Type == MS56XX_CONVERSION_TYPE_TemperatureConv) {
|
||
if (PIOS_MS56xx_WriteCommand(MS56XX_TEMP_ADDR + oversampling, conversionDelayUs) != 0) {
|
||
return -2;
|
||
}
|
||
} else if (Type == MS56XX_CONVERSION_TYPE_PressureConv) {
|
||
if (PIOS_MS56xx_WriteCommand(MS56XX_PRES_ADDR + oversampling, conversionDelayUs) != 0) {
|
||
return -2;
|
||
}
|
||
}
|
||
|
||
CurrentRead = Type;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief Return the delay for the current osr in uS
|
||
*/
|
||
static uint32_t PIOS_MS56xx_GetDelayUs()
|
||
{
|
||
switch (oversampling) {
|
||
case MS56XX_OSR_256:
|
||
return 600;
|
||
|
||
case MS56XX_OSR_512:
|
||
return 1170;
|
||
|
||
case MS56XX_OSR_1024:
|
||
return 2280;
|
||
|
||
case MS56XX_OSR_2048:
|
||
return 4540;
|
||
|
||
case MS56XX_OSR_4096:
|
||
return 9040;
|
||
|
||
case MS56XX_OSR_8192:
|
||
return 18080;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
return 10;
|
||
}
|
||
|
||
/**
|
||
* Read the ADC conversion value (once ADC conversion has completed)
|
||
* \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred
|
||
*/
|
||
int32_t PIOS_MS56xx_ReadADC(void)
|
||
{
|
||
uint8_t Data[3];
|
||
|
||
Data[0] = 0;
|
||
Data[1] = 0;
|
||
Data[2] = 0;
|
||
|
||
if (CurrentRead == MS56XX_CONVERSION_TYPE_None) {
|
||
return -2;
|
||
}
|
||
|
||
static int64_t deltaTemp;
|
||
|
||
if (PIOS_MS56xx_Read_I2C(MS56XX_ADC_READ, Data, 3) != 0) {
|
||
return -2;
|
||
}
|
||
|
||
/* Read and store the 16bit result */
|
||
if (CurrentRead == MS56XX_CONVERSION_TYPE_TemperatureConv) {
|
||
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));
|
||
if (FilteredTemperature != INT32_MIN) {
|
||
FilteredTemperature = (FilteredTemperature * (PIOS_MS56XX_TEMP_SMOOTHING - 1)
|
||
+ Temperature) / PIOS_MS56XX_TEMP_SMOOTHING;
|
||
} else {
|
||
FilteredTemperature = Temperature;
|
||
}
|
||
} else {
|
||
int64_t Offset;
|
||
int64_t Sens;
|
||
// used for second order temperature compensation
|
||
int64_t Offset2 = 0;
|
||
int64_t Sens2 = 0;
|
||
|
||
// check if temperature is less than 20°C
|
||
if (FilteredTemperature < 2000) {
|
||
// Apply compensation
|
||
// T2 = dT^2 / 2^31
|
||
// OFF2 = 5 ⋅ (TEMP – 2000)^2/2
|
||
// SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2
|
||
|
||
int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 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
|
||
if (FilteredTemperature < -1500) {
|
||
// OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
|
||
// SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
|
||
int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 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 and sensitivity at actual temperature
|
||
if (version == MS56XX_VERSION_5611) {
|
||
// 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;
|
||
// 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;
|
||
} else {
|
||
// OFF = OFFT1 + TCO * dT = C2 * 2^17 + (C4 * dT) / 2^6
|
||
Offset = ((int64_t)CalibData.C[1]) * POW2(17) + (((int64_t)CalibData.C[3]) * deltaTemp) / POW2(6) - Offset2;
|
||
// SENS = SENST1 + TCS * dT = C1 * 2^16 + (C3 * dT) / 2^7
|
||
Sens = ((int64_t)CalibData.C[0]) * POW2(16) + (((int64_t)CalibData.C[2]) * deltaTemp) / POW2(7) - 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);
|
||
}
|
||
|
||
CurrentRead = MS56XX_CONVERSION_TYPE_None;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* Return the most recently computed temperature in kPa
|
||
*/
|
||
static float PIOS_MS56xx_GetTemperature(void)
|
||
{
|
||
// Apply the second order low and very low temperature compensation offset
|
||
return ((float)(FilteredTemperature - compensation_t2)) / 100.0f;
|
||
}
|
||
|
||
/**
|
||
* Return the most recently computed pressure in Pa
|
||
*/
|
||
static float PIOS_MS56xx_GetPressure(void)
|
||
{
|
||
return (float)Pressure;
|
||
}
|
||
|
||
/**
|
||
* 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
|
||
*/
|
||
static int32_t PIOS_MS56xx_Read_I2C(uint8_t address, uint8_t *buffer, uint8_t len)
|
||
{
|
||
const struct pios_i2c_txn txn_list[] = {
|
||
{
|
||
.info = __func__,
|
||
.addr = ms56xx_address,
|
||
.rw = PIOS_I2C_TXN_WRITE,
|
||
.len = 1,
|
||
.buf = &address,
|
||
}
|
||
,
|
||
{
|
||
.info = __func__,
|
||
.addr = ms56xx_address,
|
||
.rw = PIOS_I2C_TXN_READ,
|
||
.len = len,
|
||
.buf = buffer,
|
||
}
|
||
};
|
||
|
||
enum pios_i2c_transfer_result i2c_result = PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
|
||
|
||
if (i2c_result == PIOS_I2C_TRANSFER_OK) {
|
||
return 0;
|
||
}
|
||
|
||
if (i2c_result != PIOS_I2C_TRANSFER_BUSY) {
|
||
hw_error = true;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
/**
|
||
* Writes one or more bytes to the MS56XX
|
||
* \param[in] address Register address
|
||
* \param[in] buffer source buffer
|
||
* \return 0 if operation was successful
|
||
* \return -1 if error during I2C transfer
|
||
*/
|
||
static int32_t PIOS_MS56xx_WriteCommand(uint8_t command, uint32_t delayuS)
|
||
{
|
||
const struct pios_i2c_txn txn_list[] = {
|
||
{
|
||
.info = __func__,
|
||
.addr = ms56xx_address,
|
||
.rw = PIOS_I2C_TXN_WRITE,
|
||
.len = 1,
|
||
.buf = &command,
|
||
}
|
||
,
|
||
};
|
||
|
||
lastCommandTimeRaw = PIOS_DELAY_GetRaw();
|
||
commandDelayUs = delayuS;
|
||
|
||
enum pios_i2c_transfer_result i2c_result = PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
|
||
|
||
if (i2c_result == PIOS_I2C_TRANSFER_OK) {
|
||
return 0;
|
||
}
|
||
|
||
if (i2c_result != PIOS_I2C_TRANSFER_BUSY) {
|
||
hw_error = true;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
static void PIOS_MS56xx_ReadCalibrationData()
|
||
{
|
||
uint8_t data[2];
|
||
|
||
/* reset temperature compensation values */
|
||
compensation_t2 = 0;
|
||
/* Calibration parameters */
|
||
for (int i = 0; i < 6; i++) {
|
||
if (PIOS_MS56xx_Read_I2C(MS56XX_CALIB_ADDR + i * 2, data, 2) != 0) {
|
||
break;
|
||
}
|
||
CalibData.C[i] = (data[0] << 8) | data[1];
|
||
}
|
||
}
|
||
|
||
static void PIOS_MS56xx_Reset()
|
||
{
|
||
temp_press_interleave_count = 1;
|
||
hw_error = false;
|
||
|
||
PIOS_MS56xx_WriteCommand(MS56XX_RESET, PIOS_MS56XX_RESET_DELAY_US);
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief Run self-test operation.
|
||
* \return 0 if self-test succeed, -1 if failed
|
||
*/
|
||
int32_t PIOS_MS56xx_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_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv);
|
||
PIOS_DELAY_WaitmS(10);
|
||
PIOS_MS56xx_ReadADC();
|
||
if (cur_value == Temperature) {
|
||
return -1;
|
||
}
|
||
|
||
cur_value = Pressure;
|
||
PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_PressureConv);
|
||
PIOS_DELAY_WaitmS(10);
|
||
PIOS_MS56xx_ReadADC();
|
||
if (cur_value == Pressure) {
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/* PIOS sensor driver implementation */
|
||
void PIOS_MS56xx_Register()
|
||
{
|
||
PIOS_SENSORS_Register(&PIOS_MS56xx_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0);
|
||
}
|
||
|
||
bool PIOS_MS56xx_driver_Test(__attribute__((unused)) uintptr_t context)
|
||
{
|
||
return true; // !PIOS_MS56xx_Test();
|
||
}
|
||
|
||
void PIOS_MS56xx_driver_Reset(__attribute__((unused)) uintptr_t context) {}
|
||
|
||
void PIOS_MS56xx_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
|
||
{
|
||
PIOS_Assert(size > 0);
|
||
scales[0] = 1;
|
||
}
|
||
|
||
void PIOS_MS56xx_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context)
|
||
{
|
||
PIOS_Assert(data);
|
||
memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp));
|
||
}
|
||
|
||
bool PIOS_MS56xx_driver_poll(__attribute__((unused)) uintptr_t context)
|
||
{
|
||
static MS56XX_FSM_State next_state = MS56XX_FSM_INIT;
|
||
|
||
if (PIOS_DELAY_DiffuS(lastCommandTimeRaw) < commandDelayUs) {
|
||
return false;
|
||
}
|
||
|
||
commandDelayUs = 0;
|
||
|
||
PIOS_MS56xx_ReadADC();
|
||
|
||
switch (next_state) {
|
||
case MS56XX_FSM_INIT:
|
||
PIOS_MS56xx_Reset();
|
||
next_state = MS56XX_FSM_CALIBRATION;
|
||
break;
|
||
case MS56XX_FSM_CALIBRATION:
|
||
PIOS_MS56xx_ReadCalibrationData();
|
||
/* fall through to MS56XX_FSM_TEMPERATURE */
|
||
|
||
case MS56XX_FSM_TEMPERATURE:
|
||
PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv);
|
||
next_state = MS56XX_FSM_PRESSURE;
|
||
break;
|
||
|
||
case MS56XX_FSM_PRESSURE:
|
||
PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_PressureConv);
|
||
next_state = MS56XX_FSM_CALCULATE;
|
||
break;
|
||
|
||
case MS56XX_FSM_CALCULATE:
|
||
temp_press_interleave_count--;
|
||
if (!temp_press_interleave_count) {
|
||
temp_press_interleave_count = PIOS_MS56XX_SLOW_TEMP_RATE;
|
||
PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_TemperatureConv);
|
||
next_state = MS56XX_FSM_PRESSURE;
|
||
} else {
|
||
PIOS_MS56xx_StartADC(MS56XX_CONVERSION_TYPE_PressureConv);
|
||
next_state = MS56XX_FSM_CALCULATE;
|
||
}
|
||
|
||
results.temperature = PIOS_MS56xx_GetTemperature();
|
||
results.sample = PIOS_MS56xx_GetPressure();
|
||
return true;
|
||
|
||
default:
|
||
// it should not be there
|
||
PIOS_Assert(0);
|
||
}
|
||
|
||
if (hw_error) {
|
||
lastCommandTimeRaw = PIOS_DELAY_GetRaw();
|
||
commandDelayUs = (next_state == MS56XX_FSM_CALIBRATION) ? PIOS_MS56XX_INIT_DELAY_US : 0;
|
||
CurrentRead = MS56XX_CONVERSION_TYPE_None;
|
||
next_state = MS56XX_FSM_INIT;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
/* Poll the pressure sensor and return the temperature and pressure. */
|
||
bool PIOS_MS56xx_Read(float *temperature, float *pressure)
|
||
{
|
||
if (PIOS_MS56xx_driver_poll(0)) {
|
||
*temperature = results.temperature;
|
||
*pressure = results.sample;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
#endif /* PIOS_INCLUDE_MS56XX */
|
||
|
||
/**
|
||
* @}
|
||
* @}
|
||
*/
|