rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

LP-529 [I2C] [MS5611] Remove PIOS_DELAY_WaitmS() and move (re)initialization into fsm.

Browse Source
This commit is contained in:
Vladimir Zidar 2017-06-14 16:07:44 +02:00
parent b0fa0edb64
commit 429f8c41be
4 changed files with 108 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

217
flight/pios/common/pios_ms5611.c
View File

@ -33,9 +33,6 @@
#include <pios_ms5611.h>
#define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
// Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation
#define PIOS_MS5611_SLOW_TEMP_RATE 20
@ -44,9 +41,9 @@
#endif
// Running moving average smoothing factor
#define PIOS_MS5611_TEMP_SMOOTHING 10
//
#define PIOS_MS5611_I2C_RETRIES 5
#define PIOS_MS5611_INIT_DELAY_US 1000000
#define PIOS_MS5611_RESET_DELAY_US 20000
/* Local Types */
typedef struct {
@ -61,16 +58,17 @@ typedef enum {
typedef enum {
MS5611_FSM_INIT = 0,
MS5611_FSM_CALIBRATION,
MS5611_FSM_TEMPERATURE,
MS5611_FSM_PRESSURE,
MS5611_FSM_CALCULATE,
} MS5611_FSM_State;
/* Glocal Variables */
ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
static ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
/* Local Variables */
MS5611CalibDataTypeDef CalibData;
static MS5611CalibDataTypeDef CalibData;
static uint8_t temp_press_interleave_count = 1;
/* Straight from the datasheet */
static uint32_t RawTemperature;
@ -78,16 +76,12 @@ static uint32_t RawPressure;
static int64_t Pressure;
static int64_t Temperature;
static int64_t FilteredTemperature = INT32_MIN;
static int32_t lastConversionStart;
static uint32_t conversionDelayMs;
static uint32_t lastCommandTimeRaw;
static uint32_t commandDelayUs;
static uint32_t conversionDelayUs;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
static uint32_t PIOS_MS5611_GetDelay();
static uint32_t PIOS_MS5611_GetDelayUs();
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
@ -96,8 +90,13 @@ static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
static int32_t i2c_id;
static PIOS_SENSORS_1Axis_SensorsWithTemp results;
static bool hw_error = false;
static bool sensorIsAlive = false;
// private functions
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command, uint32_t delayuS);
static uint32_t PIOS_MS5611_GetDelayUs(void);
static void PIOS_MS5611_ReadCalibrationData(void);
// sensor driver interface
bool PIOS_MS5611_driver_Test(uintptr_t context);
@ -118,43 +117,15 @@ const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
/**
* Initialise the MS5611 sensor
*/
int32_t ms5611_read_flag;
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
{
static uint32_t initTime;
i2c_id = i2c_device;
if (cfg) {
i2c_id = i2c_device;
oversampling = cfg->oversampling;
conversionDelayUs = PIOS_MS5611_GetDelayUs();
oversampling = cfg->oversampling;
conversionDelayMs = PIOS_MS5611_GetDelay();
conversionDelayUs = PIOS_MS5611_GetDelayUs();
dev_cfg = cfg; // Store cfg before enabling interrupt
} else if (PIOS_DELAY_DiffuS(initTime) < 1000000) { // Do not reinitialize too often
return;
}
initTime = PIOS_DELAY_GetRaw();
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++) {
if (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2) != 0) {
return;
}
CalibData.C[i] = (data[0] << 8) | data[1];
}
sensorIsAlive = true;
dev_cfg = cfg; // Store cfg before enabling interrupt
}
/**
* Start the ADC conversion
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
@ -162,54 +133,23 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
*/
int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
{
if (!sensorIsAlive) { /* if sensor is not alive, don't bother, wait for next poll to try to reinitialize */
return -2;
}
/* Start the conversion */
if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
if (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
if (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling, conversionDelayUs) != 0) {
return -2;
}
} else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
if (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
if (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling, conversionDelayUs) != 0) {
return -2;
}
}
lastConversionStart = PIOS_DELAY_GetRaw();
CurrentRead = Type;
return 0;
}
/**
* @brief Return the delay for the current osr
*/
static uint32_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
*/
@ -243,10 +183,6 @@ static uint32_t PIOS_MS5611_GetDelayUs()
*/
int32_t PIOS_MS5611_ReadADC(void)
{
if (!sensorIsAlive) { /* if sensor is not alive, don't bother, wait for next poll to try to reinitialize */
return -2;
}
uint8_t Data[3];
Data[0] = 0;
@ -256,18 +192,15 @@ int32_t PIOS_MS5611_ReadADC(void)
if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
return -2;
}
if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
return -1;
}
static int64_t deltaTemp;
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -2;
}
/* Read and store the 16bit result */
if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) {
/* Read the temperature conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -2;
}
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
// Difference between actual and reference temperature
// dT = D2 - TREF = D2 - C5 * 2^8
@ -288,10 +221,6 @@ int32_t PIOS_MS5611_ReadADC(void)
int64_t Offset2 = 0;
int64_t Sens2 = 0;
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -2;
}
// check if temperature is less than 20°C
if (FilteredTemperature < 2000) {
// Apply compensation
@ -327,6 +256,9 @@ int32_t PIOS_MS5611_ReadADC(void)
// P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15);
}
CurrentRead = MS5611_CONVERSION_TYPE_None;
return 0;
}
@ -375,13 +307,15 @@ static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
}
};
for (uint8_t retry = PIOS_MS5611_I2C_RETRIES; retry > 0; --retry) {
if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
return 0;
}
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;
}
sensorIsAlive = false;
if (i2c_result != PIOS_I2C_TRANSFER_BUSY) {
hw_error = true;
}
return -1;
}
@ -393,7 +327,7 @@ static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
static int32_t PIOS_MS5611_WriteCommand(uint8_t command, uint32_t delayuS)
{
const struct pios_i2c_txn txn_list[] = {
{
@ -406,17 +340,46 @@ static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
,
};
for (uint8_t retry = PIOS_MS5611_I2C_RETRIES; retry > 0; --retry) {
if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
return 0;
}
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;
}
sensorIsAlive = false;
if (i2c_result != PIOS_I2C_TRANSFER_BUSY) {
hw_error = true;
}
return -1;
}
static void PIOS_MS5611_ReadCalibrationData()
{
uint8_t data[2];
/* reset temperature compensation values */
compensation_t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
if (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2) != 0) {
break;
}
CalibData.C[i] = (data[0] << 8) | data[1];
}
}
static void PIOS_MS5611_Reset()
{
temp_press_interleave_count = 1;
hw_error = false;
PIOS_MS5611_WriteCommand(MS5611_RESET, PIOS_MS5611_RESET_DELAY_US);
}
/**
* @brief Run self-test operation.
* \return 0 if self-test succeed, -1 if failed
@ -473,32 +436,34 @@ void PIOS_MS5611_driver_fetch(void *data, __attribute__((unused)) uint8_t size,
bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
{
static uint8_t temp_press_interleave_count = 1;
static MS5611_FSM_State next_state = MS5611_FSM_INIT;
if (!sensorIsAlive) { // try to reinit
PIOS_MS5611_Init(0, 0);
if (PIOS_DELAY_DiffuS(lastCommandTimeRaw) < commandDelayUs) {
return false;
}
int32_t conversionResult = PIOS_MS5611_ReadADC();
commandDelayUs = 0;
PIOS_MS5611_ReadADC();
if (__builtin_expect(conversionResult == -1, 1)) {
return false; // wait for conversion to complete
} else if (__builtin_expect(conversionResult == -2, 0)) {
temp_press_interleave_count = 1;
next_state = MS5611_FSM_INIT;
}
switch (next_state) {
case MS5611_FSM_INIT:
PIOS_MS5611_Reset();
next_state = MS5611_FSM_CALIBRATION;
break;
case MS5611_FSM_CALIBRATION:
PIOS_MS5611_ReadCalibrationData();
/* fall through to MS5611_FSM_TEMPERATURE */
case MS5611_FSM_TEMPERATURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
next_state = MS5611_FSM_PRESSURE;
return false;
break;
case MS5611_FSM_PRESSURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
next_state = MS5611_FSM_CALCULATE;
return false;
break;
case MS5611_FSM_CALCULATE:
temp_press_interleave_count--;
@ -519,6 +484,14 @@ bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
// it should not be there
PIOS_Assert(0);
}
if (hw_error) {
lastCommandTimeRaw = PIOS_DELAY_GetRaw();
commandDelayUs = (next_state == MS5611_FSM_CALIBRATION) ? PIOS_MS5611_INIT_DELAY_US : 0;
CurrentRead = MS5611_CONVERSION_TYPE_None;
next_state = MS5611_FSM_INIT;
}
return false;
}

10
flight/pios/inc/pios_i2c.h
View File

@ -74,11 +74,11 @@ enum pios_i2c_error_count {
};
enum pios_i2c_transfer_result {
PIOS_I2C_TRANSFER_OK = 0,
PIOS_I2C_TRANSFER_BUSY = -2,
PIOS_I2C_TRANSFER_BUS_ERROR = -1,
PIOS_I2C_TRANSFER_NACK = -3,
PIOS_I2C_TRANSFER_TIMEOUT = -4,
PIOS_I2C_TRANSFER_OK = 0,
PIOS_I2C_TRANSFER_BUSY = -2,
PIOS_I2C_TRANSFER_BUS_ERROR = -1,
PIOS_I2C_TRANSFER_NACK = -3,
PIOS_I2C_TRANSFER_TIMEOUT = -4,
PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR = -5,
PIOS_I2C_TRANSFER_DEVICE_ERROR = -6,
};

10
flight/pios/stm32f30x/pios_i2c.c
View File

@ -234,9 +234,9 @@ static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken)
I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_NACKI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_TXI, DISABLE);
enum pios_i2c_transfer_result result =
i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK :
PIOS_I2C_TRANSFER_OK;
i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK :
PIOS_I2C_TRANSFER_OK;
if (i2c_adapter->transfer_result) {
*(i2c_adapter->transfer_result) = result;
@ -244,7 +244,7 @@ static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken)
pios_i2c_callback cb = i2c_adapter->callback;
if(!cb) { /* No callback? Signal that we are done */
if (!cb) { /* No callback? Signal that we are done */
#ifdef PIOS_INCLUDE_FREERTOS
signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE;
if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) {
@ -271,7 +271,7 @@ static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken)
PIOS_IRQ_Enable();
#endif /* PIOS_INCLUDE_FREERTOS */
if(cb && cb(result)) { /* User provided callback? Do it, but with bus unlocked */
if (cb && cb(result)) { /* User provided callback? Do it, but with bus unlocked */
*woken = true;
}
}