OP-1900 OP-1054 added emergency thrust cutoff feature to main fixed wing autopilot on loss of control

flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp

@@ -142,10 +142,10 @@ bool FixedWingAutoTakeoffController::isUnsafe(void)
     AttitudeStateData attitude;
     AttitudeStateGet(&attitude);
     // too much bank angle
-    if (fabsf(attitude.Roll) > fixedWingSettings->TakeOffLimits.RollDeg) {
+    if (fabsf(attitude.Roll) > fixedWingSettings->SafetyCutoffLimits.RollDeg) {
         abort = true;
     }
-    if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->TakeOffLimits.PitchDeg) {
+    if (fabsf(attitude.Pitch - fixedWingSettings->TakeOffPitch) > fixedWingSettings->SafetyCutoffLimits.PitchDeg) {
         abort = true;
     }
     float deltayaw = attitude.Yaw - initYaw;
@@ -155,7 +155,7 @@ bool FixedWingAutoTakeoffController::isUnsafe(void)
     if (deltayaw < -180.0f) {
         deltayaw += 360.0f;
     }
-    if (fabsf(deltayaw) > fixedWingSettings->TakeOffLimits.YawDeg) {
+    if (fabsf(deltayaw) > fixedWingSettings->SafetyCutoffLimits.YawDeg) {
         abort = true;
     }
     return abort;
@@ -206,7 +206,7 @@ void FixedWingAutoTakeoffController::run_inactive(void) {}
 void FixedWingAutoTakeoffController::run_launch(void)
 {
     // state transition
-    if (maxVelocity > fixedWingSettings->TakeOffLimits.MaxDecelerationDeltaMPS) {
+    if (maxVelocity > fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
         setState(FW_AUTOTAKEOFF_STATE_CLIMB);
     }
 

flight/modules/PathFollower/fixedwingflycontroller.cpp

@@ -228,6 +228,7 @@ void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
 uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
 {
     uint8_t result = 1;
+    bool cutThrust = false;
 
     const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
 
@@ -333,6 +334,7 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     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;
@@ -342,6 +344,10 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
         fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
         result = 0;
     }
+    if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed - fixedWingSettings->SafetyCutoffLimits.MaxDecelerationDeltaMPS) {
+        cutThrust = true;
+        result    = 0;
+    }
 
     /**
      * Compute desired thrust command
@@ -373,21 +379,27 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
         velocityState.Down > 0.0f && // we ARE going down
         descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError > 0.0f && // we are too slow already
+        airspeedError > 0.0f) { // we are too slow already
-        fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
         fixedWingPathFollowerStatus.Errors.Lowpower = 1;
+
+        if (fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
             result = 0;
         }
+    }
     // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
     fixedWingPathFollowerStatus.Errors.Highpower = 0;
     if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
         velocityState.Down < 0.0f && // we ARE going up
         descentspeedDesired > 0.0f && // we WANT to go down
-        airspeedError < 0.0f && // we are too fast already
+        airspeedError < 0.0f) { // we are too fast already
-        fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
+        // this alarm is often switched off because of false positives, however we still want to cut throttle if it happens
+        cutThrust = true;
         fixedWingPathFollowerStatus.Errors.Highpower = 1;
+
+        if (fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
             result = 0;
         }
+    }
 
     /**
      * Compute desired pitch command
@@ -418,7 +430,17 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
         fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
         fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
         result = 0;
+        cutThrust = true;
     }
+    // Error condition: pitch way out of wack
+    if (fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f &&
+        (attitudeState.Pitch < fixedWingSettings->PitchLimit.Min - fixedWingSettings->SafetyCutoffLimits.PitchDeg ||
+         attitudeState.Pitch > fixedWingSettings->PitchLimit.Max + fixedWingSettings->SafetyCutoffLimits.PitchDeg)) {
+        fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
+        result = 0;
+        cutThrust = true;
+    }
+
 
     /**
      * Compute desired roll command
@@ -456,7 +478,15 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
                               fixedWingSettings->RollLimit.Min,
                               fixedWingSettings->RollLimit.Max);
 
-    // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
+    // Error condition: roll way out of wack
+    fixedWingPathFollowerStatus.Errors.Rollcontrol = 0;
+    if (fixedWingSettings->Safetymargins.Rollcontrol > 0.5f &&
+        (attitudeState.Roll < fixedWingSettings->RollLimit.Min - fixedWingSettings->SafetyCutoffLimits.RollDeg ||
+         attitudeState.Roll > fixedWingSettings->RollLimit.Max + fixedWingSettings->SafetyCutoffLimits.RollDeg)) {
+        fixedWingPathFollowerStatus.Errors.Rollcontrol = 1;
+        result = 0;
+        cutThrust = true;
+    }
 
 
     /**
@@ -465,6 +495,10 @@ uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
     // TODO implement raw control mode for yaw and base on Accels.Y
     stabDesired.Yaw = 0.0f;
 
+    // safety cutoff condition
+    if (cutThrust) {
+        stabDesired.Thrust = 0.0f;
+    }
 
     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;

shared/uavobjectdefinition/fixedwingpathfollowersettings.xml

@@ -45,17 +45,21 @@
         <field name="ThrustLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
                 <!-- minimum and maximum allowed thrust and setpoint for cruise speed -->
         <field name="Safetymargins" units="" type="float"
-                elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Pitchcontrol"
+                elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Rollcontrol, Pitchcontrol"
-                defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" />
+                defaultvalue="1, 1.0, 0.5, 1.5, 1.0, 1, 1, 1, 1" />
                 <!-- Wind: degrees of crabbing allowed
                      Speeds: percentage (1.0=100%) of the limit to be over/onder
                      Power & Control: flag to turn on/off 0.0 =off 1.0 = on -->
+        <field name="SafetyCutoffLimits" units="" type="float"
+		elementnames="RollDeg,PitchDeg,YawDeg,MaxDecelerationDeltaMPS" defaultvalue="25.0, 25.0, 25.0, 4.0" />
+		<!-- maximum margins from attempted attitude allowed during takeoff
+		     In flight Roll/Pitch are added to RollLimit/PitchLimit as max limits - throttle is cut when beyond
+		     if roll > RollLimit.Max + TakeOffLimits.RollDeg
+		     the same happens when MaxDecelerationDeltaMPS below Lowspeed limit
+		     if speed < Safetymargins.Lowspeed*HorizontalVelMin - TakeOffLimits.MaxDecelerationDeltaMPS --> 
 
 
 	<field name="TakeOffPitch" units="deg" type="float" elements="1" defaultvalue="25.0"/>
-        <field name="TakeOffLimits" units="" type="float"
-		elementnames="RollDeg,PitchDeg,YawDeg,MaxDecelerationDeltaMPS" defaultvalue="25.0, 25.0, 25.0, 4.0" />
-		<!-- maximum margins allowed during takeoff --> 
 	<field name="LandingPitch" units="deg" type="float" elements="1" defaultvalue="15.0"/>
 
         <field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>

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,Pitchcontrol" />
+	<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Rollcontrol,Pitchcontrol" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="500"/>