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

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

Browse Source
This commit is contained in:
Corvus Corax 2015-05-23 10:02:53 +02:00
parent aac9159e87
commit 9a6cf9d21b
4 changed files with 55 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp
View File

@ -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);
}

48
flight/modules/PathFollower/fixedwingflycontroller.cpp
View File

@ -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,20 +379,26 @@ 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
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
airspeedError > 0.0f) { // we are too slow already
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
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
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
airspeedError < 0.0f) { // we are too fast already
// 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;
result = 0;
if (fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
result = 0;
}
}
/**
@ -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;

14
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -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"
defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" />
elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Rollcontrol, Pitchcontrol"
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"/>

2
shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
View File

@ -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"/>