rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-1273 OP-1282 OP-1283 diverse improvements in airspeed sensor handling including implementation of PixHaw sensor, airspeedalarm in Systemhealth and in all airspeed sensors; file changes

Browse Source
This commit is contained in:
Andres 2014-03-28 16:59:52 +01:00
parent ef78320112
commit 9a207fbb80
5 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
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
@ -155,18 +156,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 +188,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
break;
}
// Set the UAVO

7
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,13 +65,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_ERROR);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -95,7 +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;
AlarmsClear(SYSTEMALARMS_ALARM_I2C);
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) */

10
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)
@ -71,7 +72,8 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
int8_t retVal = PIOS_MS4525DO_Request();
if (retVal != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
@ -84,17 +86,17 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
retVal = baro_airspeedReadMS4525DO(airspeedSensor, airspeedSettings);
switch (retVal) {
case 0: AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
case 0: AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
break;
case -4:
case -5:
case -7: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
case -7: AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
break;
case -1:
case -2:
case -3:
case -6:
default: AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
default: AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
}
}

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