rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-1273 OP-1282 OP-1283 bug fixes on I2C library: check for intialization and hot-plug robustness

Browse Source
This commit is contained in:
Andres 2014-03-31 22:16:29 +02:00
parent cf1c5b3f36
commit 5ed417323e
5 changed files with 72 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/modules/Airspeed/airspeed.c
View File

@ -148,6 +148,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1) {

66
flight/modules/Airspeed/baro_airspeed_ms4525do.c
View File

@ -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) */
/**

43
flight/pios/common/pios_ms4525do.c
View File

@ -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;
}

4
flight/pios/inc/pios_i2c_priv.h
View File

@ -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;

61
flight/pios/stm32f4xx/pios_i2c.c
View File

@ -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 <pios_i2c_priv.h>
/* 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;i<num_txns;i++){
i2c_adapter->transfer_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;i<num_txns;i++){
i2c_adapter->transfer_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) {