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

Merge remote-tracking branch 'origin/andrecillo/OP-1273_OP-1282_OP-1283_Diverse_Airspeedsensor_improvements' into next

Browse Source 
Conflicts:
	ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp
This commit is contained in:
Alessio Morale 2014-04-15 21:52:34 +02:00
commit f01cf66095
12 changed files with 249 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,6 +45,7 @@
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "airspeedalarm.h"
#include "taskinfo.h"
// Private constants
@ -147,6 +148,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1) {
@ -155,18 +157,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// Update the airspeed object
AirspeedSensorGet(&airspeedData);
// if sensor type changed and the last sensor was
// either Eagletree or PixHawk, reset Airspeed alarm
// if sensor type changed reset Airspeed alarm
if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
switch (lastAirspeedSensorType) {
// Eagletree or PixHawk => Reset Airspeed alams
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO:
AlarmsDefault(SYSTEMALARMS_ALARM_AIRSPEED);
break;
// else do not reset Airspeed alarm
default: break;
}
AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
}
@ -196,6 +189,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
break;
}
// Set the UAVO

65
flight/modules/Airspeed/airspeedalarm.c Normal file
View File

@ -0,0 +1,65 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @{
*
* @file airspeedalarm.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: none
*
* Handle locally airspeed alarms issue changes to PIOS only when necessary
*
*/
#include "airspeedalarm.h"
// local variable
static SystemAlarmsAlarmOptions severitySet = SYSTEMALARMS_ALARM_UNINITIALISED;
// functions
/**
* Handle airspeed alarms and isuue an Alarm to PIOS only if necessary
*/
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity)
{
if (severity == severitySet) {
return false;
}
severitySet = severity;
return AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, severity) == 0;
}
/**
* @}
* @}
*/

4
flight/modules/Airspeed/baro_airspeed_etasv3.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_ETASV3)
@ -64,11 +65,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -93,6 +96,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
// Compute airspeed
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}

6
flight/modules/Airspeed/baro_airspeed_mpxv.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_MPXV)
@ -63,7 +64,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
// Ensure that the ADC pin is properly configured
if (airspeedADCPin < 0) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
if (sensor.type == PIOS_MPXV_UNKNOWN) {
@ -76,6 +77,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
break;
default:
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
}
@ -83,6 +85,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
if (!airspeedSettings->ZeroPoint) {
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize.
calibrationCount++;
@ -107,6 +110,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MPXV) */

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

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#include "taskinfo.h"
#if defined(PIOS_INCLUDE_MS4525DO)
@ -53,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;
@ -67,63 +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) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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: AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
break;
case -4:
case -5:
case -7: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
break;
case -1:
case -2:
case -3:
case -6:
default: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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
@ -136,7 +102,8 @@ static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, Airs
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
calibrationCount = 0;
}
return -7;
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
return;
}
}
@ -161,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) */
/**

4
flight/modules/Airspeed/gps_airspeed.c
View File

@ -37,6 +37,7 @@
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
@ -127,6 +128,7 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return; // do not calculate if gps velocity is insufficient...
}
@ -141,11 +143,13 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass

45
flight/modules/Airspeed/inc/airspeedalarm.h Normal file
View File

@ -0,0 +1,45 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @{
*
* @file airspeedalarm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, reads temperature and pressure from MS4525DO
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AIRSPEEDALARM_H
#define AIRSPEEDALARM_H
#include <openpilot.h>
#include "alarms.h"
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity);
#endif // AIRSPEEDALARM_H
/**
* @}
* @}
*/

20
flight/modules/Attitude/revolution/attitude.c
View File

@ -1072,13 +1072,22 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
vel[2] = gpsVelData.Down;
}
// Copy the position into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// airspeed correction needs current positionState
if (airspeed_updated) {
// we have airspeed available
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed;
AirspeedStateSet(&airspeed);
@ -1108,14 +1117,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors);
}
// Copy the position and velocity into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// Copy the velocity into the UAVO
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = Nav.Vel[0];

44
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,50 +52,15 @@ 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));
uint8_t status = data[0] & 0xC0;
if (status == 0x80) {
/* stale data */
return -5;
@ -117,9 +78,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;
}

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

@ -92,8 +92,12 @@ struct pios_i2c_adapter {
uint8_t busy;
#endif
bool bus_error;
bool nack;
/* 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;
volatile enum i2c_adapter_state curr_state;
const struct pios_i2c_txn *first_txn;

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

@ -933,9 +933,9 @@ 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;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return -1;
}
PIOS_DEBUG_Assert(txn_list);
PIOS_DEBUG_Assert(num_txns);
@ -977,6 +977,14 @@ 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
// 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 eight times the expected time
i2c_adapter->transfer_timeout_ticks <<= 3;
i2c_adapter->bus_error = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
@ -992,7 +1000,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 streching could make problems, but citing NPX: alsmost no slave device implements clock stretching
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)) {
@ -1019,9 +1035,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
@ -1145,9 +1161,10 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
PIOS_Assert(valid)
#if defined(PIOS_I2C_DIAGNOSTICS)
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);

83
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,18 @@ 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 stretching could make problems, but citing NPX: alsmost no slave device implements clock stretching
// 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 +970,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;
@ -968,9 +982,9 @@ 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;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return -1;
}
PIOS_DEBUG_Assert(txn_list);
PIOS_DEBUG_Assert(num_txns);
@ -1002,12 +1016,20 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
#ifdef USE_FREERTOS
/* Make sure the done/ready semaphore is consumed before we start */
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
#endif
// 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 eight times the expected time
i2c_adapter->transfer_timeout_ticks <<= 3;
i2c_adapter->callback = NULL;
i2c_adapter->bus_error = false;
i2c_adapter->nack = false;
i2c_adapter->nack = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
/* Wait for the transfer to complete */
@ -1018,7 +1040,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 stretching could make problems, but citing NPX: alsmost no slave device implements clock stretching
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)) {
@ -1049,9 +1079,10 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
if (!PIOS_I2C_validate(i2c_adapter)) {
return -1;
}
PIOS_Assert(valid)
PIOS_Assert(callback);
PIOS_DEBUG_Assert(txn_list);
@ -1081,9 +1112,18 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
#ifdef USE_FREERTOS
/* Make sure the done/ready semaphore is consumed before we start */
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
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 eight 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);
@ -1095,9 +1135,9 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
if (!PIOS_I2C_validate(i2c_adapter)) {
return;
}
uint32_t event = I2C_GetLastEvent(i2c_adapter->cfg->regs);
@ -1205,9 +1245,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:
@ -1228,16 +1268,15 @@ void PIOS_I2C_ER_IRQ_Handler(uint32_t i2c_id)
{
struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
if (!PIOS_I2C_validate(i2c_adapter)) {
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) {