rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1156 fixed EmergencyFlyaway detection algorithm

Browse Source
This commit is contained in:
Corvus Corax 2014-08-12 19:12:05 +02:00
parent 4fc071978c
commit 09e8b38407
2 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
flight/modules/PathFollower/pathfollower.c
View File

@ -96,7 +96,8 @@ struct Integrals {
float power;
float airspeed;
float poiRadius;
bool vtolEmergencyFallback;
float vtolEmergencyFallback;
bool vtolEmergencyFallbackSwitch;
};
@ -288,6 +289,7 @@ static void resetIntegrals()
i.airspeed = 0.0f;
i.poiRadius = 0.0f;
i.vtolEmergencyFallback = 0;
i.vtolEmergencyFallbackSwitch = false;
}
static uint8_t updateAutoPilotByFrameType()
@ -339,7 +341,7 @@ static uint8_t updateAutoPilotFixedWing()
*/
static uint8_t updateAutoPilotVtol()
{
if (!i.vtolEmergencyFallback) {
if (!i.vtolEmergencyFallbackSwitch) {
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true);
updateVtolDesiredAttitudeEmergencyFallback();
@ -366,9 +368,9 @@ static uint8_t updateAutoPilotVtol()
yaw = updatePOIBearing();
break;
}
updateVtolDesiredAttitude(yaw_attitude, yaw);
result = updateVtolDesiredAttitude(yaw_attitude, yaw);
if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) {
i.vtolEmergencyFallback = true;
i.vtolEmergencyFallbackSwitch = true;
}
return result;
}
@ -1120,10 +1122,23 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
}
if ( // emergency flyaway detection
(fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit
velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[0]) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[1]) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
i.vtolEmergencyFallback += dT;
if (i.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
i.vtolEmergencyFallback = 0.0f;
}
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the

1
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -14,6 +14,7 @@
<field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="manual"/>
<field name="YawControl" units="" type="enum" elements="1" options="manual,tailin,movementdirection,pathdirection,poi" defaultvalue="manual"/>
<field name="FlyawayEmergencyFallback" units="switch" type="enum" elements="1" options="disabled,enabled,always,debugtest" defaultvalue="enabled"/>
<field name="FlyawayEmergencyFallbackTriggerTime" units="s" type="float" elements="1" defaultvalue="10.0"/>
<field name="EmergencyFallbackAttitude" units="deg" type="float" elementnames="Roll,Pitch" defaultvalue="0,-10.0"/>
<field name="EmergencyFallbackYawRate" units="(deg/s)/deg" type="float" elementnames="kP,Max" defaultvalue="2.0, 30.0"/>
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>