From 5ed417323e81b6f4ae0ef90fd93ed884d3a2ac30 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Mon, 31 Mar 2014 22:16:29 +0200 Subject: [PATCH] OP-1273 OP-1282 OP-1283 bug fixes on I2C library: check for intialization and hot-plug robustness --- flight/modules/Airspeed/airspeed.c | 1 + .../modules/Airspeed/baro_airspeed_ms4525do.c | 66 +++++-------------- flight/pios/common/pios_ms4525do.c | 43 ------------ flight/pios/inc/pios_i2c_priv.h | 4 ++ flight/pios/stm32f4xx/pios_i2c.c | 61 ++++++++++++++--- 5 files changed, 72 insertions(+), 103 deletions(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index f2810cadd..bc60159c4 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -148,6 +148,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + // Main task loop portTickType lastSysTime = xTaskGetTickCount(); while (1) { diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index 86091ff6f..181f07c47 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -54,13 +54,9 @@ #define CASFACTOR 760.8802669f // sqrt(5) * speed of sound at standard #define TASFACTOR 0.05891022589f // 1/sqrt(T0) -#define max(x, y) ((x) >= (y) ? (x) : (y)) - // Private types // Private functions definitions -static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); - // Private variables static uint16_t calibrationCount = 0; @@ -68,64 +64,32 @@ static uint32_t filter_reg = 0; // Barry Dorr filter register void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) { - // request measurement first - int8_t retVal = PIOS_MS4525DO_Request(); - - if (retVal != 0) { - AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); - - return; - } - - // Datasheet of MS4525DO: conversion needs 0.5 ms + 20% more when status bit used - // delay by one Tick or at least 2 ms - const portTickType xDelay = max(2 / portTICK_RATE_MS, 1); - vTaskDelay(xDelay); - - // read the sensor - retVal = baro_airspeedReadMS4525DO(airspeedSensor, airspeedSettings); - - switch (retVal) { - case 0: AirspeedAlarm(SYSTEMALARMS_ALARM_OK); - break; - case -4: - case -5: - case -7: AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); - break; - case -1: - case -2: - case -3: - case -6: - default: AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); - } -} - - -// Private functions -static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) -{ - // Check to see if airspeed sensor is returning airspeedSensor uint16_t values[2]; + + // Read sensor int8_t retVal = PIOS_MS4525DO_Read(values); - if (retVal == 0) { - airspeedSensor->SensorValue = values[0]; - airspeedSensor->SensorValueTemperature = values[1]; - } else { + // check for errors + if (retVal != 0) { airspeedSensor->SensorValue = -1; airspeedSensor->SensorValueTemperature = -1; airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; airspeedSensor->CalibratedAirspeed = 0; - return retVal; + AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); + return; } + airspeedSensor->SensorValue = values[0]; + airspeedSensor->SensorValueTemperature = values[1]; + // only calibrate if no stored calibration is available if (!airspeedSettings->ZeroPoint) { // Calibrate sensor by averaging zero point value if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT); - return -7; + AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); + return; } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { calibrationCount++; // update filter register @@ -138,7 +102,8 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); calibrationCount = 0; } - return -7; + AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); + return; } } @@ -163,11 +128,10 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * CASFACTOR * sqrtf(powf(fabsf(dP) / P0 + 1.0f, CCEXPONENT) - 1.0f); airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - - return retVal; + // everything is fine so set ALARM_OK + AirspeedAlarm(SYSTEMALARMS_ALARM_OK); } - #endif /* if defined(PIOS_INCLUDE_MS4525DO) */ /** diff --git a/flight/pios/common/pios_ms4525do.c b/flight/pios/common/pios_ms4525do.c index 8a8fc9aab..8ad7ce093 100644 --- a/flight/pios/common/pios_ms4525do.c +++ b/flight/pios/common/pios_ms4525do.c @@ -34,10 +34,6 @@ #ifdef PIOS_INCLUDE_MS4525DO /* Local Defs and Variables */ -static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len); -static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len); - -static bool pios_ms4525do_requested = false; static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len) @@ -56,46 +52,10 @@ static int8_t PIOS_MS4525DO_ReadI2C(uint8_t *buffer, uint8_t len) } -static int8_t PIOS_MS4525DO_WriteI2C(uint8_t *buffer, uint8_t len) -{ - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = MS4525DO_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_MS4525DO_ADAPTER, txn_list, NELEMENTS(txn_list)); -} - - -int8_t PIOS_MS4525DO_Request(void) -{ - // MS4525DO expects a zero length write. - // Sending one byte is a workaround that works for the moment - uint8_t data = 0; - int8_t retVal = PIOS_MS4525DO_WriteI2C(&data, sizeof(data)); - - /* requested only when transfer worked */ - pios_ms4525do_requested = (retVal == 0); - - return retVal; -} - - // values has to ba an arrray with two elements // values stay untouched on error int8_t PIOS_MS4525DO_Read(uint16_t *values) { - if (!pios_ms4525do_requested) { - /* Do not try to read when not requested */ - /* else probably stale data will be obtained */ - return -4; - } - uint8_t data[4]; int8_t retVal = PIOS_MS4525DO_ReadI2C(data, sizeof(data)); @@ -117,9 +77,6 @@ int8_t PIOS_MS4525DO_Read(uint16_t *values) values[1] += data[3]; values[1] = (values[1] >> 5); - /* not requested anymore, only when transfer worked */ - pios_ms4525do_requested = !(retVal == 0); - return retVal; } diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h index aeeff5648..b2f340c83 100644 --- a/flight/pios/inc/pios_i2c_priv.h +++ b/flight/pios/inc/pios_i2c_priv.h @@ -92,6 +92,10 @@ struct pios_i2c_adapter { uint8_t busy; #endif + /* variables for transfer timeouts */ + uint32_t transfer_delay_uS; // approx time to transfer one byte, calculated later basen on setting use here time based on 100 kbits/s + uint32_t transfer_timeout_ticks; // take something tha makes sense for small transaction, calculated later based upon transmission desired + bool bus_error; bool nack; diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c index a5a7b6969..587afe966 100644 --- a/flight/pios/stm32f4xx/pios_i2c.c +++ b/flight/pios/stm32f4xx/pios_i2c.c @@ -754,6 +754,11 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) /* Initialize the I2C block */ I2C_Init(i2c_adapter->cfg->regs, (I2C_InitTypeDef *)&(i2c_adapter->cfg->init)); + // for delays during transfer timeouts + // one tick correspond to transmission of 1 byte i.e. 9 clock ticks + i2c_adapter->transfer_delay_uS=9000000/(((I2C_InitTypeDef *)&(i2c_adapter->cfg->init))->I2C_ClockSpeed); + + if (i2c_adapter->cfg->regs->SR2 & I2C_FLAG_BUSY) { /* Reset the I2C block */ I2C_SoftwareResetCmd(i2c_adapter->cfg->regs, ENABLE); @@ -761,7 +766,6 @@ static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter) } } -#include /* Return true if the FSM is in a terminal state */ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter) @@ -789,9 +793,16 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter) xSemaphoreGive(i2c_adapter->sem_ready); #endif /* USE_FREERTOS */ + /* transfer_timeout_ticks is set by PIOS_I2C_Transfer_Callback */ /* Spin waiting for the transfer to finish */ while (!i2c_adapter_fsm_terminated(i2c_adapter)) { - ; + //sleep 1 byte, as FSM can't be faster + // FIXME: clock delay could make problems, but citing NPX: alsmost no slave device implements clock delay + // three times the expected time should cover clock delay + PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS); + + i2c_adapter->transfer_timeout_ticks--; + if(i2c_adapter->transfer_timeout_ticks==0)break; } if (i2c_adapter_wait_for_stopped(i2c_adapter)) { @@ -957,6 +968,7 @@ int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg) NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->event.init)); NVIC_Init((NVIC_InitTypeDef *)&(i2c_adapter->cfg->error.init)); + /* No error */ return 0; @@ -970,6 +982,8 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], bool valid = PIOS_I2C_validate(i2c_adapter); + if(!valid)return -1; + PIOS_Assert(valid) PIOS_DEBUG_Assert(txn_list); @@ -1005,6 +1019,15 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif + // used in the i2c_adapter_callback_handler function + // Estimate bytes of transmission. Per txns: 1 adress byte + length + i2c_adapter->transfer_timeout_ticks= num_txns; + for(uint32_t i=0;itransfer_timeout_ticks += txn_list[i].len; + } + // timeout if it takes three times the expected time + i2c_adapter->transfer_timeout_ticks*=3; + i2c_adapter->callback = NULL; i2c_adapter->bus_error = false; i2c_adapter->nack = false; @@ -1018,9 +1041,15 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], /* Spin waiting for the transfer to finish */ while (!i2c_adapter_fsm_terminated(i2c_adapter)) { - ; + /* sleep 9 clock ticks (1 byte), because FSM can't be faster than one byte + FIXME: clock delay could make problems, but citing NPX: alsmost no slave device implements clock delay + three times the expected time should cover clock delay */ + PIOS_DELAY_WaituS(i2c_adapter->transfer_delay_uS); + + i2c_adapter->transfer_timeout_ticks--; + if(i2c_adapter->transfer_timeout_ticks==0)break; } - + if (i2c_adapter_wait_for_stopped(i2c_adapter)) { i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED); } else { @@ -1051,6 +1080,8 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx bool valid = PIOS_I2C_validate(i2c_adapter); + if(!valid)return -1; + PIOS_Assert(valid) PIOS_Assert(callback); @@ -1084,6 +1115,15 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE); #endif + // used in the i2c_adapter_callback_handler function + // Estimate bytes of transmission. Per txns: 1 adress byte + length + i2c_adapter->transfer_timeout_ticks= num_txns; + for(uint32_t i=0;itransfer_timeout_ticks += txn_list[i].len; + } + // timeout if it takes three times the expected time + i2c_adapter->transfer_timeout_ticks*=3; + i2c_adapter->callback = callback; i2c_adapter->bus_error = false; i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START); @@ -1097,6 +1137,8 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) bool valid = PIOS_I2C_validate(i2c_adapter); + if(!valid)return; + PIOS_Assert(valid) uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); @@ -1205,9 +1247,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id) /* Ignore this event and wait for TRANSMITTED in case we can't keep up */ goto skip_event; break; - case 0x30084: /* Occurs between byte tranmistted and master mode selected */ - case 0x30000: /* Need to throw away this spurious event */ - case 0x30403 & EVENT_MASK: /* Detected this after got a NACK, probably stop bit */ + case 0x30084: /* BUSY + MSL + TXE + BFT Occurs between byte transmitted and master mode selected */ + case 0x30000: /* BUSY + MSL Need to throw away this spurious event */ + case 0x30403 & EVENT_MASK: /* BUSY + MSL + SB + ADDR Detected this after got a NACK, probably stop bit */ goto skip_event; break; default: @@ -1230,14 +1272,15 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id) bool valid = PIOS_I2C_validate(i2c_adapter); + if(!valid)return; + PIOS_Assert(valid) -#if defined(PIOS_I2C_DIAGNOSTICS) uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs); +#if defined(PIOS_I2C_DIAGNOSTICS) i2c_erirq_history[i2c_erirq_history_pointer] = event; i2c_erirq_history_pointer = (i2c_erirq_history_pointer + 1) % 5; - #endif if (event & I2C_FLAG_AF) {