From 8e26338f24b05e07d51847a1a9cfc5d44d2816dd Mon Sep 17 00:00:00 2001 From: Andres <> Date: Thu, 27 Mar 2014 09:32:17 +0100 Subject: [PATCH] OP-1273 I2C Alarm reset to default when AirspeedSensorType is changed. Small cosmetic corrections --- flight/modules/Airspeed/airspeed.c | 19 ++++++++++++++++++- .../modules/Airspeed/baro_airspeed_ms4525do.c | 12 ++++++------ .../modules/StateEstimation/stateestimation.c | 7 ++++--- 3 files changed, 28 insertions(+), 10 deletions(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 078aef03f..0da2d149d 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -60,7 +60,7 @@ static xTaskHandle taskHandle; static bool airspeedEnabled = false; static AirspeedSettingsData airspeedSettings; - +static AirspeedSettingsAirspeedSensorTypeOptions lastAirspeedSensorType = 0; static int8_t airspeedADCPin = -1; @@ -118,6 +118,8 @@ int32_t AirspeedInitialize() airspeedADCPin = i; } } + + lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; AirspeedSensorInitialize(); AirspeedSettingsInitialize(); @@ -153,6 +155,21 @@ 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 I2C alarms + if(airspeedSettings.AirspeedSensorType != lastAirspeedSensorType){ + switch (lastAirspeedSensorType) { + // Eagletree or PixHawk => Reset I2C alams + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_PIXHAWKAIRSPEEDMS4525DO: + AlarmsDefault(SYSTEMALARMS_ALARM_I2C); + break; + // else do not reset I2C alarms + default: break; + } + lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; + } + switch (airspeedSettings.AirspeedSensorType) { #if defined(PIOS_INCLUDE_MPXV) case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index ea8480e0b..caeb44e99 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -44,9 +44,9 @@ #if defined(PIOS_INCLUDE_MS4525DO) -#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] -#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] -#define FILTER_SHIFT 5 // Barry Dorr filter parameter k +#define CALIBRATION_IDLE_MS 0 // Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 4000 // Time to spend calibrating, in [ms] +#define FILTER_SHIFT 5 // Barry Dorr filter parameter k #define P0 101325.0f // standard pressure #define CCEXPONENT 0.2857142857f // exponent of compressibility correction 2/7 @@ -57,14 +57,13 @@ // Private types -// Private functions +// Private functions definitions static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); // Private variables static uint16_t calibrationCount = 0; -static uint32_t filter_reg; // Barry Dorr filter register - +static uint32_t filter_reg = 0; // Barry Dorr filter register void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) { @@ -99,6 +98,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin } +// Private functions static int8_t baro_airspeedReadMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings) { // Check to see if airspeed sensor is returning airspeedSensor diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index b8dbe8e61..c4d375f56 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -74,6 +74,7 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } + #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, EXTRACHECK) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -85,6 +86,7 @@ UNSET_MASK(states.updated, SENSORUPDATES_##shortname); \ } \ } + #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(sensorname, shortname, a1, a2, EXTRACHECK) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ sensorname##Data s; \ @@ -108,6 +110,7 @@ s.a3 = states.shortname[2]; \ statename##Set(&s); \ } + #define EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(statename, shortname, a1, a2) \ if (IS_SET(states.updated, SENSORUPDATES_##shortname)) { \ statename##Data s; \ @@ -117,8 +120,8 @@ statename##Set(&s); \ } -// Private types +// Private types struct filterPipelineStruct; typedef const struct filterPipelineStruct { @@ -392,8 +395,6 @@ static void StateEstimationCb(void) FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); -// FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE); -// states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now // GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself