From aa5e5a9b0126c039e453380476a10d52114caecd Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Mon, 23 Jul 2012 16:09:36 +0200 Subject: [PATCH] Removed "airspeed" field from Airspeed UAVO. Now everything is done either with true airspeed or calibrated airspeed --- flight/Modules/Airspeed/revolution/airspeed.c | 43 +++++++++++++------ .../Airspeed/revolution/baro_airspeed.c | 8 ++-- .../fixedwingpathfollower.c | 25 ++++++----- flight/Modules/PathPlanner/pathplanner.c | 2 +- .../src/plugins/hitlnew/fgsimulator.cpp | 2 +- .../plugins/hitlnew/hitlnoisegeneration.cpp | 2 +- .../src/plugins/hitlnew/il2simulator.cpp | 2 +- .../src/plugins/hitlnew/simulator.cpp | 2 +- .../src/plugins/hitlnew/simulator.h | 2 +- .../src/plugins/hitlnew/xplanesimulator.cpp | 2 +- shared/uavobjectdefinition/baroairspeed.xml | 3 +- 11 files changed, 55 insertions(+), 38 deletions(-) diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index f478bba12..401617a74 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -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); diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed.c b/flight/Modules/Airspeed/revolution/baro_airspeed.c index 0631833e5..70b8654f8 100644 --- a/flight/Modules/Airspeed/revolution/baro_airspeed.c +++ b/flight/Modules/Airspeed/revolution/baro_airspeed.c @@ -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; diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c index ac7e636d8..64b6b832c 100644 --- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -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; } } diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c index 850cfa9e9..c6bbe5525 100644 --- a/flight/Modules/PathPlanner/pathplanner.c +++ b/flight/Modules/PathPlanner/pathplanner.c @@ -485,7 +485,7 @@ static uint8_t conditionAboveSpeed() { BaroAirspeedData baroAirspeed; BaroAirspeedGet (&baroAirspeed); if (baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONNECTED_TRUE) { - velocity = baroAirspeed.Airspeed; + velocity = baroAirspeed.TrueAirspeed; } } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp index 9823f2e5d..b9ca22b8d 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/fgsimulator.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp b/ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp index 3df84dee6..229330b40 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/hitlnoisegeneration.cpp @@ -72,6 +72,6 @@ Noise HitlNoiseGeneration::generateNoise(){ noise.accelData.y=0; noise.accelData.z=0; - noise.baroAirspeed.Airspeed=0; + noise.baroAirspeed.CalibratedAirspeed=0; } diff --git a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp index 0cbdf80ea..386a71e30 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp @@ -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; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp index 5c94f3be6..6b2f3c547 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h index 3afa4d227..a872fc117 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/simulator.h +++ b/ground/openpilotgcs/src/plugins/hitlnew/simulator.h @@ -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; diff --git a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp index b04d3ee10..d092eeb7d 100644 --- a/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitlnew/xplanesimulator.cpp @@ -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; diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/baroairspeed.xml index a67e44e35..6fd9d1ac6 100644 --- a/shared/uavobjectdefinition/baroairspeed.xml +++ b/shared/uavobjectdefinition/baroairspeed.xml @@ -3,8 +3,7 @@ The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. - - +