From 8c60c28b513e6e6e0f47a50a5f60b67b4539796d Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Thu, 8 Jun 2017 15:03:48 +0200 Subject: [PATCH 1/4] LP-529 Fixed i2c port selection for aux mag. --- flight/pios/common/pios_board_sensors.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/pios/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c index d878951e2..e07cfffd7 100644 --- a/flight/pios/common/pios_board_sensors.c +++ b/flight/pios/common/pios_board_sensors.c @@ -161,13 +161,13 @@ void PIOS_BOARD_Sensors_Configure() if (option == AUXMAGSETTINGS_TYPE_FLEXI) { // i2c_external -#ifdef PIOS_I2C_EXTERNAL_ADAPTER - i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; +#ifdef PIOS_I2C_FLEXI_ADAPTER + i2c_id = PIOS_I2C_FLEXI_ADAPTER; #endif } else if (option == AUXMAGSETTINGS_TYPE_I2C) { // i2c_internal (or Sparky2/F3 dedicated I2C port) -#ifdef PIOS_I2C_FLEXI_ADAPTER - i2c_id = PIOS_I2C_FLEXI_ADAPTER; +#ifdef PIOS_I2C_EXTERNAL_ADAPTER + i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; #endif } From a00af53710b75789da0edad8b6b0d66c026963e2 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 9 Jun 2017 12:14:43 +0200 Subject: [PATCH 2/4] LP-529 [F3] [I2C] Fix nack handling. --- flight/pios/stm32f30x/pios_i2c.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/flight/pios/stm32f30x/pios_i2c.c b/flight/pios/stm32f30x/pios_i2c.c index 8644c6f07..2a8b83535 100644 --- a/flight/pios/stm32f30x/pios_i2c.c +++ b/flight/pios/stm32f30x/pios_i2c.c @@ -260,6 +260,7 @@ static void go_starting(struct pios_i2c_adapter *i2c_adapter, __attribute__((unu i2c_adapter->active_byte = &(i2c_adapter->active_txn->buf[0]); i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); + I2C_ClearITPendingBit(i2c_adapter->cfg->regs, I2C_IT_STOPI | I2C_IT_NACKI | I2C_IT_ERRI); I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_NACKI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_TXI, ENABLE); I2C_TransferHandling( @@ -305,8 +306,9 @@ static void go_nack(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused) { i2c_adapter->nack = true; 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); - I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); - I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); + /* It seems that we don't need this with F3 i2c peripheral */ + // I2C_AcknowledgeConfig(i2c_adapter->cfg->regs, DISABLE); + // I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); } static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event, bool *woken) From b0fa0edb6424b0eb849596b648f7ca9464e2dbb8 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 14 Jun 2017 16:05:52 +0200 Subject: [PATCH 3/4] LP-529 [F3] [I2C] Move i2c bus unlocking down to irq, to save one context switch and make bus available for use by other drivers sooner. --- flight/pios/common/pios_mpu6000.c | 4 +- flight/pios/inc/pios_i2c.h | 12 ++++- flight/pios/inc/pios_i2c_priv.h | 2 + flight/pios/stm32f10x/pios_i2c.c | 11 ++-- flight/pios/stm32f30x/pios_i2c.c | 86 ++++++++++++++----------------- flight/pios/stm32f4xx/pios_i2c.c | 24 ++++----- 6 files changed, 72 insertions(+), 67 deletions(-) diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index a95dc846f..ace15cac5 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -570,9 +570,9 @@ static int32_t PIOS_MPU6000_I2C_GetReg(uint8_t address) return data; } -static bool PIOS_MPU6000_I2C_Read_Completed() +static bool PIOS_MPU6000_I2C_Read_Completed(enum pios_i2c_transfer_result result) { - return PIOS_MPU6000_HandleData(gyro_read_timestamp); + return (result == PIOS_I2C_TRANSFER_OK) ? PIOS_MPU6000_HandleData(gyro_read_timestamp) : false; } static bool PIOS_MPU6000_I2C_ReadSensor(bool *woken) diff --git a/flight/pios/inc/pios_i2c.h b/flight/pios/inc/pios_i2c.h index 546a51e2f..b37537bda 100644 --- a/flight/pios/inc/pios_i2c.h +++ b/flight/pios/inc/pios_i2c.h @@ -73,7 +73,17 @@ enum pios_i2c_error_count { PIOS_I2C_ERROR_COUNT_NUMELEM, }; -typedef bool (*pios_i2c_callback)(); +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_UNSPECIFIED_ERROR = -5, + PIOS_I2C_TRANSFER_DEVICE_ERROR = -6, +}; + +typedef bool (*pios_i2c_callback)(enum pios_i2c_transfer_result result); /* Public Functions */ extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns); diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h index 20fec3e5d..7be45ba77 100644 --- a/flight/pios/inc/pios_i2c_priv.h +++ b/flight/pios/inc/pios_i2c_priv.h @@ -73,6 +73,8 @@ struct pios_i2c_adapter { uint8_t *active_byte; uint8_t *last_byte; + + enum pios_i2c_transfer_result *transfer_result; }; int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg); diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c index e18edc2fe..c411d7eac 100644 --- a/flight/pios/stm32f10x/pios_i2c.c +++ b/flight/pios/stm32f10x/pios_i2c.c @@ -980,7 +980,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; if (!PIOS_I2C_validate(i2c_adapter)) { - return -1; + return PIOS_I2C_TRANSFER_DEVICE_ERROR; } PIOS_DEBUG_Assert(txn_list); @@ -993,9 +993,10 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else +#error PIOS_I2C_Transfer is broken for use without FreeRTOS uint32_t timeout = 0xfff; while (i2c_adapter->busy && --timeout) { ; @@ -1071,9 +1072,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], } #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - 0; + return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT : + i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR : + PIOS_I2C_TRANSFER_OK; } diff --git a/flight/pios/stm32f30x/pios_i2c.c b/flight/pios/stm32f30x/pios_i2c.c index 2a8b83535..831c5f30e 100644 --- a/flight/pios/stm32f30x/pios_i2c.c +++ b/flight/pios/stm32f30x/pios_i2c.c @@ -135,7 +135,6 @@ static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter, bool static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event, bool *woken); static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter); static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter); -static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, bool *woken); #if defined(PIOS_I2C_DIAGNOSTICS) static void i2c_adapter_log_fault(struct pios_i2c_adapter *i2c_adapter, enum pios_i2c_error_type type); @@ -234,9 +233,18 @@ 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); - if (i2c_adapter->callback) { - i2c_adapter_callback_handler(i2c_adapter, woken); - } else { + 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; + + if (i2c_adapter->transfer_result) { + *(i2c_adapter->transfer_result) = result; + } + + pios_i2c_callback cb = i2c_adapter->callback; + + 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) { @@ -249,6 +257,23 @@ static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken) } #endif /* PIOS_INCLUDE_FREERTOS */ } + +#ifdef PIOS_INCLUDE_FREERTOS + /* Unlock the bus */ + signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken); + if (xHigherPriorityTaskWoken == pdTRUE) { + *woken = true; + } +#else + PIOS_IRQ_Disable(); + i2c_adapter->busy = 0; + PIOS_IRQ_Enable(); +#endif /* PIOS_INCLUDE_FREERTOS */ + + if(cb && cb(result)) { /* User provided callback? Do it, but with bus unlocked */ + *woken = true; + } } static void go_starting(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken) @@ -609,7 +634,8 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], { // FIXME: only supports transfer sizes up to 255 bytes struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; - + enum pios_i2c_transfer_result result = PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR; + bool valid = PIOS_I2C_validate(i2c_adapter); PIOS_Assert(valid) @@ -624,13 +650,13 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else PIOS_IRQ_Disable(); if (i2c_adapter->busy) { PIOS_IRQ_Enable(); - return -2; + return PIOS_I2C_TRANSFER_BUSY; } i2c_adapter->busy = 1; PIOS_IRQ_Enable(); @@ -638,6 +664,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + i2c_adapter->transfer_result = &result; i2c_adapter->last_txn = &txn_list[num_txns - 1]; i2c_adapter->active_txn = &txn_list[0]; i2c_adapter->bus_error = false; @@ -659,30 +686,22 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], xSemaphoreGive(i2c_adapter->sem_ready); #endif /* PIOS_INCLUDE_FREERTOS */ -#ifdef PIOS_INCLUDE_FREERTOS - /* Unlock the bus */ - xSemaphoreGive(i2c_adapter->sem_busy); + /* After this point, the bus is already unlocked from ISR + * Touching i2c_adapter is not allowed */ #if defined(PIOS_I2C_DIAGNOSTICS) if (!semaphore_success) { i2c_timeout_counter++; + result = PIOS_I2C_TRANSFER_TIMEOUT; } #endif -#else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); -#endif /* PIOS_INCLUDE_FREERTOS */ - - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - i2c_adapter->nack ? -3 : - 0; + return result; } static int32_t PIOS_I2C_Transfer_Callback_Internal(struct pios_i2c_adapter *i2c_adapter, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback) { PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED); + i2c_adapter->transfer_result = 0; i2c_adapter->last_txn = &txn_list[num_txns - 1]; i2c_adapter->active_txn = &txn_list[0]; i2c_adapter->bus_error = false; @@ -773,33 +792,6 @@ int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c return PIOS_I2C_Transfer_Callback_Internal(i2c_adapter, txn_list, num_txns, callback); } - -static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, bool *woken) -{ -#ifdef PIOS_INCLUDE_FREERTOS - /* Unlock the bus */ - signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken); - if (xHigherPriorityTaskWoken == pdTRUE) { - *woken = true; - } -#else - PIOS_IRQ_Disable(); - i2c_adapter->busy = 0; - PIOS_IRQ_Enable(); -#endif /* PIOS_INCLUDE_FREERTOS */ - - - // Execute user supplied function - - if (i2c_adapter->callback()) { - *woken = true; - } - - return !i2c_adapter->bus_error; -} - - void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) { struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index 308ed8236..92bb65bc5 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -888,7 +888,7 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, s // Execute user supplied function - if (i2c_adapter->callback()) { + if (i2c_adapter->callback(PIOS_I2C_TRANSFER_OK)) { *pxHigherPriorityTaskWoken = pdTRUE; } @@ -1048,7 +1048,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; if (!PIOS_I2C_validate(i2c_adapter)) { - return -1; + return PIOS_I2C_TRANSFER_DEVICE_ERROR; } PIOS_DEBUG_Assert(txn_list); @@ -1061,13 +1061,13 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else PIOS_IRQ_Disable(); if (i2c_adapter->busy) { PIOS_IRQ_Enable(); - return -2; + return PIOS_I2C_TRANSFER_BUSY; } i2c_adapter->busy = 1; PIOS_IRQ_Enable(); @@ -1134,10 +1134,10 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], PIOS_IRQ_Enable(); #endif /* USE_FREERTOS */ - return !semaphore_success ? -2 : - i2c_adapter->bus_error ? -1 : - i2c_adapter->nack ? -3 : - 0; + return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT : + i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR : + i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK : + PIOS_I2C_TRANSFER_OK; } static int32_t PIOS_I2C_Transfer_Callback_Internal(struct pios_i2c_adapter *i2c_adapter, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback) @@ -1182,13 +1182,13 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx portTickType timeout; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else PIOS_IRQ_Disable(); if (i2c_adapter->busy) { PIOS_IRQ_Enable(); - return -2; + return PIOS_I2C_TRANSFER_BUSY; } i2c_adapter->busy = 1; PIOS_IRQ_Enable(); @@ -1220,13 +1220,13 @@ int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c } if (!locked) { - return -2; + return PIOS_I2C_TRANSFER_BUSY; } #else PIOS_IRQ_Disable(); if (i2c_adapter->busy) { PIOS_IRQ_Enable(); - return -2; + return PIOS_I2C_TRANSFER_BUSY; } i2c_adapter->busy = 1; PIOS_IRQ_Enable(); From 429f8c41be21c03aef5802163daedec41ddd2c41 Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Wed, 14 Jun 2017 16:07:44 +0200 Subject: [PATCH 4/4] LP-529 [I2C] [MS5611] Remove PIOS_DELAY_WaitmS() and move (re)initialization into fsm. --- flight/pios/common/pios_ms5611.c | 217 ++++++++++++++----------------- flight/pios/inc/pios_i2c.h | 10 +- flight/pios/inc/pios_i2c_priv.h | 2 +- flight/pios/stm32f30x/pios_i2c.c | 14 +- 4 files changed, 108 insertions(+), 135 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index d52675079..fdcdedaa4 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -33,9 +33,6 @@ #include #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; } diff --git a/flight/pios/inc/pios_i2c.h b/flight/pios/inc/pios_i2c.h index b37537bda..2f5a99e12 100644 --- a/flight/pios/inc/pios_i2c.h +++ b/flight/pios/inc/pios_i2c.h @@ -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, }; diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h index 7be45ba77..70e0840eb 100644 --- a/flight/pios/inc/pios_i2c_priv.h +++ b/flight/pios/inc/pios_i2c_priv.h @@ -73,7 +73,7 @@ struct pios_i2c_adapter { uint8_t *active_byte; uint8_t *last_byte; - + enum pios_i2c_transfer_result *transfer_result; }; diff --git a/flight/pios/stm32f30x/pios_i2c.c b/flight/pios/stm32f30x/pios_i2c.c index 831c5f30e..f269f96bb 100644 --- a/flight/pios/stm32f30x/pios_i2c.c +++ b/flight/pios/stm32f30x/pios_i2c.c @@ -234,17 +234,17 @@ 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; } - + 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; } } @@ -635,7 +635,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], // FIXME: only supports transfer sizes up to 255 bytes struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; enum pios_i2c_transfer_result result = PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR; - + bool valid = PIOS_I2C_validate(i2c_adapter); PIOS_Assert(valid)