OP-1900 add failsafe to FW pathfollower to fly safely without valid airspeed estimate

flight/modules/PathFollower/fixedwingflycontroller.cpp

@@ -63,6 +63,7 @@ void FixedWingFlyController::Activate(void)
         SettingsUpdated();
         resetGlobals();
         mMode   = pathDesired->Mode;
+        lastAirspeedUpdate = 0;
     }
 }
 
@@ -229,6 +230,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
 {
     uint8_t result   = 1;
     bool cutThrust   = false;
+    bool hasAirspeed = true;
 
     const float dT   = fixedWingSettings->UpdatePeriod / 1000.0f;
 
@@ -243,7 +245,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     float groundspeedProjection;
     float indicatedAirspeedState;
     float indicatedAirspeedDesired;
-    float airspeedError;
+    float airspeedError = 0.0f;
 
     float pitchCommand;
 
@@ -266,9 +268,26 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     AirspeedStateGet(&airspeedState);
     SystemSettingsGet(&systemSettings);
 
+
     /**
      * Compute speed error and course
      */
+
+    // 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;
+    }
+
+
+    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
@@ -317,13 +336,6 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
         // Airspeed error
         airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
 
-    // Vertical speed error
-    descentspeedDesired = boundf(
-        velocityDesired.Down,
-        -fixedWingSettings->VerticalVelMax,
-        fixedWingSettings->VerticalVelMax);
-    descentspeedError = descentspeedDesired - velocityState.Down;
-
         // Error condition: plane too slow or too fast
         fixedWingPathFollowerStatus.Errors.Highspeed = 0;
         fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
@@ -348,17 +360,28 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
             cutThrust = true;
             result    = 0;
         }
+    }
+
+    // Vertical speed error
+    descentspeedDesired = boundf(
+        velocityDesired.Down,
+        -fixedWingSettings->VerticalVelMax,
+        fixedWingSettings->VerticalVelMax);
+    descentspeedError = descentspeedDesired - velocityState.Down;
 
     /**
      * Compute desired thrust command
      */
 
     // Compute the cross feed from vertical speed to pitch, with saturation
-    float speedErrorToPowerCommandComponent = boundf(
+    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,6 +427,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     /**
      * Compute desired pitch command
      */
+    if (hasAirspeed) {
         // Compute the cross feed from vertical speed to pitch, with saturation
         float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
                                                             -fixedWingSettings->VerticalToPitchCrossFeed.Max,
@@ -432,6 +456,11 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
             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
      */
+    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();
 }

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;

shared/uavobjectdefinition/fixedwingpathfollowersettings.xml

@@ -20,6 +20,9 @@
         <field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
                 <!-- proportional coefficient for desired vertical speed in relation to altitude displacement-->
 
+	<field name="UseAirspeedSensor" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True"/>
+		<!-- allows to ignore the airspeed sensor, set to false if you have none, which will trigger fallback algorithm without indicating a fault -->
+
         <!-- these coefficients control actual control outputs -->
         <field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
                 <!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading -->

shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml

@@ -4,7 +4,7 @@
 	<field name="Error" units="" type="float" elementnames="Course,Speed,Power" />
 	<field name="ErrorInt" units="" type="float" elementnames="Course,Speed,Power" />
 	<field name="Command" units="" type="float" elementnames="Course,Speed,Power" />
-	<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol" />
+	<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol,AirspeedSensor" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="500"/>