rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merged in mindnever/librepilot/LP-529_I2C_Improvements_and_bugfixes (pull request #438)

Browse Source 
LP-529 I2C Improvements and bugfixes

Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
Approved-by: Vladimir Zidar <mr_w@mindnever.org>
Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Brian Webb <webbbn@gmail.com>
Approved-by: Alessio Morale <alessiomorale@gmail.com>
This commit is contained in:
Vladimir Zidar 2017-06-26 18:38:00 +00:00 committed by Lalanne Laurent
commit ebef943762
8 changed files with 174 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/pios/common/pios_board_sensors.c
View File

@ -161,13 +161,13 @@ void PIOS_BOARD_Sensors_Configure()
if (option == AUXMAGSETTINGS_TYPE_FLEXI) { if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
// i2c_external // i2c_external
#ifdef PIOS_I2C_EXTERNAL_ADAPTER #ifdef PIOS_I2C_FLEXI_ADAPTER
i2c_id = PIOS_I2C_EXTERNAL_ADAPTER; i2c_id = PIOS_I2C_FLEXI_ADAPTER;
#endif #endif
} else if (option == AUXMAGSETTINGS_TYPE_I2C) { } else if (option == AUXMAGSETTINGS_TYPE_I2C) {
// i2c_internal (or Sparky2/F3 dedicated I2C port) // i2c_internal (or Sparky2/F3 dedicated I2C port)
#ifdef PIOS_I2C_FLEXI_ADAPTER #ifdef PIOS_I2C_EXTERNAL_ADAPTER
i2c_id = PIOS_I2C_FLEXI_ADAPTER; i2c_id = PIOS_I2C_EXTERNAL_ADAPTER;
#endif #endif
} }

4
flight/pios/common/pios_mpu6000.c
View File

@ -570,9 +570,9 @@ static int32_t PIOS_MPU6000_I2C_GetReg(uint8_t address)
return data; 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) static bool PIOS_MPU6000_I2C_ReadSensor(bool *woken)

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

@ -33,9 +33,6 @@
#include <pios_ms5611.h> #include <pios_ms5611.h>
#define POW2(x) (1 << x) #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 // Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation // Undef for normal operation
#define PIOS_MS5611_SLOW_TEMP_RATE 20 #define PIOS_MS5611_SLOW_TEMP_RATE 20
@ -44,9 +41,9 @@
#endif #endif
// Running moving average smoothing factor // Running moving average smoothing factor
#define PIOS_MS5611_TEMP_SMOOTHING 10 #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 */ /* Local Types */
typedef struct { typedef struct {
@ -61,16 +58,17 @@ typedef enum {
typedef enum { typedef enum {
MS5611_FSM_INIT = 0, MS5611_FSM_INIT = 0,
MS5611_FSM_CALIBRATION,
MS5611_FSM_TEMPERATURE, MS5611_FSM_TEMPERATURE,
MS5611_FSM_PRESSURE, MS5611_FSM_PRESSURE,
MS5611_FSM_CALCULATE, MS5611_FSM_CALCULATE,
} MS5611_FSM_State; } MS5611_FSM_State;
/* Glocal Variables */ static ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
/* Local Variables */ static MS5611CalibDataTypeDef CalibData;
MS5611CalibDataTypeDef CalibData;
static uint8_t temp_press_interleave_count = 1;
/* Straight from the datasheet */ /* Straight from the datasheet */
static uint32_t RawTemperature; static uint32_t RawTemperature;
@ -78,16 +76,12 @@ static uint32_t RawPressure;
static int64_t Pressure; static int64_t Pressure;
static int64_t Temperature; static int64_t Temperature;
static int64_t FilteredTemperature = INT32_MIN; 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 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 // Second order temperature compensation. Temperature offset
static int64_t compensation_t2; static int64_t compensation_t2;
@ -96,8 +90,13 @@ static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg; static const struct pios_ms5611_cfg *dev_cfg;
static int32_t i2c_id; static int32_t i2c_id;
static PIOS_SENSORS_1Axis_SensorsWithTemp results; 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 // sensor driver interface
bool PIOS_MS5611_driver_Test(uintptr_t context); bool PIOS_MS5611_driver_Test(uintptr_t context);
@ -118,43 +117,15 @@ const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
/** /**
* Initialise the MS5611 sensor * Initialise the MS5611 sensor
*/ */
int32_t ms5611_read_flag;
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
{ {
static uint32_t initTime; i2c_id = i2c_device;
if (cfg) { oversampling = cfg->oversampling;
i2c_id = i2c_device; conversionDelayUs = PIOS_MS5611_GetDelayUs();
oversampling = cfg->oversampling; dev_cfg = cfg; // Store cfg before enabling interrupt
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;
} }
/** /**
* Start the ADC conversion * Start the ADC conversion
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR * \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) 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 */ /* Start the conversion */
if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) { 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; return -2;
} }
} else if (Type == MS5611_CONVERSION_TYPE_PressureConv) { } 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; return -2;
} }
} }
lastConversionStart = PIOS_DELAY_GetRaw();
CurrentRead = Type; CurrentRead = Type;
return 0; 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 * @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) 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]; uint8_t Data[3];
Data[0] = 0; Data[0] = 0;
@ -256,18 +192,15 @@ int32_t PIOS_MS5611_ReadADC(void)
if (CurrentRead == MS5611_CONVERSION_TYPE_None) { if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
return -2; return -2;
} }
if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
return -1;
}
static int64_t deltaTemp; static int64_t deltaTemp;
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -2;
}
/* Read and store the 16bit result */ /* Read and store the 16bit result */
if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) { 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]; RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
// Difference between actual and reference temperature // Difference between actual and reference temperature
// dT = D2 - TREF = D2 - C5 * 2^8 // dT = D2 - TREF = D2 - C5 * 2^8
@ -288,10 +221,6 @@ int32_t PIOS_MS5611_ReadADC(void)
int64_t Offset2 = 0; int64_t Offset2 = 0;
int64_t Sens2 = 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 // check if temperature is less than 20°C
if (FilteredTemperature < 2000) { if (FilteredTemperature < 2000) {
// Apply compensation // Apply compensation
@ -327,6 +256,9 @@ int32_t PIOS_MS5611_ReadADC(void)
// P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15 // P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15
Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15); Pressure = (((((int64_t)RawPressure) * Sens) / POW2(21)) - Offset) / POW2(15);
} }
CurrentRead = MS5611_CONVERSION_TYPE_None;
return 0; 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) { enum pios_i2c_transfer_result i2c_result = PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list));
if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) {
return 0; if (i2c_result == PIOS_I2C_TRANSFER_OK) {
} return 0;
} }
sensorIsAlive = false; if (i2c_result != PIOS_I2C_TRANSFER_BUSY) {
hw_error = true;
}
return -1; 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 0 if operation was successful
* \return -1 if error during I2C transfer * \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[] = { 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) { lastCommandTimeRaw = PIOS_DELAY_GetRaw();
if (PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)) == 0) { commandDelayUs = delayuS;
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; 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. * @brief Run self-test operation.
* \return 0 if self-test succeed, -1 if failed * \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) 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; static MS5611_FSM_State next_state = MS5611_FSM_INIT;
if (!sensorIsAlive) { // try to reinit if (PIOS_DELAY_DiffuS(lastCommandTimeRaw) < commandDelayUs) {
PIOS_MS5611_Init(0, 0); 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) { switch (next_state) {
case MS5611_FSM_INIT: 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: case MS5611_FSM_TEMPERATURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv); PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
next_state = MS5611_FSM_PRESSURE; next_state = MS5611_FSM_PRESSURE;
return false; break;
case MS5611_FSM_PRESSURE: case MS5611_FSM_PRESSURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv); PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
next_state = MS5611_FSM_CALCULATE; next_state = MS5611_FSM_CALCULATE;
return false; break;
case MS5611_FSM_CALCULATE: case MS5611_FSM_CALCULATE:
temp_press_interleave_count--; temp_press_interleave_count--;
@ -519,6 +484,14 @@ bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
// it should not be there // it should not be there
PIOS_Assert(0); 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; return false;
} }

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

@ -73,7 +73,17 @@ enum pios_i2c_error_count {
PIOS_I2C_ERROR_COUNT_NUMELEM, 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 */ /* Public Functions */
extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns); extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns);

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

@ -73,6 +73,8 @@ struct pios_i2c_adapter {
uint8_t *active_byte; uint8_t *active_byte;
uint8_t *last_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); int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg);

11
flight/pios/stm32f10x/pios_i2c.c
View File

@ -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; struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
if (!PIOS_I2C_validate(i2c_adapter)) { if (!PIOS_I2C_validate(i2c_adapter)) {
return -1; return PIOS_I2C_TRANSFER_DEVICE_ERROR;
} }
PIOS_DEBUG_Assert(txn_list); 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; portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
#else #else
#error PIOS_I2C_Transfer is broken for use without FreeRTOS
uint32_t timeout = 0xfff; uint32_t timeout = 0xfff;
while (i2c_adapter->busy && --timeout) { 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 */ #endif /* USE_FREERTOS */
return !semaphore_success ? -2 : return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT :
i2c_adapter->bus_error ? -1 : i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
0; PIOS_I2C_TRANSFER_OK;
} }

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

@ -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_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_fsm_init(struct pios_i2c_adapter *i2c_adapter);
static void i2c_adapter_reset_bus(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) #if defined(PIOS_I2C_DIAGNOSTICS)
static void i2c_adapter_log_fault(struct pios_i2c_adapter *i2c_adapter, enum pios_i2c_error_type type); 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); 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) { enum pios_i2c_transfer_result result =
i2c_adapter_callback_handler(i2c_adapter, woken); i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
} else { 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 #ifdef PIOS_INCLUDE_FREERTOS
signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE;
if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) { 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 */ #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) static void go_starting(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken)
@ -260,6 +285,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->active_byte = &(i2c_adapter->active_txn->buf[0]);
i2c_adapter->last_byte = &(i2c_adapter->active_txn->buf[i2c_adapter->active_txn->len - 1]); 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_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( I2C_TransferHandling(
@ -305,8 +331,9 @@ static void go_nack(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)
{ {
i2c_adapter->nack = true; 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_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); /* It seems that we don't need this with F3 i2c peripheral */
I2C_GenerateSTOP(i2c_adapter->cfg->regs, ENABLE); // 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) static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event, bool *woken)
@ -607,6 +634,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 // FIXME: only supports transfer sizes up to 255 bytes
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; 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); bool valid = PIOS_I2C_validate(i2c_adapter);
@ -622,13 +650,13 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
portTickType timeout; portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
#else #else
PIOS_IRQ_Disable(); PIOS_IRQ_Disable();
if (i2c_adapter->busy) { if (i2c_adapter->busy) {
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
i2c_adapter->busy = 1; i2c_adapter->busy = 1;
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
@ -636,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); 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->last_txn = &txn_list[num_txns - 1];
i2c_adapter->active_txn = &txn_list[0]; i2c_adapter->active_txn = &txn_list[0];
i2c_adapter->bus_error = false; i2c_adapter->bus_error = false;
@ -657,30 +686,22 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
xSemaphoreGive(i2c_adapter->sem_ready); xSemaphoreGive(i2c_adapter->sem_ready);
#endif /* PIOS_INCLUDE_FREERTOS */ #endif /* PIOS_INCLUDE_FREERTOS */
#ifdef PIOS_INCLUDE_FREERTOS /* After this point, the bus is already unlocked from ISR
/* Unlock the bus */ * Touching i2c_adapter is not allowed */
xSemaphoreGive(i2c_adapter->sem_busy);
#if defined(PIOS_I2C_DIAGNOSTICS) #if defined(PIOS_I2C_DIAGNOSTICS)
if (!semaphore_success) { if (!semaphore_success) {
i2c_timeout_counter++; i2c_timeout_counter++;
result = PIOS_I2C_TRANSFER_TIMEOUT;
} }
#endif #endif
#else return result;
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;
} }
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) 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); 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->last_txn = &txn_list[num_txns - 1];
i2c_adapter->active_txn = &txn_list[0]; i2c_adapter->active_txn = &txn_list[0];
i2c_adapter->bus_error = false; i2c_adapter->bus_error = false;
@ -771,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); 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) void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
{ {
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id; struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;

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

@ -888,7 +888,7 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, s
// Execute user supplied function // Execute user supplied function
if (i2c_adapter->callback()) { if (i2c_adapter->callback(PIOS_I2C_TRANSFER_OK)) {
*pxHigherPriorityTaskWoken = pdTRUE; *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; struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
if (!PIOS_I2C_validate(i2c_adapter)) { if (!PIOS_I2C_validate(i2c_adapter)) {
return -1; return PIOS_I2C_TRANSFER_DEVICE_ERROR;
} }
PIOS_DEBUG_Assert(txn_list); 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; portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
#else #else
PIOS_IRQ_Disable(); PIOS_IRQ_Disable();
if (i2c_adapter->busy) { if (i2c_adapter->busy) {
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
i2c_adapter->busy = 1; i2c_adapter->busy = 1;
PIOS_IRQ_Enable(); 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(); PIOS_IRQ_Enable();
#endif /* USE_FREERTOS */ #endif /* USE_FREERTOS */
return !semaphore_success ? -2 : return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT :
i2c_adapter->bus_error ? -1 : i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
i2c_adapter->nack ? -3 : i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK :
0; 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) 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; portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS; timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) { if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
#else #else
PIOS_IRQ_Disable(); PIOS_IRQ_Disable();
if (i2c_adapter->busy) { if (i2c_adapter->busy) {
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
i2c_adapter->busy = 1; i2c_adapter->busy = 1;
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
@ -1220,13 +1220,13 @@ int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c
} }
if (!locked) { if (!locked) {
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
#else #else
PIOS_IRQ_Disable(); PIOS_IRQ_Disable();
if (i2c_adapter->busy) { if (i2c_adapter->busy) {
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();
return -2; return PIOS_I2C_TRANSFER_BUSY;
} }
i2c_adapter->busy = 1; i2c_adapter->busy = 1;
PIOS_IRQ_Enable(); PIOS_IRQ_Enable();