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

Removed "airspeed" field from Airspeed UAVO. Now everything is done either with true airspeed or calibrated airspeed

Browse Source
This commit is contained in:
Kenz Dale 2012-07-23 16:09:36 +02:00
parent 21527544f9
commit aa5e5a9b01
11 changed files with 55 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
flight/Modules/Airspeed/revolution/airspeed.c
View File

@ -79,6 +79,8 @@
#define F_PI 3.141526535897932f
#define DEG2RAD (F_PI/180.0f)
#define T0 288.15f
#define BARO_TEMPERATURE_OFFSET 5
// Private types
@ -211,12 +213,18 @@ static void airspeedTask(void *parameters)
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
//Calculate true airspeed, not taking into account compressibility effects
#define T0 288.15f
float baroTemperature;
BaroAltitudeTemperatureGet(&baroTemperature);
baroTemperature-=5; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
airspeed_tas_baro=airspeedData.CAS * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
baroTemperature-=BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
#ifdef GPS_AIRSPEED_PRESENT
//GPS present, so use baro sensor to filter TAS
airspeed_tas_baro=airspeedData.CalibratedAirspeed * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#else
//No GPS, so TAS comes only from baro sensor
airspeedData.TrueAirspeed=airspeedData.CalibratedAirspeed * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#endif
}
else
#endif
@ -266,6 +274,13 @@ static void airspeedTask(void *parameters)
//There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function.
airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP;
/* Note:
This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However,
this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't
know how to control for temperature bias.
*/
}
else
#endif
@ -274,18 +289,22 @@ static void airspeedTask(void *parameters)
//case, filter the airspeed for smoother output
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
//Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor
float baroTemperature;
BaroAltitudeTemperatureGet(&baroTemperature);
baroTemperature-=BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
airspeedData.CalibratedAirspeed =airspeedData.TrueAirspeed / sqrtf((baroTemperature+273.15)/T0);
}
}
#ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter data with baro airspeed
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=airspeedData.TrueAirspeed*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
}
#endif
#endif
//Legacy UAVO support, this needs to be removed in favor of using TAS and CAS, as appropriate.
if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_FALSE){ //If we only have a GPS, use GPS data as airspeed...
airspeedData.Airspeed=airspeedData.TrueAirspeed;
}
else{ //...otherwise, use the only baro airspeed because we still don't trust true airspeed calculations
airspeedData.Airspeed=airspeedData.CAS;
}
//Set the UAVO
BaroAirspeedSet(&airspeedData);

8
flight/Modules/Airspeed/revolution/baro_airspeed.c
View File

@ -151,10 +151,10 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT
}
//Filter CAS
float alpha=SAMPLING_DELAY_MS_MPXV/(SAMPLING_DELAY_MS_MPXV + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
float filteredAirspeed = calibratedAirspeed*(alpha) + baroAirspeedData->CAS*(1.0f-alpha);
float filteredAirspeed = calibratedAirspeed*(alpha) + baroAirspeedData->CalibratedAirspeed*(1.0f-alpha);
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
baroAirspeedData->CAS = filteredAirspeed;
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
#else
//Whoops, no sensor defined in pios_config.h. Revert to GPS.
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
@ -179,7 +179,7 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (baroAirspeedData->SensorValue==-1) {
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
baroAirspeedData->Airspeed = 0;
baroAirspeedData->CalibratedAirspeed = 0;
BaroAirspeedSet(&baroAirspeedData);
return;
}
@ -204,7 +204,7 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed?
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CAS = calibratedAirspeed;
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
#else
//Whoops, no EagleTree sensor defined in pios_config.h. Declare it unconnected...
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;

25
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -363,7 +363,7 @@ static uint8_t updateFixedDesiredAttitude()
// float groundspeedActual;
float groundspeedDesired;
float airspeedActual;
float calibratedAirspeedActual;
float airspeedDesired;
float airspeedError;
float accelActual;
@ -401,8 +401,7 @@ static uint8_t updateFixedDesiredAttitude()
// Current ground speed
// groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
// airspeedActual = groundspeedActual + baroAirspeedBias;
airspeedActual = baroAirspeed.Airspeed;
calibratedAirspeedActual = baroAirspeed.CalibratedAirspeed;
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
@ -411,7 +410,7 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerSettings.CruiseSpeed);
// Airspeed error
airspeedError = airspeedDesired - airspeedActual;
airspeedError = airspeedDesired - calibratedAirspeedActual;
// Vertical speed error
descentspeedDesired = bound (
@ -429,28 +428,28 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) {
if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
result = 0;
}
if ( airspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0;
}
if (airspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0;
}
if (airspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
if (airspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
if (airspeedActual<1e-6) {
if (calibratedAirspeedActual<1e-6) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
@ -555,7 +554,7 @@ static uint8_t updateFixedDesiredAttitude()
*/
// compute desired acceleration
if(0){
accelDesired = bound( (airspeedError/airspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
accelDesired = bound( (airspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
@ -579,7 +578,7 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = accelIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = accelCommand;
pitchCommand= -accelCommand + bound ( (-descentspeedError/airspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
pitchCommand= -accelCommand + bound ( (-descentspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
@ -715,7 +714,7 @@ static void baroAirspeedUpdatedCb(UAVObjEvent * ev)
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
baroAirspeedBias = baroAirspeed.Airspeed - groundspeed;
baroAirspeedBias = baroAirspeed.TrueAirspeed - groundspeed;
}
}

2
flight/Modules/PathPlanner/pathplanner.c
View File

@ -485,7 +485,7 @@ static uint8_t conditionAboveSpeed() {
BaroAirspeedData baroAirspeed;
BaroAirspeedGet (&baroAirspeed);
if (baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONNECTED_TRUE) {
velocity = baroAirspeed.Airspeed;
velocity = baroAirspeed.TrueAirspeed;
}
}

2
ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -311,7 +311,7 @@ void FGSimulator::processUpdate(const QByteArray& inp)
out.altitude = altitude;
out.groundspeed = groundspeed;
out.airspeed = airspeed;
out.calibratedAirspeed = airspeed;
// Update BaroAltitude object

2
ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp
View File

@ -72,6 +72,6 @@ Noise HitlNoiseGeneration::generateNoise(){
noise.accelData.y=0;
noise.accelData.z=0;
noise.baroAirspeed.Airspeed=0;
noise.baroAirspeed.CalibratedAirspeed=0;
}

2
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

@ -273,7 +273,7 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
out.longitude = settings.longitude.toFloat() * 1e7;
out.groundspeed = current.groundspeed;
out.airspeed = current.ias;
out.calibratedAirspeed = current.ias;
out.dstN=current.Y;
out.dstE=current.X;

2
ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp
View File

@ -414,7 +414,7 @@ void Simulator::updateUAVOs(Output2OP out){
// Update BaroAirspeed object
BaroAirspeed::DataFields baroAirspeedData;
memset(&baroAirspeedData, 0, sizeof(BaroAirspeed::DataFields));
baroAirspeedData.Airspeed = out.airspeed + noise.baroAirspeed.Airspeed;
baroAirspeedData.CalibratedAirspeed = out.calibratedAirspeed + noise.baroAirspeed.CalibratedAirspeed;
baroAirspeed->setData(baroAirspeedData);
//Update gyroscope sensor data

2
ground/openpilotgcs/src/plugins/hitlnew/simulator.h
View File

@ -118,7 +118,7 @@ struct Output2OP{
float altitude;
float heading;
float groundspeed; //[m/s]
float airspeed; //[m/s]
float calibratedAirspeed; //[m/s]
float pitch;
float roll;
float pressure;

2
ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -301,7 +301,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
out.altitude = altitude;
out.groundspeed = groundspeed_ktgs*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
out.airspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
out.calibratedAirspeed = airspeed_keas*1.15*1.6089/3.6; //Convert from [kts] to [m/s]
// Update BaroAltitude object
out.temperature = temperature;

3
shared/uavobjectdefinition/baroairspeed.xml
View File

@ -3,8 +3,7 @@
<description>The raw data from the dynamic pressure sensor with pressure, temperature and airspeed.</description>
<field name="BaroConnected" units="" type="enum" elements="1" options="False,True"/>
<field name="SensorValue" units="raw" type="uint16" elements="1"/>
<field name="Airspeed" units="m/s" type="float" elements="1"/>
<field name="CAS" units="m/s" type="float" elements="1"/>
<field name="CalibratedAirspeed" units="m/s" type="float" elements="1"/>
<field name="GPSAirspeed" units="m/s" type="float" elements="1"/>
<field name="TrueAirspeed" units="m/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>