From 23f9f4d9c9cd3d059b2bb211f06fb81d364e62b0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 31 May 2015 11:17:49 +0200 Subject: [PATCH] OP-1900 add failsafe to FW pathfollower to fly safely without valid airspeed estimate --- .../PathFollower/fixedwingflycontroller.cpp | 238 ++++++++++-------- .../PathFollower/inc/fixedwingflycontroller.h | 1 + .../fixedwingpathfollowersettings.xml | 3 + .../fixedwingpathfollowerstatus.xml | 2 +- 4 files changed, 142 insertions(+), 102 deletions(-) diff --git a/flight/modules/PathFollower/fixedwingflycontroller.cpp b/flight/modules/PathFollower/fixedwingflycontroller.cpp index 3721fb1e1..432b97c4e 100644 --- a/flight/modules/PathFollower/fixedwingflycontroller.cpp +++ b/flight/modules/PathFollower/fixedwingflycontroller.cpp @@ -63,6 +63,7 @@ void FixedWingFlyController::Activate(void) SettingsUpdated(); resetGlobals(); mMode = pathDesired->Mode; + lastAirspeedUpdate = 0; } } @@ -227,10 +228,11 @@ void FixedWingFlyController::updatePathVelocity(float kFF, bool limited) */ uint8_t FixedWingFlyController::updateFixedDesiredAttitude() { - uint8_t result = 1; - bool cutThrust = false; + uint8_t result = 1; + bool cutThrust = false; + bool hasAirspeed = true; - const float dT = fixedWingSettings->UpdatePeriod / 1000.0f; + const float dT = fixedWingSettings->UpdatePeriod / 1000.0f; VelocityDesiredData velocityDesired; VelocityStateData velocityState; @@ -243,7 +245,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude() float groundspeedProjection; float indicatedAirspeedState; float indicatedAirspeedDesired; - float airspeedError; + float airspeedError = 0.0f; float pitchCommand; @@ -266,56 +268,99 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude() AirspeedStateGet(&airspeedState); SystemSettingsGet(&systemSettings); + /** * Compute speed error and course */ - // missing sensors for airspeed-direction we have to assume within - // reasonable error that measured airspeed is actually the airspeed - // component in forward pointing direction - // airspeedVector is normalized - airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); - airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); - // current ground speed projected in forward direction - groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; - - // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, - // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction - // than airspeed and gps sensors alone - indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; - - // fluidMovement is a vector describing the aproximate movement vector of - // the surrounding fluid in 2d space (aka wind vector) - fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); - fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); - - // calculate the movement vector we need to fly to reach velocityDesired - - // taking fluidMovement into account - courseComponent[0] = velocityDesired.North - fluidMovement[0]; - courseComponent[1] = velocityDesired.East - fluidMovement[1]; - - indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), - fixedWingSettings->HorizontalVelMin, - fixedWingSettings->HorizontalVelMax); - - // if we could fly at arbitrary speeds, we'd just have to move towards the - // courseComponent vector as previously calculated and we'd be fine - // unfortunately however we are bound by min and max air speed limits, so - // we need to recalculate the correct course to meet at least the - // velocityDesired vector direction at our current speed - // this overwrites courseComponent - bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); - - // Error condition: wind speed too high, we can't go where we want anymore - fixedWingPathFollowerStatus.Errors.Wind = 0; - if ((!valid) && - fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on - fixedWingPathFollowerStatus.Errors.Wind = 1; + // check for airspeed sensor + fixedWingPathFollowerStatus.Errors.AirspeedSensor = 0; + if (fixedWingSettings->UseAirspeedSensor == FIXEDWINGPATHFOLLOWERSETTINGS_USEAIRSPEEDSENSOR_FALSE) { + // fallback algo triggered voluntarily + hasAirspeed = false; + fixedWingPathFollowerStatus.Errors.AirspeedSensor = 1; + } else if (PIOS_DELAY_GetuSSince(lastAirspeedUpdate) > 1000000) { + // no airspeed update in one second, assume airspeed sensor failure + hasAirspeed = false; result = 0; + fixedWingPathFollowerStatus.Errors.AirspeedSensor = 1; } - // Airspeed error - airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; + + if (hasAirspeed) { + // missing sensors for airspeed-direction we have to assume within + // reasonable error that measured airspeed is actually the airspeed + // component in forward pointing direction + // airspeedVector is normalized + airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); + airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); + + // current ground speed projected in forward direction + groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, + // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; + + // fluidMovement is a vector describing the aproximate movement vector of + // the surrounding fluid in 2d space (aka wind vector) + fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); + fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); + + // calculate the movement vector we need to fly to reach velocityDesired - + // taking fluidMovement into account + courseComponent[0] = velocityDesired.North - fluidMovement[0]; + courseComponent[1] = velocityDesired.East - fluidMovement[1]; + + indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), + fixedWingSettings->HorizontalVelMin, + fixedWingSettings->HorizontalVelMax); + + // if we could fly at arbitrary speeds, we'd just have to move towards the + // courseComponent vector as previously calculated and we'd be fine + // unfortunately however we are bound by min and max air speed limits, so + // we need to recalculate the correct course to meet at least the + // velocityDesired vector direction at our current speed + // this overwrites courseComponent + bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); + + // Error condition: wind speed too high, we can't go where we want anymore + fixedWingPathFollowerStatus.Errors.Wind = 0; + if ((!valid) && + fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Wind = 1; + result = 0; + } + + // Airspeed error + airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; + + // Error condition: plane too slow or too fast + fixedWingPathFollowerStatus.Errors.Highspeed = 0; + fixedWingPathFollowerStatus.Errors.Lowspeed = 0; + if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) { + fixedWingPathFollowerStatus.Errors.Overspeed = 1; + result = 0; + } + if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) { + fixedWingPathFollowerStatus.Errors.Highspeed = 1; + result = 0; + cutThrust = true; + } + if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) { + fixedWingPathFollowerStatus.Errors.Lowspeed = 1; + result = 0; + } + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) { + fixedWingPathFollowerStatus.Errors.Stallspeed = 1; + result = 0; + } + if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) { + cutThrust = true; + result = 0; + } + } // Vertical speed error descentspeedDesired = boundf( @@ -324,41 +369,19 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude() fixedWingSettings->VerticalVelMax); descentspeedError = descentspeedDesired - velocityState.Down; - // Error condition: plane too slow or too fast - fixedWingPathFollowerStatus.Errors.Highspeed = 0; - fixedWingPathFollowerStatus.Errors.Lowspeed = 0; - if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) { - fixedWingPathFollowerStatus.Errors.Overspeed = 1; - result = 0; - } - if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) { - fixedWingPathFollowerStatus.Errors.Highspeed = 1; - result = 0; - cutThrust = true; - } - if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) { - fixedWingPathFollowerStatus.Errors.Lowspeed = 1; - result = 0; - } - if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) { - fixedWingPathFollowerStatus.Errors.Stallspeed = 1; - result = 0; - } - if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) { - cutThrust = true; - result = 0; - } - /** * Compute desired thrust command */ // Compute the cross feed from vertical speed to pitch, with saturation - float speedErrorToPowerCommandComponent = boundf( - (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp, - -fixedWingSettings->AirspeedToPowerCrossFeed.Max, - fixedWingSettings->AirspeedToPowerCrossFeed.Max - ); + float speedErrorToPowerCommandComponent = 0.0f; + if (hasAirspeed) { + speedErrorToPowerCommandComponent = boundf( + (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp, + -fixedWingSettings->AirspeedToPowerCrossFeed.Max, + fixedWingSettings->AirspeedToPowerCrossFeed.Max + ); + } // Compute final thrust response powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) + @@ -404,34 +427,40 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude() /** * Compute desired pitch command */ - // Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp, - -fixedWingSettings->VerticalToPitchCrossFeed.Max, - fixedWingSettings->VerticalToPitchCrossFeed.Max - ); + if (hasAirspeed) { + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp, + -fixedWingSettings->VerticalToPitchCrossFeed.Max, + fixedWingSettings->VerticalToPitchCrossFeed.Max + ); - // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; - fixedWingPathFollowerStatus.Error.Speed = airspeedError; - fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator; - fixedWingPathFollowerStatus.Command.Speed = pitchCommand; + fixedWingPathFollowerStatus.Error.Speed = airspeedError; + fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator; + fixedWingPathFollowerStatus.Command.Speed = pitchCommand; - stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand, - fixedWingSettings->PitchLimit.Min, - fixedWingSettings->PitchLimit.Max); + stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand, + fixedWingSettings->PitchLimit.Min, + fixedWingSettings->PitchLimit.Max); - // Error condition: high speed dive - fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0; - if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up - velocityState.Down > 0.0f && // we ARE going down - descentspeedDesired < 0.0f && // we WANT to go up - airspeedError < 0.0f && // we are too fast already - fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on - fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1; - result = 0; - cutThrust = true; + // Error condition: high speed dive + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0; + if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError < 0.0f && // we are too fast already + fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1; + result = 0; + cutThrust = true; + } + } else { + // no airspeed sensor means we fly with constant pitch, like for landing pathfollower + stabDesired.Pitch = fixedWingSettings->LandingPitch; } + // Error condition: pitch way out of wack if (fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f && (attitudeState.Pitch < fixedWingSettings->PitchLimit.Min - fixedWingSettings->SafetyCutoffLimits.PitchDeg || @@ -445,7 +474,12 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude() /** * Compute desired roll command */ - courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; + if (hasAirspeed) { + courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; + } else { + // fallback based on effective movement direction when in fallback mode, hope that airspeed > wind velocity, or we will never get home + courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North)) - RAD2DEG(atan2f(velocityState.East, velocityState.North)); + } if (courseError < -180.0f) { courseError += 360.0f; @@ -632,4 +666,6 @@ void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVO // changes to groundspeed to offset the airspeed by the same measurement. // This has a side effect that in the absence of any airspeed updates, the // pathfollower will fly using groundspeed. + + lastAirspeedUpdate = PIOS_DELAY_GetuS(); } diff --git a/flight/modules/PathFollower/inc/fixedwingflycontroller.h b/flight/modules/PathFollower/inc/fixedwingflycontroller.h index c79b0644c..a78c7c503 100644 --- a/flight/modules/PathFollower/inc/fixedwingflycontroller.h +++ b/flight/modules/PathFollower/inc/fixedwingflycontroller.h @@ -70,6 +70,7 @@ private: void updatePathVelocity(float kFF, bool limited); uint8_t updateFixedDesiredAttitude(); bool correctCourse(float *C, float *V, float *F, float s); + int32_t lastAirspeedUpdate; struct pid PIDposH[2]; struct pid PIDposV; diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index 9d3e45f2c..13b90f1f2 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -20,6 +20,9 @@ + + + diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml index 2b2482de9..26fd4cdfc 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml @@ -4,7 +4,7 @@ - +