rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge branch 'next' into corvuscorax/OP-1259_Cruise_Control_Tweaks

Browse Source
This commit is contained in:
Corvus Corax 2014-05-04 11:21:52 +02:00
commit 23654cf565
7 changed files with 34 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

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

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

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

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

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

@ -37,7 +37,6 @@
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
@ -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,39 @@ 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)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
} else if (!IS_REAL(airspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
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

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

@ -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 <openpilot.h>
#include "alarms.h"
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity);
#endif // AIRSPEEDALARM_H
/**
* @}
* @}
*/