From 0d6e8c8efb76ceb8ef8d4f5b94b6f5a91dea18b2 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 30 Apr 2014 18:39:35 +0200 Subject: [PATCH] OP-1315 changed error alarm in gps_airspeedGet for |v|<1 into a warning to allow arming. cos(5deg) exchanged by a constant expression. pow (x,2.0f) exchanged by a inline function based on multiplication. Deleted arispeedalarm.h and airspeedalarm.c and exchanged AirspeedAlarm() by a direct call to AlarmsSet(). --- flight/modules/Airspeed/airspeed.c | 3 +- flight/modules/Airspeed/airspeedalarm.c | 65 ------------------- .../modules/Airspeed/baro_airspeed_etasv3.c | 7 +- flight/modules/Airspeed/baro_airspeed_mpxv.c | 9 ++- .../modules/Airspeed/baro_airspeed_ms4525do.c | 9 ++- flight/modules/Airspeed/gps_airspeed.c | 27 +++++--- flight/modules/Airspeed/inc/airspeedalarm.h | 45 ------------- 7 files changed, 31 insertions(+), 134 deletions(-) delete mode 100644 flight/modules/Airspeed/airspeedalarm.c delete mode 100644 flight/modules/Airspeed/inc/airspeedalarm.h diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index bc60159c4..8703aaa33 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -45,7 +45,6 @@ #include "baro_airspeed_etasv3.h" #include "baro_airspeed_mpxv.h" #include "gps_airspeed.h" -#include "airspeedalarm.h" #include "taskinfo.h" // Private constants @@ -159,7 +158,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) // if sensor type changed reset Airspeed alarm if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) { - AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT); lastAirspeedSensorType = airspeedSettings.AirspeedSensorType; } diff --git a/flight/modules/Airspeed/airspeedalarm.c b/flight/modules/Airspeed/airspeedalarm.c deleted file mode 100644 index c14b62ca1..000000000 --- a/flight/modules/Airspeed/airspeedalarm.c +++ /dev/null @@ -1,65 +0,0 @@ -/** - ****************************************************************************** - * @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; -} - - -/** - * @} - * @} - */ diff --git a/flight/modules/Airspeed/baro_airspeed_etasv3.c b/flight/modules/Airspeed/baro_airspeed_etasv3.c index 66f047fb8..f9666fc76 100644 --- a/flight/modules/Airspeed/baro_airspeed_etasv3.c +++ b/flight/modules/Airspeed/baro_airspeed_etasv3.c @@ -40,7 +40,6 @@ #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) @@ -65,13 +64,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); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); return; } // only calibrate if no stored calibration is available if (!airspeedSettings->ZeroPoint) { - AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); // Calibrate sensor by averaging zero point value if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; @@ -96,7 +95,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); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); } diff --git a/flight/modules/Airspeed/baro_airspeed_mpxv.c b/flight/modules/Airspeed/baro_airspeed_mpxv.c index 086159e49..6a02780e3 100644 --- a/flight/modules/Airspeed/baro_airspeed_mpxv.c +++ b/flight/modules/Airspeed/baro_airspeed_mpxv.c @@ -40,7 +40,6 @@ #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) @@ -64,7 +63,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); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); return; } if (sensor.type == PIOS_MPXV_UNKNOWN) { @@ -77,7 +76,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa break; default: airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); return; } } @@ -85,7 +84,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); if (!airspeedSettings->ZeroPoint) { - AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, 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++; @@ -110,7 +109,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); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); } #endif /* if defined(PIOS_INCLUDE_MPXV) */ diff --git a/flight/modules/Airspeed/baro_airspeed_ms4525do.c b/flight/modules/Airspeed/baro_airspeed_ms4525do.c index 181f07c47..2a850d884 100644 --- a/flight/modules/Airspeed/baro_airspeed_ms4525do.c +++ b/flight/modules/Airspeed/baro_airspeed_ms4525do.c @@ -40,7 +40,6 @@ #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) @@ -75,7 +74,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin airspeedSensor->SensorValueTemperature = -1; airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; airspeedSensor->CalibratedAirspeed = 0; - AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); return; } @@ -88,7 +87,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { calibrationCount++; filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT); - AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); return; } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { calibrationCount++; @@ -102,7 +101,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); calibrationCount = 0; } - AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); return; } } @@ -129,7 +128,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T); airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; // everything is fine so set ALARM_OK - AirspeedAlarm(SYSTEMALARMS_ALARM_OK); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); } #endif /* if defined(PIOS_INCLUDE_MS4525DO) */ diff --git a/flight/modules/Airspeed/gps_airspeed.c b/flight/modules/Airspeed/gps_airspeed.c index 794b7fa08..91319568e 100644 --- a/flight/modules/Airspeed/gps_airspeed.c +++ b/flight/modules/Airspeed/gps_airspeed.c @@ -37,7 +37,6 @@ #include "airspeedsettings.h" #include "gps_airspeed.h" #include "CoordinateConversions.h" -#include "airspeedalarm.h" #include @@ -46,6 +45,7 @@ #define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO #define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO #define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO +#define COSINE_OF_5_DEG 0.9961947f // Private types struct GPSGlobals { @@ -60,6 +60,12 @@ struct GPSGlobals { static struct GPSGlobals *gps; // Private functions +// a simple square inline function based on multiplication faster than powf(x,2.0f) +static inline float Sq(float x) +{ + return x * x; +} + /* * Initialize function loads first data sets, and allocates memory for structure. @@ -103,6 +109,11 @@ void gps_airspeedInitialize() * where "f" is the fuselage vector in earth coordinates. * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. */ +/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is + * not constant, i.e. when the change in V_gps does not solely come from a reorientation + * this error depends strongly on the time scale considered. Is the time between t1 and t2 too + * small, "spikes" absorving unconsidred acceleration will arise + */ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { float Rbe[3][3]; @@ -121,35 +132,35 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]); // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. - if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) { + if (fabsf(cosDiff) < COSINE_OF_5_DEG) { VelocityStateData gpsVelData; VelocityStateGet(&gpsVelData); 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); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); return; // do not calculate if gps velocity is insufficient... } // Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f); + float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D); // Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f); + float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]); // Airspeed magnitude is the ratio between the two difference norms float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2); - if (!IS_REAL(airspeedData->CalibratedAirspeed)) { + if (!IS_REAL(airspeed)) { airspeedData->CalibratedAirspeed = 0; airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); } 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); + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); } // Save old variables for next pass diff --git a/flight/modules/Airspeed/inc/airspeedalarm.h b/flight/modules/Airspeed/inc/airspeedalarm.h deleted file mode 100644 index 0a5d6c25f..000000000 --- a/flight/modules/Airspeed/inc/airspeedalarm.h +++ /dev/null @@ -1,45 +0,0 @@ -/** - ****************************************************************************** - * @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 -#include "alarms.h" - - -bool AirspeedAlarm(SystemAlarmsAlarmOptions severity); - -#endif // AIRSPEEDALARM_H - -/** - * @} - * @} - */